Category Archives: C++

Examples of Dynamic Programming with C++ and Matlab

In this tutorial, I will give you examples of using dynamic programming for solving the following problems:

1)Minimum number of coins for summing X.


2)The most (least) costly path on a grid (dynamic time warping).


3)Levenshtein edit distance.


4)Seam Carving. I have written a tutorial on that here and the recursive part is in the following lines:


The examples are taken from “Competitive Programmer’s Handbook” written by Antti Laaksonen.

Peak Signal-to-Noise Ratio (PSNR) in Image using OpenCV and Matlab


Peak signal-to-noise ratio (PSNR) shows the ratio between the maximum possible power of a signal and the power of the same image with noise. PSNR is usually expressed in logarithmic decibel scale.

\( MSE =1/m*n \sum_{i=0}^{m-1} \sum_{j=0}^{n-1}  [   Image( i,j)  -NoisyImage( i,j)  ]  ^2   \)
\( PSNR =10* \log_{10} \Bigg(MAXI^2/MSE\Bigg)  \)

MSE is Mean Square Error and MAXI is the maximum possible pixel value of the image. For instance if the image is uint8, it will be 255.


Ref [1],[2], [3], 4

Eigenvectors of PCL pointcloud (moment of inertia)

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


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

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.

White dots are original scene and rgb dots are from the cropped cloud. Values for the volume of cuboid are coming from sliders.

get template parameter name and type with Boost demangle

Have you ever tied to get the type of a variable by its name? for instance “int”  or “string“?

The following code snippet shows how to use boost demangle to get the type:


List of OpenCv matrix types and mapping numbers

ref: Special thanks to this post.

How to create octomap tree from mesh

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 them won’t appear in the final octomap tree. So I found the following solution:

1) First, download the latest version of binvox from (more about binvox, source code available here or you can try this)
2) Convert you mesh file into binvox file i.e

3) grab the binvox2bt.cpp from octomap at GitHub and compile it, then

4) visualize the bt file install octovis:


mesh file (Dude.ply).

octomap bt file (

The sample file can be downloaded at Dude.ply,


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


Roll, pitch, yaw using Eigen and KDL Frame


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:

or like this:

But note that unfortunately, because of how C++ works, you can not do this:

and with KDL Frame:


ROS packages for Dynamic Time Warping

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 good c++ library for Fast Dynamic Time Warping and I developed my wrapper to make a ROS package for that. Here you can download my code at GitHub.

The number of possible warping paths through the grid is exponentially explosive. The image is taken from [1].

A good wrapping function should have following characteristics:

  • monotonicity: The alignment path does not go back in “time” index.

    The image is taken from [1]

  • continuity: The alignment path does not jump in “time” index.

    The image is taken from [1]

  • boundary conditions: The alignment path starts at the bottom left and ends at the top right.

    The image is taken from [1].

  • warping window: A good alignment path is unlikely to wander too far from the diagonal.

    The image is taken from [1].

  • slope constraint: The alignment path should not be too steep or too shallow.

    The image is taken from [1].

  • References: [1]


References: [1]