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 »
|
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 |
/* 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 »
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 »
|
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 |
/* 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 »
|
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 |
/* 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:
|
1 |
Transform t(AngleAxis(angle,axis)); |
or like this:
|
1 2 |
Transform t; t = AngleAxis(angle,axis); |
But note that unfortunately, because of
Roll, pitch, yaw using Eigen and KDL Frame Read More »
Dynamic Time Warping (DTW) is a method to align two sequences under certain constraints. For instance, two trajectories that are very similar but one of them performed in a longer time. The alignment should be is such way that minimizes the distance between these two sequences. Here I have done DTW between two-time series with python. I found a
ROS packages for Dynamic Time Warping Read More »
Gaussian Mixture Regression is basically Multivariate normal distribution with Conditional distribution. The more about the theory could be found at [1], [2], [3], [4]. For this work, I have added the functionality of adding Gaussian Mixture Regression to this project on the GitHub by forking the main project, my forked project can be download at here Github The main changes
Gaussian Mixture Regression Read More »
I found a really good code at GitHub for fitting a Gaussian Mixture Model (GMM) with Expectation Maximization (EM) for ROS. There are so many parameters that you can change. Some of the most important ones are:
|
1 2 3 4 5 6 7 |
// minimum number of gaussians #define PARAM_NAME_GAUSSIAN_COUNT_MIN "gaussian_count_min" #define PARAM_DEFAULT_GAUSSIAN_COUNT_MIN 1 // search will terminate when the gaussian count reaches this #define PARAM_NAME_GAUSSIAN_COUNT_MAX "gaussian_count_max" #define PARAM_DEFAULT_GAUSSIAN_COUNT_MAX 10 |
To find the optimal number of components, it uses Bayesian information criterion (BIC). There are other methods to find
Expectation Maximization algorithm to obtain Gaussian mixture models for ROS Read More »
Eigen is a great tool for matrix operations, here I found a small piece of code in Github that enables you to generate multivariate normal distribution samples using C++11 and Eigen library. The below is an example:
|
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 |
#include <fstream> #include <Eigen/Eigen> #include <Eigen/Core> #include <Eigen/Dense> #include "eigenmvn.h" int main(int argc, char ** argv) { //number of columns const uint n_cols = 2; // number of rows const uint n_rows = 5000; Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> total_gaussian_data_eigen, first_gaussian_data_eigen,second_gaussian_data_eigen; first_gaussian_data_eigen.setZero(n_rows,n_cols); second_gaussian_data_eigen.setZero(n_rows,n_cols); //2*n_rows since we want to concat first and second and put it in this matrix total_gaussian_data_eigen.setZero(2*n_rows,n_cols); Eigen::Vector2f mean; Eigen::Matrix2f covar; float theta; mean << 0,0; // Set the mean /* our covar matrix: |10 0| |0 1| */ covar <<10,0,0,1; //first gaussian Eigen::EigenMultivariateNormal<float> normX_solver1(mean,covar); first_gaussian_data_eigen<< normX_solver1.samples(n_rows).transpose() ; //secodn gaussian mean << 4,4; // Set the mean // Create a covariance matrix rotated clockwise by theta theta=M_PI/4.0; covar=genCovar(1,4,theta); Eigen::EigenMultivariateNormal<float> normX_solver2(mean,covar); second_gaussian_data_eigen<<normX_solver2.samples(n_rows).transpose() ; //concating two gaussians total_gaussian_data_eigen<<first_gaussian_data_eigen,second_gaussian_data_eigen; //writing data into output in csv format std::cout<<"x,y\n"; for (uint i = 0; i < total_gaussian_data_eigen.rows(); i++) { for (uint h = 0; h < n_cols-1; h++) { std::cout<<total_gaussian_data_eigen(i,h); std::cout<<","; } std::cout<<total_gaussian_data_eigen(i,n_cols-1); std::cout<<std::endl; } } |
Generating multivariate normal distribution samples using C++11 and Eigen library. Read More »
This work is the second part of my master thesis (part I). In this part, I developed an algorithm for 2D pose estimation of the human body. To do this, I created a software with QT that could generate 2D contours representing human body. Then I send these contours for evaluation to CNS(Contrast Normalized Sobel) [1]
2D pose estimation of human body using CNS and PCA Read More »
