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