Moment of inertia and eccentricity based descriptors

In this tutorial we will learn how to use the pcl::MomentOfInertiaEstimation class in order to obtain descriptors based on eccentricity and moment of inertia. This class also allows to extract axis aligned and oriented bounding boxes of the cloud. But keep in mind that extracted OBB is not the minimal possible bounding box.

Theoretical Primer

The idea of the feature extraction method is as follows. First of all the covariance matrix of the point cloud is calculated and its eigen values and vectors are extracted. You can consider that the resultant eigen vectors are normalized and always form the right-handed coordinate system (major eigen vector represents X-axis and the minor vector represents Z-axis). On the next step the iteration process takes place. On each iteration major eigen vector is rotated. Rotation order is always the same and is performed around the other eigen vectors, this provides the invariance to rotation of the point cloud. Henceforth, we will refer to this rotated major vector as current axis.

_images/eigen_vectors.png

For every current axis moment of inertia is calculated. Moreover, current axis is also used for eccentricity calculation. For this reason current vector is treated as normal vector of the plane and the input cloud is projected onto it. After that eccentricity is calculated for the obtained projection.

_images/projected_cloud.png

Implemented class also provides methods for getting AABB and OBB. Oriented bounding box is computed as AABB along eigen vectors.

The code

First of all you will need the point cloud for this tutorial. This is the one presented on the screenshots. Next what you need to do is to create a file moment_of_inertia.cpp in any editor you prefer and copy the following code inside of it:

  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
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>

