Robotics with ROS

Eigenvectors of PCL pointcloud (moment of inertia)

Leave a Comment / C++, Computer Vision, Tutorials / admin

The snippet below shows how to computer PCA eigenvectors and eigenvalues (in this case could be interpreted as the moment of inertia).

#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;
}

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

}

Eigenvectors of PCL pointcloud (moment of inertia) Read More »

A GUI ROS-package for cropping pcl pointcloud with dynamic reconfigure

2 Comments / C++, Computer Vision, Image Processing, ROS, Tutorials / admin

This ROS package enables you to crop the scene from Kinect (input topic type: PCL pointcloud). You can even enable fitting a plane to remove the ground from the scene and by adjusting correct parameter you can get the desired object from the scene. code available on my Github.

A GUI ROS-package for cropping pcl pointcloud with dynamic reconfigure Read More »

List of OpenCv matrix types and mapping numbers

Leave a Comment / C++, Computer Vision, Image Processing, Tutorials / admin

/*

            C1  C2   C3  C4
    CV_8U	0	8	16	24
    CV_8S	1	9	17	25
    CV_16U	2	10	18	26
    CV_16S	3	11	19	27
    CV_32S	4	12	20	28
    CV_32F	5	13	21	29
    CV_64F	6	14	22	30
    Unsigned 8bits uchar 0~255
    IplImage: IPL_DEPTH_8U
    Mat: CV_8UC1, CV_8UC2, CV_8UC3, CV_8UC4

    Signed 8bits char -128~127
    IplImage: IPL_DEPTH_8S
    Mat: CV_8SC1，CV_8SC2，CV_8SC3，CV_8SC4

    Unsigned 16bits ushort 0~65535
    IplImage: IPL_DEPTH_16U
    Mat: CV_16UC1，CV_16UC2，CV_16UC3，CV_16UC4

    Signed 16bits short -32768~32767
    IplImage: IPL_DEPTH_16S
    Mat: CV_16SC1，CV_16SC2，CV_16SC3，CV_16SC4

    Signed 32bits int -2147483648~2147483647
    IplImage: IPL_DEPTH_32S
    Mat: CV_32SC1，CV_32SC2，CV_32SC3，CV_32SC4

    Float 32bits float -1.18*10-38~3.40*10-38
    IplImage: IPL_DEPTH_32F
    Mat: CV_32FC1，CV_32FC2，CV_32FC3，CV_32FC4

    Double 64bits double
    Mat: CV_64FC1，CV_64FC2，CV_64FC3，CV_64FC4

    Unsigned 1bit bool
    IplImage: IPL_DEPTH_1U
*/
//example:

    int number_of_rows, number_of_columns;
    number_of_rows=3;
    number_of_columns=4;
    cv::Mat mat_CV_8U(number_of_rows, number_of_columns,CV_8UC1);
    std::cout<<"mat_CV_8U.type(): " <<mat_CV_8U.type() <<std::endl;


    cv::Mat mat_CV_64FC1(number_of_rows, number_of_columns,CV_64FC1);
    std::cout<<"mat_CV_64FC1.type(): " <<mat_CV_64FC1.type() <<std::endl;

C1 C2 C3 C4

CV_8U 0 8 16 24

CV_8S 1 9 17 25

CV_16U 2 10 18 26

CV_16S 3 11 19 27

CV_32S 4 12 20 28

CV_32F 5 13 21 29

CV_64F 6 14 22 30

Unsigned 8bits uchar 0~255

IplImage: IPL_DEPTH_8U

Mat: CV_8UC1, CV_8UC2, CV_8UC3, CV_8UC4

Signed 8bits char -128~127

IplImage: IPL_DEPTH_8S

Mat: CV_8SC1，CV_8SC2，CV_8SC3，CV_8SC4

Unsigned 16bits ushort 0~65535

IplImage: IPL_DEPTH_16U

Mat: CV_16UC1，CV_16UC2，CV_16UC3，CV_16UC4

Signed 16bits short -32768~32767

IplImage: IPL_DEPTH_16S

Mat: CV_16SC1，CV_16SC2，CV_16SC3，CV_16SC4

Signed 32bits int -2147483648~2147483647

IplImage: IPL_DEPTH_32S

Mat: CV_32SC1，CV_32SC2，CV_32SC3，CV_32SC4

Float 32bits float -1.18*10-38~3.40*10-38

IplImage: IPL_DEPTH_32F

Mat: CV_32FC1，CV_32FC2，CV_32FC3，CV_32FC4

Double 64bits double

Mat: CV_64FC1，CV_64FC2，CV_64FC3，CV_64FC4

Unsigned 1bit bool

