float Projector_Calibrator::fitPlaneToCloud(const Cloud& cloud, Eigen::Vector4f& model){ // ROS_INFO("Fitting plane to cloud with %zu points", cloud.size()); if (cloud.size() < 1000){ ROS_WARN("fitPlaneToCloud: cloud size very small: %zu", cloud.size()); } // http://pointclouds.org/documentation/tutorials/random_sample_consensus.php#random-sample-consensus pcl::SampleConsensusModelPlane<pcl_Point>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl_Point> (cloud.makeShared())); pcl::RandomSampleConsensus<pcl_Point> ransac(model_p); ransac.setDistanceThreshold(0.005); // max dist of 5mm ransac.computeModel(); Eigen::VectorXf c; ransac.getModelCoefficients(c); model = c; std::vector<int> inliers; ransac.getInliers(inliers); float inlier_pct = inliers.size()*100.0/cloud.size(); if (inlier_pct<0.5){ ROS_WARN("Only %.3f %% inlier in fitPlaneToCloud!", inlier_pct); } return inlier_pct; }
/* ******************************************************************************************** */ int main () { for(int i = 0; i < 4; i++) cs.push_back(Cloud()); pthread_mutex_init(&lock, NULL); SimpleOpenNIViewer v1(1), v2(2); v1.init(); v2.init(); // Create the viewer Viewer* viewer = new Viewer ("3D Viewer"); viewer->setBackgroundColor (0, 0, 0); viewer->addCoordinateSystem (1.0); viewer->initCameraParameters (); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample"); // Visualize while(true) { Cloud c; pthread_mutex_lock(&lock); for(int i = 0; i < 2; i++) { printf("%d\t", cs[i].size()); c += (cs[i]); } printf("\n"); pthread_mutex_unlock(&lock); viewer->removePointCloud(); Cloud::Ptr cp = c.makeShared(); pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(cp); viewer->addPointCloud(cp, rgb); viewer->spinOnce(); // printf("%d\n", c.size()); } }