static int moment_of_inertia_features(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& inputCluster) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ()); // Get cloud from publisher pcl::copyPointCloud(*inputCluster, *cloud); std::cout << "Hello1" << std::endl; //* pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor; std::cout << "Hello2" << std::endl; //* feature_extractor.setInputCloud (cloud); std::cout << "Hello3" << std::endl; //* feature_extractor.compute (); std::cout << "Hello4" << std::endl; //* std::vector <float> moment_of_inertia; std::vector <float> eccentricity; pcl::PointXYZ min_point_AABB; pcl::PointXYZ max_point_AABB; pcl::PointXYZ min_point_OBB; pcl::PointXYZ max_point_OBB; pcl::PointXYZ 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); std::cout << "Hello5" << std::endl; //* feature_extractor.getEccentricity (eccentricity); std::cout << "Hello6" << std::endl; //* feature_extractor.getAABB (min_point_AABB, max_point_AABB); std::cout << "Hello7" << std::endl; //* feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); std::cout << "Hello8" << std::endl; //* feature_extractor.getEigenValues (major_value, middle_value, minor_value); std::cout << "Hello9" << std::endl; //* feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector); std::cout << "Hello10" << std::endl; //* feature_extractor.getMassCenter (mass_center); std::cout << "Hello11" << std::endl; //* boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0, 0, 0); viewer->addCoordinateSystem (1.0); viewer->initCameraParameters (); viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud"); viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB"); Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); Eigen::Quaternionf quat (rotational_matrix_OBB); viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB"); 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"); while(!viewer->wasStopped()) { viewer->spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } return (0); }
int main (int argc, char** argv) { if (argc != 3) return (0); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ()); if (pcl::io::loadPLYFile (argv[1], *cloud) == -1) return (-1); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::io::loadPLYFile(argv[2], *cloud2); pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor; feature_extractor.setInputCloud (cloud); feature_extractor.compute (); std::vector <float> moment_of_inertia; std::vector <float> eccentricity; pcl::PointXYZ min_point_AABB; pcl::PointXYZ max_point_AABB; pcl::PointXYZ min_point_OBB; pcl::PointXYZ max_point_OBB; pcl::PointXYZ 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 (2.0, 0); viewer->initCameraParameters (); viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud"); viewer->addPointCloud<pcl::PointXYZ> (cloud2, "object cloud"); viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB"); Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); Eigen::Quaternionf quat (rotational_matrix_OBB); viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB"); 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"); while(!viewer->wasStopped()) { viewer->spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } return (0); }