void readerCallback (const sensor_msgs::PointCloud2ConstPtr& input)
{
const pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
const pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::fromROSMsg(*input, *cloud);
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
pcl::RegionGrowingRGB<PointT> reg_seg;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> sac_seg;
sac_seg.setOptimizeCoefficients (true);
sac_seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
sac_seg.setNormalDistanceWeight (0.1);
sac_seg.setMethodType (pcl::SAC_RANSAC);
sac_seg.setMaxIterations (200);
sac_seg.setDistanceThreshold (0.03);
filterCloud(cloud, cloud, normals);
removeModel(cloud, normals, sac_seg, true);
sac_seg.setDistanceThreshold (0.01);
removeModel(cloud, normals, sac_seg, true);
//sac_seg.setModelType (pcl::SACMODEL_LINE);
//sac_seg.setDistanceThreshold (0.15);
//removeModel(cloud, normals, sac_seg, false);
std::cerr << "PointCloud representing the extracted component: " << cloud->points.size () << " data points." << std::endl;
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud, output);
pc_pub.publish(output);
}
void CGraphFitting::generateCandidates()
{
double thres=0.25;
int numLines=getValidNum()-1;
for(int i=0;i<numLines;++i)
{
MyLine3D& line=getLine(i);
if(line.goodness<thres)
{
printf("%d line kmeans start\n",i);
line.kmeanLines(line.supportPoints,*pImg3D,true);
for(int j=0;j<line.clusterLines.size();++j)
appendModel(line.clusterLines[j]);
printf("%d line kmeans End\n",i);
}
}
for(int i=0;i<numLines;++i)
{
MyLine3D& line=getLine(i);
if(line.goodness<thres)
removeModel(i);
}
adjustModels();
}
/*!
Called before a view is detached from \a model.
This function is not called if Model::detachViewWithOutNotification is used.
The default implementation
is removing the reference to the model from the view.
\sa Model::detachView()
*/
void AbstractView::modelAboutToBeDetached(Model *)
{
removeModel();
}
