Explaining Quaternion Rotation Matrix

With a quaternion \({\bf q} = q_{0} + q_{1}i + q_{2}j + q_{3}k\), the rotation matrix is:

\[ \begin{bmatrix}
    q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2q_{1}q_{2}-2q_{0}q_{3} & 2q_{1}q_{3}+2q_{0}q_{2} \\
    2q_{1}q_{2}+2q_{0}q_{3} & q_{0}^{2}+q_{2}^{2}-q_{1}^{2}-q_{3}^{2} & 2q_{2}q_{3}-2q_{0}q_{1}\\
    2q_{1}q_{3}-2q_{0}q_{2} & 2q_{2}q_{3}+2q_{0}q_{1} &  q_{0}^{2}+q_{3}^{2}-q_{1}^{2}-q_{2}^{2}  \\
\end{bmatrix}
\]

A quaternion is determined by two factors: normalized rotation axis \((a_{x},a_{y},a_{z})\) and rotation angle \(\theta\). \({\bf q} = cos(\theta/2) + (a_{x}i + a_{y}j + a_{z}k)sin(\theta/2)\) and \({\bf q^{-1}} = cos(\theta/2) - (a_{x}i + a_{y}j + a_{z}k)sin(\theta/2)\). This means that \(||{\bf q}|| = 1\).

The rotation matrix in term of \((a_{x},a_{y},a_{z})\) and \(\theta\) is:
\[ \begin{bmatrix}
1+(a_{x}^{2}-1)(1-cos(\theta)) & -a_{z}sin(\theta)+a_{x}a_{y}(1-cos(\theta)) & a_{y}sin(\theta)+a_{x}a_{z}(1-cos(\theta))\\
a_{z}sin(\theta)+a_{x}a_{y}(1-cos(\theta)) & 1+(a_{y}^2-1)(1-cos(\theta)) & -a_{x}sin(\theta)+a_{y}a_{z}(1-cos(\theta))\\
-a_{y}sin(\theta)+a_{x}a_{z}(1-cos(\theta)) & a_{x}sin(\theta)+a_{y}a_{z}(1-cos(\theta)) & 1+(a_{z}^{2}-1)(1-cos(\theta))
\end{bmatrix}
\]

From the expression of quaternion, we have
\[cos(\theta) = cos^{2}(\theta/2)-sin^{2}(\theta/2)
=q_{0}^2-(q_{1}^2+q_{2}^2+q_{3}^2)\]
\[1-cos(\theta) = 1-cos^{2}(\theta/2)+sin^{2}(\theta/2)
=2sin^{2}(\theta/2)\]
\[(a_{x},a_{y},a_{z})=(q_{1},q_{2},q_{3})/sin(\theta/2)\]

Based on these equations, we can derive the quaternion rotation matrix. For example,
\[1+(a_{x}^{2}-1)(1-cos(\theta)) = cos(\theta)+a_{x}^{2}(1-cos(\theta))\\
=q_{0}^2-(q_{1}^2+q_{2}^2+q_{3}^2)+2q_{1}^{2}\\
= q_{0}^2+q_{1}^2-q_{2}^2-q_{3}^2\]
\[-a_{z}sin(\theta)+a_{x}a_{y}(1-cos(\theta)) = \frac{-q_{3}}{sin(\theta/2)}sin(\theta)+\frac{q_{1}q_{2}}{sin^{2}(\theta/2)}2sin^{2}(\theta/2)\\
=2q_{1}q_{2}-2q_{0}q_{3}\]

Merging Kinect 3D Point Clouds With 2D/3D Data Fusion

How to merge 3D point clouds is a problem we face everyday while dealing with 3D data. One approach is to use ICP algorithm to merge multiple point clouds. However, in our own practice, ICP algorithm does not give the best results. It is either due to the limitation of ICP such as it does not guarantee global optimality, or just because we are not technically savvy enough to bring out the best side of ICP algorithm. Instead of ICP, we pursue an alternative solution. As the first step, we identify 2D features in images. Then by using optical flow method such as Lucas-Kanade, we are able to align images. By mapping 2D features to its correspondence in 3D point clouds, the alignment information acquired in image analysis can be used to merge 3D point clouds. The source code and test data can be downloaded from https://drive.google.com/drive/folders/0B05MVyfGr8lmOXV1dXZ6STZDalk

