int main(int argc,char** argv){
if (argc < 2){
std::cout<<"Please enter <input.pcd> file"<<std::endl;
return 0;
}
if (pcl::io::loadPCDFile (argv[1], *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
return (-1);
}
model_name = argv[1];
model_name = model_name.substr(0,model_name.size()-4);
if(pcl::console::find_switch(argc,argv,"-vfh")){
vfh = true;
}
else if(pcl::console::find_switch(argc,argv,"-rv")){
std::cout<<"performing Resultant vector feature calculation"<<std::endl;
rv = true;
}else{
std::cout<<"no algorithm specified using default algorithm vfh"<<std::endl;
vfh = true;
}
if (pcl::console::find_switch (argc, argv, "-ds"))
{
_downsample = true;
std::cout<<"performing downsampling on the input file"<<std::endl;
}
if (pcl::console::find_switch (argc, argv, "-sn"))
{
show_normals = true;
std::cout<<"showing calclated normals"<<std::endl;
}
if(_downsample){
rec.setInputCloud(model,_leaf_size);
std::cout<<"saving downsampled file to model_down.pcd"<<std::endl;
pcl::io::savePCDFileASCII ("model_down.pcd", *(rec.getCloud()));
}
else{
rec.setInputCloud(model);
std::cout<<"setting input model without further downsampling"<<std::endl;
}
if(pcl::console::find_switch(argc,argv,"--showaxis")){
_show_axis = true;
}
if(vfh){
std::cout<<"estimating VFH features"<<std::endl;
rec.Estimate_VFH_Features();
}
else if(rv){
std::cout<<"estimating features using the resultant vector"<<std::endl;
rec.Estimate_RV_Features();
PointNormalCloudT::Ptr cloud (new PointNormalCloudT);
pcl::PointCloud<pcl::Normal>::Ptr norm_cloud (new pcl::PointCloud<pcl::Normal>);
cloud = rec.getPointNormalCloud();
norm_cloud = rec.getNormalCloud();
pcl::PointCloud<pcl::PointXYZ>::Ptr plaincloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud,*plaincloud);
//pcl::PointXYZ mass_center(rec.rv_centroid.x,rec.rv_centroid.y,rec.rv_centroid.z);
pcl::PointXYZ mass_center(0,0,0);
pcl::PointXYZ kinectZ(0,0,-1);
pcl::PointXYZ res_vec (rec.rv_resultant.normal_x,rec.rv_resultant.normal_y,rec.rv_resultant.normal_z);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(plaincloud);
//viewer.addPointCloud<pcl::PointXYZ> (plaincloud, rgb, "model_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> single_color(cloud, double(0), double(255), double(0));
viewer.addPointCloud(cloud,single_color,"sample cloud");
if(_show_axis){
viewer.addLine(mass_center,res_vec,1.0f,0.0f,0.0f,"resultantvector");
viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,"KinectZ");
}
std::cout<<"resultant vector :"<<res_vec.x<<" i"<<" + "<<res_vec.y<<" j"<<" + "<<res_vec.z<<" k"<<std::endl;
if(show_normals){
std::cout<<"showing 1 in "<<normalsratio<<" normals"<<std::endl;
viewer.addPointCloudNormals<pcl::PointNormal,pcl::Normal>(cloud, norm_cloud,normalsratio, 0.05, "normalscloud");
}
while(!viewer.wasStopped())
viewer.spinOnce();
}
std::cout<<"feature calculation complete"<<std::endl;
ofstream myfile;
if (vfh){
std::stringstream ss;
ss<<"vfh_"<<model_name<<".txt";
myfile.open(ss.str().c_str());
for(size_t k =0 ;k<308;k++){
if(k != 307)
myfile << rec._vfh_features->points[0].histogram[k]<<",";
else
myfile << rec._vfh_features->points[0].histogram[k];
}
std::cout<<"wrote the histogram to file :" <<ss.str()<<std::endl;
myfile.close();
}else if(rv){
std::stringstream ss;
ss<<"rv_"<<model_name<<".txt";
myfile.open(ss.str().c_str());
for(int k = 0;k<181;k++){
if(k != 180)
myfile << rec._rv_features_181[k]<<",";
else
myfile << rec._rv_features_181[k];
}
std::cout<<"wrote the histogram to file: "<< ss.str()<<std::endl;
//myfile.open(ss.c_str());
}
}
void
process ()
{
std::cout << "threshold: " << threshold_ << std::endl;
std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n");
unsigned char red [6] = {255, 0, 0, 255, 255, 0};
unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
refinement_compare->setDistanceThreshold (threshold_, depth_dependent_);
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (5000);
mps.setAngularThreshold (0.017453 * 3.0); //3 degrees
mps.setDistanceThreshold (0.03); //2cm
mps.setRefinementComparator (refinement_compare);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>);
char name[1024];
typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
double normal_start = pcl::getTime ();
ne.setInputCloud (cloud);
ne.compute (*normal_cloud);
double normal_end = pcl::getTime ();
std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
double plane_extract_start = pcl::getTime ();
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud);
if (refine_)
mps.segmentAndRefine (regions);
else
mps.segment (regions);
double plane_extract_end = pcl::getTime ();
std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl;
std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
viewer.removeAllPointClouds (0);
viewer.removeAllShapes (0);
pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
viewer.addPointCloud<PointT> (cloud, single_color, "cloud");
pcl::PlanarPolygon<PointT> approx_polygon;
//Draw Visualization
for (size_t i = 0; i < regions.size (); i++)
{
Eigen::Vector3f centroid = regions[i].getCentroid ();
Eigen::Vector4f model = regions[i].getCoefficients ();
pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
centroid[1] + (0.5f * model[1]),
centroid[2] + (0.5f * model[2]));
sprintf (name, "normal_%d", unsigned (i));
viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name));
contour->points = regions[i].getContour ();
sprintf (name, "plane_%02d", int (i));
pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
viewer.addPointCloud (contour, color, name);
pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_);
approx_contour->points = approx_polygon.getContour ();
std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl;
typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour;
// sprintf (name, "approx_plane_%02d", int (i));
// viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
for (unsigned idx = 0; idx < approx_contour->points.size (); ++idx)
{
sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx));
viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
}
}
}
void PbMapVisualizer::viz_cb (pcl::visualization::PCLVisualizer& viz)
{
if (pbmap.globalMapPtr->empty())
{
mrpt::system::sleep(10);
return;
}
// Render the data
{
viz.removeAllShapes();
viz.removeAllPointClouds();
char name[1024];
if(graphRepresentation)
{
// cout << "show graphRepresentation\n";
for(size_t i=0; i<pbmap.vPlanes.size(); i++)
{
pcl::PointXYZ center(2*pbmap.vPlanes[i].v3center[0], 2*pbmap.vPlanes[i].v3center[1], 2*pbmap.vPlanes[i].v3center[2]);
double radius = 0.1 * sqrt(pbmap.vPlanes[i].areaVoxels);
// cout << "radius " << radius << endl;
sprintf (name, "sphere%u", static_cast<unsigned>(i));
viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name);
if( !pbmap.vPlanes[i].label.empty() )
viz.addText3D (pbmap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], pbmap.vPlanes[i].label);
else
{
sprintf (name, "P%u", static_cast<unsigned>(i));
viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
// Draw edges
// for(set<unsigned>::iterator it = pbmap.vPlanes[i].nearbyPlanes.begin(); it != pbmap.vPlanes[i].nearbyPlanes.end(); it++)
// {
// if(*it > pbmap.vPlanes[i].id)
// break;
//
// sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it));
// pcl::PointXYZ center_it(2*pbmap.vPlanes[*it].v3center[0], 2*pbmap.vPlanes[*it].v3center[1], 2*pbmap.vPlanes[*it].v3center[2]);
// viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
// }
for(map<unsigned,unsigned>::iterator it = pbmap.vPlanes[i].neighborPlanes.begin(); it != pbmap.vPlanes[i].neighborPlanes.end(); it++)
{
if(it->first > pbmap.vPlanes[i].id)
break;
sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
pcl::PointXYZ center_it(2*pbmap.vPlanes[it->first].v3center[0], 2*pbmap.vPlanes[it->first].v3center[1], 2*pbmap.vPlanes[it->first].v3center[2]);
viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
char commonObs[8];
sprintf (commonObs, "%u", it->second);
pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 );
viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name);
}
}
}
else
{ // Regular representation
if (!viz.updatePointCloud (pbmap.globalMapPtr, "cloud"))
viz.addPointCloud (pbmap.globalMapPtr, "cloud");
sprintf (name, "PointCloud size %u", static_cast<unsigned>( pbmap.globalMapPtr->size() ) );
viz.addText(name, 10, 20);
for(size_t i=0; i<pbmap.vPlanes.size(); i++)
{
Plane &plane_i = pbmap.vPlanes[i];
// sprintf (name, "normal_%u", static_cast<unsigned>(i));
name[0] = *(mrpt::format("normal_%u", static_cast<unsigned>(i)).c_str());
pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization
pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]);
pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
if( !plane_i.label.empty() )
viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label);
else
{
sprintf (name, "n%u", static_cast<unsigned>(i));
viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
sprintf (name, "approx_plane_%02d", int (i));
viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name);
}
}
}
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
{
const std::string &cloudname = this->name;
if(firstRun)
{
visualizer.addPointCloud(cloud, cloudname);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
}
else
{
visualizer.updatePointCloud(cloud, cloudname);
visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
visualizer.removeAllShapes();
}
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0.2, 0, 0), 1, 0, 0, "X");
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0.2, 0), 0, 1, 0, "Y");
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0, 0.2), 0, 0, 1, "Z");
tf::Vector3 origin = worldToCam * tf::Vector3(0, 0, 0);
tf::Vector3 lineX = worldToCam * tf::Vector3(0.2, 0, 0);
tf::Vector3 lineY = worldToCam * tf::Vector3(0, 0.2, 0);
tf::Vector3 lineZ = worldToCam * tf::Vector3(0, 0, 0.2);
pcl::PointXYZ pclOrigin(origin.x(), origin.y(), origin.z());
pcl::PointXYZ pclLineX(lineX.x(), lineX.y(), lineX.z());
pcl::PointXYZ pclLineY(lineY.x(), lineY.y(), lineY.z());
pcl::PointXYZ pclLineZ(lineZ.x(), lineZ.y(), lineZ.z());
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pclOrigin, 1, 1, 1, "line");
visualizer.addLine(pclOrigin, pclLineX, 1, 0, 0, "lineX");
visualizer.addLine(pclOrigin, pclLineY, 0, 1, 0, "lineY");
visualizer.addLine(pclOrigin, pclLineZ, 0, 0, 1, "lineZ");
for(int i = 0; i < regions.size(); ++i)
{
const Region ®ion = regions[i];
tf::Transform transform = worldToCam * region.transform;
std::ostringstream oss;
oss << "region_" << i;
tf::Vector3 originB = transform * tf::Vector3(0, 0, 0);
tf::Vector3 lineXB = transform * tf::Vector3(0.2, 0, 0);
tf::Vector3 lineYB = transform * tf::Vector3(0, 0.2, 0);
tf::Vector3 lineZB = transform * tf::Vector3(0, 0, 0.2);
pcl::PointXYZ pclOriginB(originB.x(), originB.y(), originB.z());
pcl::PointXYZ pclLineXB(lineXB.x(), lineXB.y(), lineXB.z());
pcl::PointXYZ pclLineYB(lineYB.x(), lineYB.y(), lineYB.z());
pcl::PointXYZ pclLineZB(lineZB.x(), lineZB.y(), lineZB.z());
visualizer.addLine(pclOrigin, pclOriginB, 1, 1, 1, "line_" + oss.str());
visualizer.addLine(pclOriginB, pclLineXB, 1, 0, 0, "lineX_" + oss.str());
visualizer.addLine(pclOriginB, pclLineYB, 0, 1, 0, "lineY_" + oss.str());
visualizer.addLine(pclOriginB, pclLineZB, 0, 0, 1, "lineZ_" + oss.str());
Eigen::Vector3d translation;
Eigen::Quaterniond rotation;
tf::vectorTFToEigen(transform.getOrigin(), translation);
tf::quaternionTFToEigen(transform.getRotation(), rotation);
visualizer.addCube(translation.cast<float>(), rotation.cast<float>(), region.width, region.height, region.depth, oss.str());
}
}