int main (int argc, char** argv)
{
  if (argc != 2)
    return (0);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  if (pcl::io::loadPCDFile (argv[1], *cloud) == -1)
    return (-1);

  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 (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");

  //Eigen::Vector3f p1 (min_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p2 (min_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p3 (max_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p4 (max_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p5 (min_point_OBB.x, max_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p6 (min_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p7 (max_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p8 (max_point_OBB.x, max_point_OBB.y, min_point_OBB.z);

  //p1 = rotational_matrix_OBB * p1 + position;
  //p2 = rotational_matrix_OBB * p2 + position;
  //p3 = rotational_matrix_OBB * p3 + position;
  //p4 = rotational_matrix_OBB * p4 + position;
  //p5 = rotational_matrix_OBB * p5 + position;
  //p6 = rotational_matrix_OBB * p6 + position;
  //p7 = rotational_matrix_OBB * p7 + position;
  //p8 = rotational_matrix_OBB * p8 + position;

  //pcl::PointXYZ pt1 (p1 (0), p1 (1), p1 (2));
  //pcl::PointXYZ pt2 (p2 (0), p2 (1), p2 (2));
  //pcl::PointXYZ pt3 (p3 (0), p3 (1), p3 (2));
  //pcl::PointXYZ pt4 (p4 (0), p4 (1), p4 (2));
  //pcl::PointXYZ pt5 (p5 (0), p5 (1), p5 (2));
  //pcl::PointXYZ pt6 (p6 (0), p6 (1), p6 (2));
  //pcl::PointXYZ pt7 (p7 (0), p7 (1), p7 (2));
  //pcl::PointXYZ pt8 (p8 (0), p8 (1), p8 (2));

  //viewer->addLine (pt1, pt2, 1.0, 0.0, 0.0, "1 edge");
  //viewer->addLine (pt1, pt4, 1.0, 0.0, 0.0, "2 edge");
  //viewer->addLine (pt1, pt5, 1.0, 0.0, 0.0, "3 edge");
  //viewer->addLine (pt5, pt6, 1.0, 0.0, 0.0, "4 edge");
  //viewer->addLine (pt5, pt8, 1.0, 0.0, 0.0, "5 edge");
  //viewer->addLine (pt2, pt6, 1.0, 0.0, 0.0, "6 edge");
  //viewer->addLine (pt6, pt7, 1.0, 0.0, 0.0, "7 edge");
  //viewer->addLine (pt7, pt8, 1.0, 0.0, 0.0, "8 edge");
  //viewer->addLine (pt2, pt3, 1.0, 0.0, 0.0, "9 edge");
  //viewer->addLine (pt4, pt8, 1.0, 0.0, 0.0, "10 edge");
  //viewer->addLine (pt3, pt4, 1.0, 0.0, 0.0, "11 edge");
  //viewer->addLine (pt3, pt7, 1.0, 0.0, 0.0, "12 edge");

  while(!viewer->wasStopped())
  {
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return (0);
}

The explanation

Now let’s study out what is the purpose of this code. First few lines will be omitted, as they are obvious.

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  if (pcl::io::loadPCDFile (argv[1], *cloud) == -1)
    return (-1);

These lines are simply loading the cloud from the .pcd file.

  pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
  feature_extractor.setInputCloud (cloud);
  feature_extractor.compute ();

Here is the line where the instantiation of the pcl::MomentOfInertiaEstimation class takes place. Immediately after that we set the input cloud and start the computational process, that easy.

  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;

This is were we declare all necessary variables needed to store descriptors and bounding boxes.

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

These lines show how to access computed descriptors and other features.

  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");

These lines simply create the instance of PCLVisualizer class for result visualization. Here we also add the cloud and the AABB for visualization.

  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");

Visualization of the OBB is little more complex. So here we create a quaternion from the rotational matrix, set OBBs position and pass it to the visualizer.

  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");

This lines are responsible for eigen vectors visualization.

  //Eigen::Vector3f p1 (min_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p2 (min_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p3 (max_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p4 (max_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p5 (min_point_OBB.x, max_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p6 (min_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p7 (max_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p8 (max_point_OBB.x, max_point_OBB.y, min_point_OBB.z);

  //p1 = rotational_matrix_OBB * p1 + position;
  //p2 = rotational_matrix_OBB * p2 + position;
  //p3 = rotational_matrix_OBB * p3 + position;
  //p4 = rotational_matrix_OBB * p4 + position;
  //p5 = rotational_matrix_OBB * p5 + position;
  //p6 = rotational_matrix_OBB * p6 + position;
  //p7 = rotational_matrix_OBB * p7 + position;
  //p8 = rotational_matrix_OBB * p8 + position;

  //pcl::PointXYZ pt1 (p1 (0), p1 (1), p1 (2));
  //pcl::PointXYZ pt2 (p2 (0), p2 (1), p2 (2));
  //pcl::PointXYZ pt3 (p3 (0), p3 (1), p3 (2));
  //pcl::PointXYZ pt4 (p4 (0), p4 (1), p4 (2));
  //pcl::PointXYZ pt5 (p5 (0), p5 (1), p5 (2));
  //pcl::PointXYZ pt6 (p6 (0), p6 (1), p6 (2));
  //pcl::PointXYZ pt7 (p7 (0), p7 (1), p7 (2));
  //pcl::PointXYZ pt8 (p8 (0), p8 (1), p8 (2));

  //viewer->addLine (pt1, pt2, 1.0, 0.0, 0.0, "1 edge");
  //viewer->addLine (pt1, pt4, 1.0, 0.0, 0.0, "2 edge");
  //viewer->addLine (pt1, pt5, 1.0, 0.0, 0.0, "3 edge");
  //viewer->addLine (pt5, pt6, 1.0, 0.0, 0.0, "4 edge");
  //viewer->addLine (pt5, pt8, 1.0, 0.0, 0.0, "5 edge");
  //viewer->addLine (pt2, pt6, 1.0, 0.0, 0.0, "6 edge");
  //viewer->addLine (pt6, pt7, 1.0, 0.0, 0.0, "7 edge");
  //viewer->addLine (pt7, pt8, 1.0, 0.0, 0.0, "8 edge");
  //viewer->addLine (pt2, pt3, 1.0, 0.0, 0.0, "9 edge");
  //viewer->addLine (pt4, pt8, 1.0, 0.0, 0.0, "10 edge");
  //viewer->addLine (pt3, pt4, 1.0, 0.0, 0.0, "11 edge");
  //viewer->addLine (pt3, pt7, 1.0, 0.0, 0.0, "12 edge");

This huge amount of code shows how to work with the oriented bounding box. Note that you need to rotate each of the vertices of the OBB. This code does the same thing as PCLVisualizer::addCube () method. Its only purpose is to show how to work with OBB if you don’t have such usable method as PCLVisualizer::addCube ().

Few lines that left simply launch the visualization process.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(moment_of_inertia)

find_package(PCL 1.8 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (moment_of_inertia moment_of_inertia.cpp)
target_link_libraries (moment_of_inertia ${PCL_LIBRARIES})

After you have made the executable, you can run it. Simply do:

$ ./moment_of_inertia lamppost.pcd

You should see something similar to this image. Here AABB is yellow, OBB is red. You can also see the eigen vectors.

_images/moment_of_inertia.png