Orthonormalize a Rotation Matrix

If you use a 3×3 R matrix to store the result of the multiplication of a series of rotation transformations, it could be the case that sometimes you end up with a matrix that is not orthonormal (i.e. det(R) != 1 and R.inv(R) != eye).

In such cases, you need to re-orthonormalize the rotation matrix, which can be done in either of the two following ways:

  1. Use the SVD decomposition as follows (MATLAB syntax):
  2. Alternatively, you can express the rotation matrix as a quaternion, normalize the quaternion, then convert the quaternion back to the rotation matrix form. In MATLAB, you could do this:

    Note that the above syntax requires MATLAB’s robotics toolbox.

Align Depth and Color Frames – Depth and RGB Registration

Sometimes it is necessary to create a point cloud from a given depth and color (RGB) frame. This is especially the case when a scene is captured using depth cameras such as Kinect. The process of aligning the depth and the RGB frame is called “registration” and it is very easy to do (and the algorithm’s pseudo-code is surprisingly hard to find with a simple Google search! ūüėÄ )

To perform registration, you would need 4 pieces of information:

  1. The depth camera intrinsics:
    1. Focal lengths fxd and fyd (in pixel units)
    2. Optical centers (sometimes called image centers) Cxd and Cyd
  2. The RGB camera intrinsics:
    1. Focal lengths fxrgb and fyrgb (in pixel units)
    2. Optical centers (sometimes called image centers) Cxrgb and Cyrgb
  3. The extrinsics relating the depth camera to the¬†RGB camera. This is a 4×4 matrix containing rotation and translation values.
  4. (Obviously) the depth and the RGB frames. Note that they do not have to have the same resolution. Applying the intrinsics takes care of the resolution issue. Using camera’s such as Kinect, the depth values should usually be in meters (the unit of the depth values is very important as using incorrect units will result in a registration in which the colors and the depth values are off and are clearly misaligned).
    Also, note that some data sets apply a scale and a bias to the depth values in the depth frame. Make sure to account for this scaling and offsetting before proceeding. In order words, make sure there are no scales applied to the depth values of your depth frame.

Let depthData contain the depth frame and rgbData contain the RGB frame. The pseudo-code for registration in MATLAB is as follows:

A few things to note here:

  1. The indices x and y in the second group of for loops may be invalid which indicates that the obtained RGB pixel is not visible to the RGB camera.
  2. Some kind of interpolation may be necessary when using x and y. I just did rounding.
  3. This code can be readily used with savepcd function to save the point cloud into a PCL compatible format.

The registration formulas were obtained from the paper “On-line Incremental 3D¬†Human Body Reconstruction for HMI¬†or AR¬†Applications” by Almeida et al (2011).¬†The same formulas can be found¬†here. Hope this helps ūüôā


3D Line Fitting in 5 Easy Steps with SVD

Least squares fit is used for 2D line fitting. In 3D space, the line is called 3D Orthogonal Distance Regression (ODR) line. The line can be easily found in 3D using SVD (singular value decomposition).

Assuming that we have a bunch of 3D points (x0, y0, z0) to (xn, yn, zn), the algorithm (in MATLAB) is as follows:


Fix MATLAB Error: “dlopen: cannot load any more object with static TLS”

On Linux, you may occasionally encounter the error “dlopen: cannot load any more object with static TLS” in MATLAB. This is a known bug since way back!

To fix, create a file “startup.m” in the directory that you start MATLAB from with the following content:

I know! It’s ugly… But it works!!

EDIT:¬†Since it was not clear, the folder that you start MATLAB from is by default “~/Documents/MATLAB” under Linux. On Windows, that would be “Documents\MATLAB”.

Tutorial : Use CUDA and C++11 Code in MATLAB

As it turns out, incorporating CUDA code in MATLAB can be easily done! ūüôā

MATLAB provides functionality for loading arbitrary dynamic libraries and invoking their functions. This is especially easy for invoking C/C++ code in a MATLAB program. Such functionality is possible using the so called MEX functions.


Mex functions can be created with the mex command in MATLAB. Essentially, mex takes as input a C/C++ source file, invokes the default C/C++ compiler installed in the operating system (GCC or CL), and creates a mexa64 file (on a 64-bit machine) which can be used like any other MATLAB function.

The C/C++ file that is passed to mex must have the following included in it:

The arguments that are passed from MATLAB are accessible using the prhs parameter (which stands for parameters-right hand side). Any output that the gateway function generates can be returned using the plhs parameter (which stands for parameter-left hand side). The number of the arguments that are passed to the gateway function is stored in the nrhs parameter and the number of outputs that the MATLAB code expects from the gateway function is stored in the nlhs parameter. From this point on, I refer to the file containing the above code as the mex gateway file. Also, I will refer to the mexFunction above as the gateway function.

Continue reading Tutorial : Use CUDA and C++11 Code in MATLAB