Eigenvectors of PCL pointcloud (moment of inertia)
The snippet below shows how to computer PCA eigenvectors and eigenvalues (in this case could be interpreted as the moment of inertia).
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 |
#include <pcl/features/moment_of_inertia_estimation.h> #include <pcl/point_cloud.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/cloud_viewer.h> #include <pcl/io/ply_io.h> template <typename PointT> void computeMomentOfInertia(boost::shared_ptr<pcl::PointCloud<PointT> > cloud_ptr) { pcl::MomentOfInertiaEstimation <PointT> feature_extractor; feature_extractor.setInputCloud (cloud_ptr); feature_extractor.compute (); std::vector <float> moment_of_inertia; std::vector <float> eccentricity; pcl::PointXYZRGB min_point_AABB; pcl::PointXYZRGB max_point_AABB; pcl::PointXYZRGB min_point_OBB; pcl::PointXYZRGB max_point_OBB; pcl::PointXYZRGB position_OBB; Eigen::Matrix3f rotational_matrix_OBB; float major_value, middle_value, minor_value; Eigen::Vector3f major_vector, middle_vector, minor_vector; Eigen::Vector3f mass_center; feature_extractor.getMomentOfInertia (moment_of_inertia); feature_extractor.getEccentricity (eccentricity); feature_extractor.getAABB (min_point_AABB, max_point_AABB); feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); feature_extractor.getEigenValues (major_value, middle_value, minor_value); feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector); feature_extractor.getMassCenter (mass_center); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0, 0, 0); viewer->addCoordinateSystem (0.1); viewer->initCameraParameters (); pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2)); pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2)); pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2)); pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2)); viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector"); viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector"); viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector"); viewer->addPointCloud<pcl::PointXYZRGB> (cloud_ptr, "Moment Of Inertia (PCA Eigen Vector Axes)"); while (!viewer->wasStopped ()) { viewer->spinOnce (); } } int main(int argc, char **argv) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudrgb_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PLYReader PLYFileReader; const int offset=0; PLYFileReader.read< pcl::PointXYZRGB >(argv[1],*cloudrgb_ptr,offset); //pcl::io::loadPCDFile(argv[1],*cloudrgb_ptr); computeMomentOfInertia(cloudrgb_ptr); return 0; } |
