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());
}
}
