/home/laradmin/lar/perception/3D/polygon_primitives_extraction/src/polygon_primitive_planefitting.cpp
Go to the documentation of this file.00001 /************************************************************************************************** 00002 Software License Agreement (BSD License) 00003 00004 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt 00005 All rights reserved. 00006 00007 Redistribution and use in source and binary forms, with or without modification, are permitted 00008 provided that the following conditions are met: 00009 00010 *Redistributions of source code must retain the above copyright notice, this list of 00011 conditions and the following disclaimer. 00012 *Redistributions in binary form must reproduce the above copyright notice, this list of 00013 conditions and the following disclaimer in the documentation and/or other materials provided 00014 with the distribution. 00015 *Neither the name of the University of Aveiro nor the names of its contributors may be used to 00016 endorse or promote products derived from this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 00019 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 00020 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 00021 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00022 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00023 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER 00024 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 00025 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00026 ***************************************************************************************************/ 00036 #ifndef _polygon_primitive_planefitting_CPP_ 00037 #define _polygon_primitive_planefitting_CPP_ 00038 00039 00040 #include "polygon_primitive.h" 00041 00042 #include <CGAL/Cartesian.h> 00043 #include <CGAL/linear_least_squares_fitting_3.h> 00044 #include <list> 00045 00054 double c_polygon_primitive::fit_plane_to_pc(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::ModelCoefficients::Ptr plane) 00055 { 00056 00057 //STEP1. Create a list of pts xyz 00058 std::list<CGAL::Cartesian<double>::Point_3> points; 00059 00060 for (int i=0; i<(int)pcin->points.size(); i++) 00061 { 00062 points.push_back(CGAL::Cartesian<double>::Point_3(pcin->points[i].x, pcin->points[i].y, pcin->points[i].z)); 00063 } 00064 00065 //STEP2. Fit the plane to the list of points 00066 CGAL::Cartesian<double>::Plane_3 pl; 00067 double fit_quality = CGAL::linear_least_squares_fitting_3(points.begin(),points.end(),pl,CGAL::Dimension_tag<0>()); 00068 00069 //STEP3. Copy to the model coefficients 00070 plane->values[0] = pl.a(); 00071 plane->values[1] = pl.b(); 00072 plane->values[2] = pl.c(); 00073 plane->values[3] = pl.d(); 00074 00075 return fit_quality;} 00076 00087 double c_polygon_primitive::fit_plane_to_two_pc_and_ratio(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin1, pcl::PointCloud<pcl::PointXYZ>::Ptr pcin2, double ratio, pcl::ModelCoefficients::Ptr plane) 00088 { 00089 00090 //STEP1. Create a list of pts xyz 00091 std::list<CGAL::Cartesian<double>::Point_3> points; 00092 00093 //find out many insertions of points according to ratio 00094 double weight_pcin1 = (1-ratio)/(double)pcin1->points.size(); 00095 double weight_pcin2 = ratio/(double)pcin2->points.size(); 00096 00097 //int count_pcin1 = (int)(weight_pcin1*((int)pcin1->points.size() + (int)pcin2->points.size())); 00098 //int count_pcin2 = (int)(weight_pcin2*((int)pcin1->points.size() + (int)pcin2->points.size())); 00099 00100 //ROS_INFO("weight_pcin1 %f count_pcin1 %d", weight_pcin1, count_pcin1); 00101 //ROS_INFO("weight_pcin2 %f count_pcin2 %d", weight_pcin2, count_pcin2); 00102 00103 for (int i=0; i<(int)pcin1->points.size(); i++) 00104 { 00105 for (double j=0; j<weight_pcin1; j+=0.00001) 00106 points.push_back(CGAL::Cartesian<double>::Point_3(pcin1->points[i].x, pcin1->points[i].y, pcin1->points[i].z)); 00107 } 00108 00109 for (int i=0; i<(int)pcin2->points.size(); i++) 00110 { 00111 for (double j=0; j<weight_pcin2; j+=0.00001) 00112 points.push_back(CGAL::Cartesian<double>::Point_3(pcin2->points[i].x, pcin2->points[i].y, pcin2->points[i].z)); 00113 } 00114 00115 //STEP2. Fit the plane to the list of points 00116 CGAL::Cartesian<double>::Plane_3 pl; 00117 double fit_quality = CGAL::linear_least_squares_fitting_3(points.begin(),points.end(),pl,CGAL::Dimension_tag<0>()); 00118 00119 //STEP3. Copy to the model coefficients 00120 plane->values[0] = pl.a(); 00121 plane->values[1] = pl.b(); 00122 plane->values[2] = pl.c(); 00123 plane->values[3] = pl.d(); 00124 00125 return fit_quality;} 00126 00127 00128 00129 00130 #endif 00131 00134 /*Previous 3 lines appended automatically on Sun Feb 5 19:40:16 WET 2012 */