The first step is to use openCV library to identify feature points in an image. By processing the image through a differential filter in both horizontal and vertical directions, a Hessian matrix for each point of an image can be obtained as:
\[H(p)= \begin{bmatrix}
\frac{\partial^2 I}{\partial x^{2}} & \frac{\partial^2 I}{\partial x \partial y}\\
\frac{\partial^2 I}{\partial x \partial y} & \frac{\partial^2 I}{\partial y^{2}} \\
\end{bmatrix}\]
\(\partial I/\partial x\) and \(\partial I/\partial y\) come from passing the image through differential filter in horizontal and vertical directions. Various ways exist for identifying feature points based on Hessian matrices. Harris' method computes the difference between the determinant and trace and then compare the difference to a threshold. Shi and Tomasi's method compares the smaller one of the two eigenvalues of the matrix \(H(p)\) to a threshold. The cvGoodFeaturesToTrack() function in OpenCV uses Shi and Tomasi's method. The input "imgA" is the input image. In addition, cvFindCornerSubPix() function is used for fractionally interpolation to improve accuracy of feature point locations.

    // To find features to track

    IplImage* eig_image = cvCreateImage( img_sz, IPL_DEPTH_32F, 1 );
    IplImage* tmp_image = cvCreateImage( img_sz, IPL_DEPTH_32F, 1 );
    int corner_count = MAX_CORNERS;
    CvPoint2D32f* cornersA = new CvPoint2D32f[ MAX_CORNERS ];
    cvGoodFeaturesToTrack(
        imgA,
        eig_image,
        tmp_image,
        cornersA,
        &corner_count,
        0.01,
        5.0,
        0,
        3,
        0,
        0.04
    );
    cvFindCornerSubPix(
        imgA,
        cornersA,
        corner_count,
        cvSize(win_size,win_size),
        cvSize(-1,-1),
        cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)
    );

After feature points found, pyramid Lucas-Kanade is used to trace the feature points. The goal is to find the corresponding points in image B for features in image A.

    // Call the Lucas Kanade algorithm

    //

    char features_found[ MAX_CORNERS ];

    float feature_errors[ MAX_CORNERS ];

    int iCorner = 0, iCornerTemp = 0;

    CvSize pyr_sz = cvSize( imgA->width+8, imgB->height/3 );

    IplImage* pyrA = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );

IplImage* pyrB = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );

CvPoint2D32f* cornersB = new CvPoint2D32f[ MAX_CORNERS ];

cvCalcOpticalFlowPyrLK(

imgA,

imgB,

pyrA,

pyrB,

cornersA,

cornersB,

corner_count,

cvSize( win_size,win_size ),

5,

features_found,

feature_errors,

cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 ),

0

);

Thereafter, the features in image A and B are mapped into each one's corresponding point cloud. The function is carried out by findDepth() function. pointA in this function is 2D input and cloudPointA is 3D output. The constants in the function, widthCoef and heightCoef, are intrinsic parameters of the camera. The camera used here is Asus XtionPro live camera. These two parameters help to derive the actual horizontal and vertical locations based on depth information.

The value of depth is obtained through fractional interpolation. We first find four neighboring points with integer x and y values. Then interpolation is performed in horizontal direction and followed by vertical direction with the output to be depthFinal. There is another possible way for fractional interpolation. Interpolation in 2D can be written as z=a1+a2*x+a3*y+a4*x*y, which is bilinear interpolation formula. With four neighboring points, we can solve the equations and find a1/a2/a3/a4. Then the value of depthFinal can be obtained using bilinear formula.

// Find depth of a feature point found in 2d image