IplImage: IPL_DEPTH_1U

//example:

int number_of_rows, number_of_columns;

number_of_rows=3;

number_of_columns=4;

cv::Mat mat_CV_8U(number_of_rows, number_of_columns,CV_8UC1);

std::cout<<"mat_CV_8U.type(): " <<mat_CV_8U.type() <<std::endl;

cv::Mat mat_CV_64FC1(number_of_rows, number_of_columns,CV_64FC1);

std::cout<<"mat_CV_64FC1.type(): " <<mat_CV_64FC1.type() <<std::endl;

ref: Special thanks to this post.

List of OpenCv matrix types and mapping numbers Read More »

Kernel Density Estimation (KDE) for estimating probability distribution function

Leave a Comment / Machine Learning, Python, Tutorials / admin

There are several approaches for estimating the probability distribution function of a given data: 1)Parametric 2)Semi-parametric 3)Non-parametric A parametric one is GMM via algorithm such as expectation maximization. Here is my other post for expectation maximization. Example of Non-parametric is the histogram, where data are assigned to only one bin and depending on the number bins that fall within

Kernel Density Estimation (KDE) for estimating probability distribution function Read More »

Silhouette coefficient for finding optimal number of clusters

1 Comment / Machine Learning, Python, Tutorials / admin

Silhouette coefficient is another method to determine the optimal number of clusters. Here I introduced c-index earlier. The silhouette coefficient of a data measures how well data are assigned to its own cluster and how far they are from other clusters. A silhouette close to 1 means the data points are in an appropriate cluster and a silhouette

Silhouette coefficient for finding optimal number of clusters Read More »

Finding optimal number of Clusters by using Cluster validation

1 Comment / Machine Learning, Python, Tutorials / admin

This module finds the optimal number of components (number of clusters) for a given dataset. In order to find the optimal number of components for, first we used k-means algorithm with a different number of clusters, starting from 1 to a fixed max number. Then we checked the cluster validity by deploying \( C-index \) algorithm and

Finding optimal number of Clusters by using Cluster validation Read More »

How to create octomap tree from mesh

2 Comments / C++, ROS, Tutorials / admin

I was looking for a way to create an octomap tree from arbitrary mesh file. Well at first I tried to convert my data into PCL pointcloud and then convert them into an octomap tree. But the problem was, for instance when you a cuboid mesh, you only have 8 vertex, which gives you 8 point and the walls between

How to create octomap tree from mesh Read More »

Octomap explanierend

Leave a Comment / Machine Learning, ROS, Tutorials / admin

In this tutorial, I explain the concept, probabilistic sensor fusion model and the sensor model used in Octomap library. related publication: OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees 1)Octamap Volumetric Model 2)Probabilistic Sensor Fusion Model 3)Sensor Model for Laser Range Data

Octomap explanierend Read More »

Finding roll, pitch yaw from 3X3 rotation matrix with Eigen

Leave a Comment / C++, ROS, Tutorials / admin

/*  http://planning.cs.uiuc.edu/node103.html

    |r11 r12 r13 |
    |r21 r22 r23 |
    |r31 r32 r33 |

    yaw: alpha=arctan(r21/r11)
    pitch: beta=arctan(-r31/sqrt( r32^2+r33^2 ) )
    roll: gamma=arctan(r32/r33)
*/
    double roll, pitch, yaw;
    roll=M_PI/3;
    pitch=M_PI/4;
    yaw=M_PI/6;
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaternion<double> q =  yawAngle*pitchAngle *rollAngle;
    Eigen::Matrix3d rotationMatrix = q.matrix();



    std::cout<<"roll is Pi/" <<M_PI/atan2( rotationMatrix(2,1),rotationMatrix(2,2) ) <<std::endl;
    std::cout<<"pitch: Pi/" <<M_PI/atan2( -rotationMatrix(2,0), std::pow( rotationMatrix(2,1)*rotationMatrix(2,1) +rotationMatrix(2,2)*rotationMatrix(2,2) ,0.5  )  ) <<std::endl;
    std::cout<<"yaw is Pi/" <<M_PI/atan2( rotationMatrix(1,0),rotationMatrix(0,0) ) <<std::endl;

