int main(int argc, char** argv) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudFiltered (new pcl::PointCloud<pcl::PointXYZRGBA>); int k=atoi(argv[1]); double min_mah_dist=atof(argv[2]); int max_gauss=atoi(argv[3]); std::vector <Gauss> gaussians_learned; pcl::PCDReader reader; reader.read (argv[4], *cloud); pcl::console::TicToc timer; timer.tic(); // Filtering Filtering filtering; cloudFiltered=filtering.filter(cloud); // Segmentation Segmentation segmentation; std::vector <Surface> surfaces=segmentation.segment (cloudFiltered); // Colour extraction from point cloud ExtractColour extractcolour; cv::Mat samples=extractcolour.pcd2colourRGB(surfaces[0].segmented_cloud); // Point cloud analysis ColourAnalysis colouranalysis; std::vector <Gauss> gaussians = colouranalysis.analyzeColourCluster(k, surfaces[0].segmented_cloud, surfaces[0].b, surfaces[0].d, samples); // Learning Learning learning; gaussians_learned=learning.learnGaussians(gaussians, gaussians_learned, max_gauss); // ROI ROI roi; std::vector<Line> lines=roi.roi(surfaces); std::vector<int> index=roi.extractIndex(lines); // Colour extraction from image std::vector<Matrix_double> colours=extractcolour.indexPCD2VectorRGB(*cloud, index); // Image analysis std::vector <int> pixels_labeled=colouranalysis.labelIndex(colours, k, gaussians_learned, min_mah_dist); // Map reconstruction Map map; std::vector<Matrix_double> distancesLabeled=map.mapDistances(index, surfaces[0].a ,surfaces[0].b, surfaces[0].c, surfaces[0].d); std::cout << "Part 1 finished" << std::endl; timer.toc_print(); timer.tic(); // Generate image cv::Mat environment=extractcolour.pcd2image(*cloud); // Calcule begin and end points for a line std::vector <LinePoint> linePoints=roi.calculeLinePoints(lines); // Draw lines Visualize visualize; environment=visualize.drawLines (environment, linePoints, "green"); // Print floor std::vector<int> pixels=colouranalysis.labelPixel(pixels_labeled, index, colours); environment=visualize.drawFloor (pixels, environment, "cyan"); std::vector <int> kinectIndex=roi.extractFloorIdx(surfaces[0].a, surfaces[0].b, surfaces[0].c, surfaces[0].d, cloud); environment=visualize.drawFloor (kinectIndex, environment, "blue"); // Reconstruct kinect floor std::vector <int> kinect (kinectIndex.size(), 1); std::vector<Matrix_double> distancesKinect=map.mapDistances(kinectIndex, surfaces[0].a ,surfaces[0].b, surfaces[0].c, surfaces[0].d); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudEnhanced=map.mapEnhance(distancesLabeled, index, cloud, pixels_labeled, surfaces[0].a ,surfaces[0].b, surfaces[0].c, surfaces[0].d); // Generate map reconstruction image cv::Mat map_reconstruction(cv::Size(400,650),CV_8UC3); map_reconstruction=cv::Scalar(0,0,0); map_reconstruction=visualize.mapGeneration(map_reconstruction, pixels_labeled, distancesLabeled, "blue", "white"); map_reconstruction=visualize.mapGeneration(map_reconstruction, kinect, distancesKinect, "red", "yellow"); // Show images on a window std::string name_window="Floor window"; visualize.visualizeImage (environment, name_window); name_window="Map window"; visualize.visualizeImage (map_reconstruction, name_window); visualize.visualizeCloud(cloudEnhanced, "Cloud"); // Save images Save save; char name[50]="map.png"; save.saveImage(map_reconstruction, name); char name2[50]="environment.png"; save.saveImage(environment, name2); // Save cloud std::string names= "cloud.pcd"; save.saveCloud(surfaces[0].segmented_cloud, names); std::cout << "Part 2 finished" << std::endl; timer.toc_print(); cv::waitKey(); return 0; }
int main(int argc, char** argv) { SimKinect::Ptr kinect(new SimKinect(argc, argv, "/home/sachin/Workspace/eih/data/pr2-test.env.xml")); std::cin.get(); }
int main( int argc, char** argv ){ cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); int numusers = 0; if( argc == 2 ){ sscanf( argv[1], "%d", &numusers ); } cisstOpenNI kinect( numusers ); kinect.Configure( SAMPLE_CONFIG_PATH ); if( 0 < numusers ){ kinect.InitSkeletons(); } while( true ){ // Wait and Update All kinect.Update( WAIT_AND_UPDATE_ALL ); if( 0 < numusers ){ kinect.UpdateUserSkeletons(); } { vctDynamicMatrix<unsigned char> rgb; if( kinect.GetRGBImage( rgb ) != cisstOpenNI::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to get RGB image" << std::endl; return -1; } std::ofstream ofs( "rgb" ); for( size_t r=0; r<rgb.rows(); r++ ){ for( size_t c=0; c<rgb.cols(); c++ ) { ofs << (int)rgb[r][c] << " "; } ofs << std::endl; } ofs.close(); } { vctDynamicMatrix<double> depth; if( kinect.GetDepthImageRaw( depth ) != cisstOpenNI::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to get RGB image" << std::endl; return -1; } std::ofstream ofs( "depth" ); ofs << depth; ofs.close(); } { vctDynamicMatrix<double> range; if( kinect.GetRangeData( range ) != cisstOpenNI::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to get RGB image" << std::endl; return -1; } std::ofstream ofs( "range" ); ofs << range; ofs.close(); } if( 0 < numusers ) { std::vector<cisstOpenNISkeleton*> skeletons = kinect.UpdateAndGetUserSkeletons(); } } return 0; }