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