/* http://planning.cs.uiuc.edu/node103.html

|r11 r12 r13 |

|r21 r22 r23 |

|r31 r32 r33 |

yaw: alpha=arctan(r21/r11)

pitch: beta=arctan(-r31/sqrt( r32^2+r33^2 ) )

roll: gamma=arctan(r32/r33)

double roll, pitch, yaw;

roll=M_PI/3;

pitch=M_PI/4;

yaw=M_PI/6;

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());

Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());

Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

Eigen::Quaternion<double> q = yawAngle*pitchAngle *rollAngle;

Eigen::Matrix3d rotationMatrix = q.matrix();

std::cout<<"roll is Pi/" <<M_PI/atan2( rotationMatrix(2,1),rotationMatrix(2,2) ) <<std::endl;

std::cout<<"pitch: Pi/" <<M_PI/atan2( -rotationMatrix(2,0), std::pow( rotationMatrix(2,1)*rotationMatrix(2,1) +rotationMatrix(2,2)*rotationMatrix(2,2) ,0.5 ) ) <<std::endl;

std::cout<<"yaw is Pi/" <<M_PI/atan2( rotationMatrix(1,0),rotationMatrix(0,0) ) <<std::endl;

Finding roll, pitch yaw from 3X3 rotation matrix with Eigen Read More »

Roll, pitch, yaw using Eigen and KDL Frame

Leave a Comment / C++, Linear Algebra, ROS, Tutorials / admin

/*
yaw:
    A yaw is a counterclockwise rotation of alpha about the  z-axis. The rotation matrix is given by

    R_z

    |cos(alpha) -sin(alpha) 0|
    |sin(apha)   cos(alpha) 0|
    |    0            0     1|

pitch:
    R_y
    A pitch is a counterclockwise rotation of  beta about the  y-axis. The rotation matrix is given by

    |cos(beta)  0   sin(beta)|
    |0          1       0    |
    |-sin(beta) 0   cos(beta)|

roll:
    A roll is a counterclockwise rotation of  gamma about the  x-axis. The rotation matrix is given by
    R_x
    |1          0           0|
    |0 cos(gamma) -sin(gamma)|
    |0 sin(gamma)  cos(gamma)|



    It is important to note that   R_z R_y R_x performs the roll first, then the pitch, and finally the yaw
    Roration matrix: R_z*R_y*R_x
*/

    double roll, pitch, yaw;
    roll=M_PI/3;
    pitch=M_PI/4;
    yaw=M_PI/6;
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());




    Eigen::Quaternion<double> q =  yawAngle*pitchAngle *rollAngle;

    Eigen::Matrix3d rotationMatrix = q.matrix();
    std::cout<<rotationMatrix <<std::endl;

yaw:

A yaw is a counterclockwise rotation of alpha about the z-axis. The rotation matrix is given by

R_z

|cos(alpha) -sin(alpha) 0|

|sin(apha) cos(alpha) 0|

| 0 0 1|

pitch:

R_y

A pitch is a counterclockwise rotation of beta about the y-axis. The rotation matrix is given by

|cos(beta) 0 sin(beta)|

|0 1 0 |

|-sin(beta) 0 cos(beta)|

roll:

A roll is a counterclockwise rotation of gamma about the x-axis. The rotation matrix is given by

R_x

|1 0 0|

|0 cos(gamma) -sin(gamma)|

|0 sin(gamma) cos(gamma)|

It is important to note that R_z R_y R_x performs the roll first, then the pitch, and finally the yaw

Roration matrix: R_z*R_y*R_x

double roll, pitch, yaw;

roll=M_PI/3;

pitch=M_PI/4;

yaw=M_PI/6;

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());

Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());

Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

Eigen::Quaternion<double> q = yawAngle*pitchAngle *rollAngle;

Eigen::Matrix3d rotationMatrix = q.matrix();

std::cout<<rotationMatrix <<std::endl;

From Eigen documentation: If you are working with OpenGL 4×4 matrices then Affine3f and Affine3d are what you want. Since Eigen defaults to column-major storage, you can directly use the Transform::data() method to pass your transformation matrix to OpenGL. construct a Transform:

Transform t(AngleAxis(angle,axis));

1	Transform t(AngleAxis(angle,axis));

or like this:

Transform t;
t = AngleAxis(angle,axis);

1 2	Transform t; t = AngleAxis(angle,axis);

But note that unfortunately, because of

Roll, pitch, yaw using Eigen and KDL Frame Read More »

Cookie	Duration	Description
cookielawinfo-checbox-analytics	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Analytics".
cookielawinfo-checbox-functional	11 months	The cookie is set by GDPR cookie consent to record the user consent for the cookies in the category "Functional".
cookielawinfo-checbox-others	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Other.
cookielawinfo-checkbox-necessary	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookies is used to store the user consent for the cookies in the category "Necessary".
cookielawinfo-checkbox-performance	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Performance".
viewed_cookie_policy	11 months	The cookie is set by the GDPR Cookie Consent plugin and is used to store whether or not user has consented to the use of cookies. It does not store any personal data.