void findDepth(const float (*pointA)[2], float (*cloudPointA)[3], const int totalPoints, const pcl::PointCloud<pcl::PointXYZ> &cloudA)
{
// widthCoef and heightCoef are parameters coming from camera characterization
    const float widthCoef = 1.21905;
    const float heightCoef = 0.914286;
    int width = cloudA.width;
    int height = cloudA.height;
    int xIndex = 0, yIndex = 0, i;
    float xOffset, yOffset, depth00, depth01, depth10, depth11, depthX0, depthX1, depthFinal;
    bool printOn = false;
    for (i = 0; i < totalPoints; i++)
    {
        xIndex = (int) floor(pointA[i][0]);
        yIndex = (int) ceil(pointA[i][1]);
        xOffset = pointA[i][0] - floor(pointA[i][0]);
        yOffset = ceil(pointA[i][1]) - pointA[i][1];
        if (printOn)
        {
            std::cout << "xIndex,yIndex = " << xIndex << "," << yIndex <<std::endl;
            std::cout << "xOffset,yOffset = " << xOffset << "," << yOffset <<std::endl;
        }
        if ((cloudA.points[yIndex*width+xIndex].z > 0) && (cloudA.points[yIndex*width+xIndex].z < 10)) // To filter out z = NaN
            depth00 = cloudA.points[yIndex*width+xIndex].z;
        else
            depth00 = 0; // Make it equal to 0 maybe is not the best way
        if ((cloudA.points[yIndex*width+xIndex+1].z > 0) && (cloudA.points[yIndex*width+xIndex+1].z < 10))
            depth01 = cloudA.points[yIndex*width+xIndex+1].z;
        else
            depth01 = 0;
        if ((cloudA.points[(yIndex-1)*width+xIndex].z > 0) && (cloudA.points[(yIndex-1)*width+xIndex].z < 10))
            depth10 = cloudA.points[(yIndex-1)*width+xIndex].z;
        else
            depth10 = 0;
        if ((cloudA.points[(yIndex-1)*width+xIndex+1].z > 0) && (cloudA.points[(yIndex-1)*width+xIndex+1].z < 10))
            depth11 = cloudA.points[(yIndex-1)*width+xIndex+1].z;
        else
            depth11 = 0;
        // 2D linear interpolation
        depthX0 = (1-xOffset)*depth00 + xOffset*depth01;
        depthX1 = (1-xOffset)*depth10 + xOffset*depth11;
        depthFinal = (1-yOffset)*depthX0 + yOffset*depthX1;
        cloudPointA[i][2] = depthFinal;
        // Calculate x and y based on depth
        cloudPointA[i][0] = depthFinal*(pointA[i][0]-width/2)/width*widthCoef;
        cloudPointA[i][1] = depthFinal*(pointA[i][1]-height/2)/height*heightCoef;
        if (printOn)
        {
            std::cout << "point[" << i << "] " << pointA[i][0] << "," << pointA[i][1] << std::endl;
            std::cout << "cloudPoint[" << i << "] " << cloudPointA[i][0] << "," << cloudPointA[i][1] << "," << cloudPointA[i][2] << std::endl;
        }
    }
}

With 3D feature points from two clouds, we can find their linear transform. This function is done by findTransform(). The core is SVD computation. R_eigen is the unitary rotation between these two clouds and t_vec is the offset between these two clouds.

            // svd

            Eigen::JacobiSVD<Eigen::Matrix3f> svd (m, Eigen::ComputeFullU | Eigen::ComputeFullV);

            Eigen::Matrix3f u = svd.matrixU ();

            Eigen::Matrix3f v = svd.matrixV ();

            Eigen::Matrix3f R_eigen = v * u.transpose ();

            for (int j = 0; j < 3; j++)

            {

                for (int k = 0; k < 3; k++)

                {

                    R[j][k] = R_eigen(j,k);

                    RtMat[3*i+j][k] = R[j][k];

                }

                t_vec[j] = tgtMean[j] - R[j][0]*srcMean[0] - R[j][1]*srcMean[1] - R[j][2]*srcMean[2];

                RtMat[3*i+j][3] = t_vec[j];

            }

Multiple point clouds can be combined pair after pair in this way. Finally, we use function in PCL libary to convert the combined point cloud to a mesh network stored in a .ply file. This conversion is implemented in plygen() function.

A Tip for Installing Point Cloud Library 1.7.2 + Visual Studio 2012 in X64 Machine

Point Cloud Library (PCL) is a convenient SW library for 3D point cloud data processing. It is like OpenCV for 3D data. However, in recent time, installing PCL library for Windows becomes less straightforward. The current all-in-one windows builds in PCL library main webpage (http://pointclouds.org/downloads/windows.html) only support the obsolete versions of Microsoft Visual Studio 2010 and 2008. These two Visual Studio releases can't be found from Microsoft webpages any more.

Thanks to the work of unanancyowen.com (http://unanancyowen.com/?p=1255&lang=en), all-in-one builds of PCL 1.7.2 with Visual Studio 2012/2013/2015 are provided. However, after I downloaded the build for Visual Studio 2012 + X64 and installed it, Cmake of my PCL project complained that VTK library required by PCL can't be found. In case that you met with the similar issue, PCLConfig.cmake file in C:\Program Files\PCL 1.7.2\cmake directory needs to be modified to solve this issue. The modification is like below:

macro(find_VTK)

  if(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID)

    #set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/vtk-5.8")

    set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/cmake/vtk-6.2")

  elseif(NOT VTK_DIR AND NOT ANDROID)

    set(VTK_DIR "C:/Program Files/VTK/lib/cmake/vtk-6.2")

  endif(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID)

The original PCL cmake points to VTK version 5.8. But since the installed VTK build is actually version 6.2, in order to find valid cmake file for VTK, PCLConfig.cmake needs to point to the new location: "C:\Program Files\PCL 1.7.2\3rdParty\VTK\lib\cmake\vtk-6.2". After making this change, Cmake for my PCL projects starts to work.