使用pcl::MomentOfInertiaEstimation
类来获取基于偏心率和惯性矩的描述符。该类还允许提取云的轴对齐和定向的边界框。但是提取的OBB并非最小可能的边界框。
包围体(包容盒)是一个简单的几何空间,里面包含着复杂形状的物体。为物体添加包围体的目的是快速的进行碰撞检测或者进行精确的碰撞检测之前进行过滤(即当包围体碰撞,才进行精确碰撞检测和处理)。包围体类型包括球体、轴对齐包围盒(AABB)、有向包围盒(OBB)、8-DOP以及凸壳(CONVEX HULL)。
常见包容盒( Bounding Volumes)分类:
如上图所示,还有K-DOP,CONVEX HULL等包容盒,越靠右,包容效果好、越紧密。但是检测速度更慢,也更消耗内存资源。
#include
#include
#include
#include
#include
#include
using namespace std::chrono_literals;
int main (int argc, char** argv)
{
//if (argc != 2)
// return (0);
pcl::PointCloud::Ptr cloud (new pcl::PointCloud ());
if (pcl::io::loadPCDFile ("table_scene_lms400_downsampled.pcd", *cloud) == -1)
return (-1);
pcl::MomentOfInertiaEstimation feature_extractor;
feature_extractor.setInputCloud (cloud);
feature_extractor.compute ();
std::vector moment_of_inertia;
std::vector 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);
pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (1);
viewer->initCameraParameters ();
viewer->addPointCloud (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");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "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");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "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);
std::this_thread::sleep_for(100ms);
}
return (0);
}
黄色立方体为AABB包容盒,白色立方体为OBB包容盒
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
class FeatureCloud
{
public:
// A bit of shorthand
typedef pcl::PointCloud PointCloud;
typedef pcl::PointCloud SurfaceNormals;
typedef pcl::PointCloud LocalFeatures;
typedef pcl::search::KdTree SearchMethod;
FeatureCloud () :
search_method_xyz_ (new SearchMethod),
normal_radius_ (0.02f),
feature_radius_ (0.02f)
{}
~FeatureCloud () {}
// Process the given cloud
void
setInputCloud (PointCloud::Ptr xyz)
{
xyz_ = xyz;
processInput ();
}
// Load and process the cloud in the given PCD file
void
loadInputCloud (const std::string &pcd_file)
{
xyz_ = PointCloud::Ptr (new PointCloud);
pcl::io::loadPCDFile (pcd_file, *xyz_);
processInput ();
}
// Get a pointer to the cloud 3D points
PointCloud::Ptr
getPointCloud () const
{
return (xyz_);
}
// Get a pointer to the cloud of 3D surface normals
SurfaceNormals::Ptr
getSurfaceNormals () const
{
return (normals_);
}
// Get a pointer to the cloud of feature descriptors
LocalFeatures::Ptr
getLocalFeatures () const
{
return (features_);
}
protected:
// Compute the surface normals and local features
void
processInput ()
{
computeSurfaceNormals ();
computeLocalFeatures ();
}
// Compute the surface normals
void
computeSurfaceNormals ()
{
normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
pcl::NormalEstimation norm_est;
norm_est.setInputCloud (xyz_);
norm_est.setSearchMethod (search_method_xyz_);
norm_est.setRadiusSearch (normal_radius_);
norm_est.compute (*normals_);
}
// Compute the local feature descriptors
void
computeLocalFeatures ()
{
features_ = LocalFeatures::Ptr (new LocalFeatures);
pcl::FPFHEstimation fpfh_est;
fpfh_est.setInputCloud (xyz_);
fpfh_est.setInputNormals (normals_);
fpfh_est.setSearchMethod (search_method_xyz_);
fpfh_est.setRadiusSearch (feature_radius_);
fpfh_est.compute (*features_);
}
private:
// Point cloud data
PointCloud::Ptr xyz_;
SurfaceNormals::Ptr normals_;
LocalFeatures::Ptr features_;
SearchMethod::Ptr search_method_xyz_;
// Parameters
float normal_radius_;
float feature_radius_;
};
class TemplateAlignment
{
public:
// A struct for storing alignment results
struct Result
{
float fitness_score;
Eigen::Matrix4f final_transformation;
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
TemplateAlignment () :
min_sample_distance_ (0.05f),
max_correspondence_distance_ (0.01f*0.01f),
nr_iterations_ (500)
{
// Initialize the parameters in the Sample Consensus Initial Alignment (SAC-IA) algorithm
sac_ia_.setMinSampleDistance (min_sample_distance_);
sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
sac_ia_.setMaximumIterations (nr_iterations_);
}
~TemplateAlignment () {}
// Set the given cloud as the target to which the templates will be aligned
void
setTargetCloud (FeatureCloud &target_cloud)
{
target_ = target_cloud;
sac_ia_.setInputTarget (target_cloud.getPointCloud ());
sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
}
// Add the given cloud to the list of template clouds
void
addTemplateCloud (FeatureCloud &template_cloud)
{
templates_.push_back (template_cloud);
}
// Align the given template cloud to the target specified by setTargetCloud ()
void
align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)
{
sac_ia_.setInputSource (template_cloud.getPointCloud ());
sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());
pcl::PointCloud registration_output;
sac_ia_.align (registration_output);
result.fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);
result.final_transformation = sac_ia_.getFinalTransformation ();
}
// Align all of template clouds set by addTemplateCloud to the target specified by setTargetCloud ()
void
alignAll (std::vector > &results)
{
results.resize (templates_.size ());
for (std::size_t i = 0; i < templates_.size (); ++i)
{
align (templates_[i], results[i]);
}
}
// Align all of template clouds to the target cloud to find the one with best alignment score
int
findBestAlignment (TemplateAlignment::Result &result)
{
// Align all of the templates to the target cloud
std::vector > results;
alignAll (results);
// Find the template with the best (lowest) fitness score
float lowest_score = std::numeric_limits::infinity ();
int best_template = 0;
for (std::size_t i = 0; i < results.size (); ++i)
{
const Result &r = results[i];
if (r.fitness_score < lowest_score)
{
lowest_score = r.fitness_score;
best_template = (int) i;
}
}
// Output the best alignment
result = results[best_template];
return (best_template);
}
private:
// A list of template clouds and the target to which they will be aligned
std::vector templates_;
FeatureCloud target_;
// The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
pcl::SampleConsensusInitialAlignment sac_ia_;
float min_sample_distance_;
float max_correspondence_distance_;
int nr_iterations_;
};
// Align a collection of object templates to a sample point cloud
int
main (int argc, char **argv)
{
if (argc < 3)
{
printf ("No target PCD file given!\n");
return (-1);
}
// Load the object templates specified in the object_templates.txt file
std::vector object_templates;
std::ifstream input_stream (argv[1]);
object_templates.resize (0);
std::string pcd_filename;
while (input_stream.good ())
{
std::getline (input_stream, pcd_filename);
if (pcd_filename.empty () || pcd_filename.at (0) == '#') // Skip blank lines or comments
continue;
FeatureCloud template_cloud;
template_cloud.loadInputCloud (pcd_filename);
object_templates.push_back (template_cloud);
}
input_stream.close ();
// Load the target cloud PCD file
pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
pcl::io::loadPCDFile (argv[2], *cloud);
// Preprocess the cloud by...
// ...removing distant points
const float depth_limit = 1.0;
pcl::PassThrough pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, depth_limit);
pass.filter (*cloud);
// ... and downsampling the point cloud
const float voxel_grid_size = 0.005f;
pcl::VoxelGrid vox_grid;
vox_grid.setInputCloud (cloud);
vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
//vox_grid.filter (*cloud); // Please see this http://www.pcl-developers.org/Possible-problem-in-new-VoxelGrid-implementation-from-PCL-1-5-0-td5490361.html
pcl::PointCloud::Ptr tempCloud (new pcl::PointCloud);
vox_grid.filter (*tempCloud);
cloud = tempCloud;
// Assign to the target FeatureCloud
FeatureCloud target_cloud;
target_cloud.setInputCloud (cloud);
// Set the TemplateAlignment inputs
TemplateAlignment template_align;
for (std::size_t i = 0; i < object_templates.size (); ++i)
{
template_align.addTemplateCloud (object_templates[i]);
}
template_align.setTargetCloud (target_cloud);
// Find the best template alignment
TemplateAlignment::Result best_alignment;
int best_index = template_align.findBestAlignment (best_alignment);
const FeatureCloud &best_template = object_templates[best_index];
// Print the alignment fitness score (values less than 0.00002 are good)
printf ("Best fitness score: %f\n", best_alignment.fitness_score);
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);
Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf ("R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf ("t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
// Save the aligned template for visualization
pcl::PointCloud transformed_cloud;
pcl::transformPointCloud (*best_template.getPointCloud (), transformed_cloud, best_alignment.final_transformation);
pcl::io::savePCDFileBinary ("output.pcd", transformed_cloud);
return (0);
}