convert.cpp
Go to the documentation of this file.
00001 /*
00002  * scan.cpp
00003  *
00004  *  Created on: Apr 17, 2012
00005  *      Author: vsu
00006  */
00007 
00008 #include <pcl17/console/parse.h>
00009 #include <pcl17/common/transforms.h>
00010 
00011 #include <pcl17/ModelCoefficients.h>
00012 #include <pcl17/io/pcd_io.h>
00013 #include <pcl17/point_types.h>
00014 #include <pcl17/sample_consensus/method_types.h>
00015 #include <pcl17/sample_consensus/model_types.h>
00016 #include <pcl17/segmentation/sac_segmentation.h>
00017 #include <pcl17/filters/extract_indices.h>
00018 
00019 #include <pcl17/filters/voxel_grid.h>
00020 
00021 int main(int argc, char** argv)
00022 {
00023 
00024   if (argc < 5)
00025   {
00026     PCL17_INFO ("Usage %s -input_file /in_file -output_file /out_file [options]\n", argv[0]);
00027     PCL17_INFO (" * where options are:\n"
00028         "         -tilt <X>  : tilt. Default : 30\n"
00029         "");
00030     return -1;
00031   }
00032 
00033   int tilt = 30;
00034   std::string input_file;
00035   std::string output_file;
00036 
00037   pcl17::console::parse_argument(argc, argv, "-input_file", input_file);
00038   pcl17::console::parse_argument(argc, argv, "-output_file", output_file);
00039   pcl17::console::parse_argument(argc, argv, "-tilt", tilt);
00040 
00041   pcl17::PointCloud<pcl17::PointXYZRGB> cloud, cloud_transformed, cloud_aligned, cloud_filtered;
00042 
00043   pcl17::io::loadPCDFile(input_file, cloud);
00044 
00045   Eigen::Affine3f view_transform;
00046   view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
00047 
00048   Eigen::AngleAxis<float> rot(tilt * M_PI / 180, Eigen::Vector3f(0, 1, 0));
00049 
00050   view_transform.prerotate(rot);
00051 
00052   pcl17::transformPointCloud(cloud, cloud_transformed, view_transform);
00053 
00054   pcl17::ModelCoefficients::Ptr coefficients(new pcl17::ModelCoefficients);
00055   pcl17::PointIndices::Ptr inliers(new pcl17::PointIndices);
00056 
00057   pcl17::SACSegmentation<pcl17::PointXYZRGB> seg;
00058 
00059   seg.setOptimizeCoefficients(true);
00060 
00061   seg.setModelType(pcl17::SACMODEL_PLANE);
00062   seg.setMethodType(pcl17::SAC_RANSAC);
00063   seg.setDistanceThreshold(0.05);
00064   seg.setProbability(0.99);
00065 
00066   seg.setInputCloud(cloud_transformed.makeShared());
00067   seg.segment(*inliers, *coefficients);
00068 
00069   pcl17::ExtractIndices<pcl17::PointXYZRGB> extract;
00070 
00071   extract.setInputCloud(cloud_transformed.makeShared());
00072   extract.setIndices(inliers);
00073   extract.setNegative(true);
00074   extract.filter(cloud_transformed);
00075 
00076   std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " "
00077       << coefficients->values[2] << " " << coefficients->values[3] << std::endl;
00078 
00079   Eigen::Vector3f z_current(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
00080   Eigen::Vector3f y(0, 1, 0);
00081 
00082   Eigen::Affine3f rotation;
00083   rotation = pcl17::getTransFromUnitVectorsZY(z_current, y);
00084   rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3]));
00085 
00086   pcl17::transformPointCloud(cloud_transformed, cloud_aligned, rotation);
00087 
00088   Eigen::Affine3f res = (rotation * view_transform);
00089 
00090   cloud_aligned.sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1);
00091   cloud_aligned.sensor_orientation_ = res.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();
00092 
00093   seg.setInputCloud(cloud_aligned.makeShared());
00094   seg.segment(*inliers, *coefficients);
00095 
00096   std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " "
00097       << coefficients->values[2] << " " << coefficients->values[3] << std::endl;
00098 
00099   pcl17::io::savePCDFile(output_file, cloud_aligned);
00100 
00101   return 0;
00102 }
00103 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


furniture_classification
Author(s): Vladyslav Usenko
autogenerated on Sun Oct 6 2013 12:12:30