Arduino Bluetooth Module Test Run

In this blog, we introduce how to make a test run for a Bluetooth module under Arduino. Bluetooth is a wireless standard which pairs two or multiple devices. It is usually used in indoor and short distance scenarios. The Bluetooth device used here is Virtuabotix Bluetooth to serial slave (BT2S-SLAVE). As shown below, this Bluetooth module has four pins: VCC, GND, TXD and RXD. The purposes of these pins are straightforward. VCC and GND are used for power supply. You can connect VCC pin to 5V voltage connection in Arduino board and GND pin to ground. All connectivity devices need to transmit and receive information. TXD pin is for transmitting while RXD pin is for receiving. After VCC and GND pins are properly connected, a red LED on the device starts to blink.

Even without writing any Arduino Bluetooth code, we can still make a test run. The way is to short connect the TXD and RXD pins (as shown below with the red-colored wire). Therefore, the signal received in RXD will be looped back to TXD and sent out. When communicating with this device, one should see a "mirror". Whatever you send out to the device, it will play back exactly the same content to you.

After the Bluetooth device powered up, you can now talk to this device from a computer and the computer should also have Bluetooth connection. The first step is to pair the computer with the Bluetooth device, which is common procedure if the computer wants to talk to any Blueooth device. Next, you can open Arduino SW. After setting the right board and port, serial monitor can be launched. If all settings are correct, the moment that serial monitor is launch, the LED light in the Bluetooth device should stop blinking and turn solid red. If you see that, that is a good sign. Then you can start to type the message and you can see the identical message sent back to you. In my case, I send "hello" and get back "hello". If you see that, then congratulations! Your test run is successful.

Projecting 3D Point to Screen in Android OpenGL ES 2.0/3.0 Engine (Rajawali)

Project a 3D point to screen is a common task in OpenGL-alike platform. It will go through two matrices: pose matrix and projection matrix. In Rajawali, this function can be implemented as the follows:

        poseMatrix.toArray(valuePos);
        double[] valueProj = new double[16];
        projectionMatrix.toArray(valueProj);
        outputV2 = new Vector3(0, 0, 0);


        // To follow the example of gluProject
        double[] temp = new double[8];
        temp[0] = valuePos[0]*inputV.x+valuePos[4]*inputV.y+valuePos[8]*inputV.z+valuePos[12];
        temp[1] = valuePos[1]*inputV.x+valuePos[5]*inputV.y+valuePos[9]*inputV.z+valuePos[13];
        temp[2] = valuePos[2]*inputV.x+valuePos[6]*inputV.y+valuePos[10]*inputV.z+valuePos[14];
        temp[3] = valuePos[3]*inputV.x+valuePos[7]*inputV.y+valuePos[11]*inputV.z+valuePos[15];

        temp[4] = valueProj[0]*temp[0]+valueProj[4]*temp[1]+valueProj[8]*temp[2]+valueProj[12]*temp[3];
        temp[5] = valueProj[1]*temp[0]+valueProj[5]*temp[1]+valueProj[9]*temp[2]+valueProj[13]*temp[3];
        temp[6] = valueProj[2]*temp[0]+valueProj[6]*temp[1]+valueProj[10]*temp[2]+valueProj[14]*temp[3];
        temp[7] = -temp[2]; // The row of ProjectionMatrix is 0,0,-1,0

        if(temp[7]==0.0) //The w value
        {
            outputV2.x = 0;
            outputV2.y = 0;
        }

        temp[7]=1.0/temp[7];
        //Perspective division
        temp[4]*=temp[7];
        temp[5]*=temp[7];
        temp[6]*=temp[7];
        outputV2.x = temp[4];
        outputV2.y = temp[5];

Pose matrix can be obtained as

poseMatrix = getCurrentCamera().getViewMatrix();

Projection matrix can be obtained as

projectionMatrix = ScenePoseCalculator.calculateProjectionMatrix(

                intrinsics.width, intrinsics.height,

                intrinsics.fx, intrinsics.fy, intrinsics.cx, intrinsics.cy);

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.

To Estimate the Transformation Between Two Images

The transformation of a 3D object from one point cloud to another point cloud is characterized by a unitary matrix and a offset. For 2D image, if multiple images are for the same scene, then we are facing with the similar task of matching between different images. Assuming that by feature point matching or tracking or other algorithms, it is found that N matching pairs of points \((x, y)\) and \((x', y')\) in two images. Our task is to identify a transformation for \((x, y) \rightarrow (x', y')\).


For 3D points under similar scenario, given two matrices with the size of \(3 \times N\) in each, we find the cross product of them, run SVD on the cross product, then the transform matrix is \(U \times V\) whereas \(U\) and \(V\) are the two unitary matrices obtained by SVD. For 2D matching, the procedure is more complicated. The first step is to add one dimension. Instead of  \((x, y)\) and \((x', y')\), now we have  \((x, y, 1)\) and \((x', y', 1)\). This added dimension can be treated as a dimension for the infinite horizon. This new triplet is insensitive to scaling, which means that \((x, y, 1)\) and \((kx, ky, k)\) represent the same pixel in the image. The cross product between these two triplets are \({\bf 0}\):
\[ \begin{bmatrix}
    i   &    j & k  \\
    x    &   y & 1  \\
    kx     &  ky & k  \\
\end{bmatrix}={\bf 0}
\]
The transformation between two images is defined as the projective transformation matrix \({\bf P}\) which is
\[

\begin{bmatrix}   kx'   \\
   ky'   \\
    k
\end{bmatrix} = {\bf P} \begin{bmatrix}
   x   \\
   y   \\
    1
\end{bmatrix}
\]
Assuming that
\[ {\bf P} = \begin{bmatrix}
    h_{1}   &    h_{2}&  h_{3}  \\
    h_{4}    &   h_{5}& h_{6}  \\
    h_{7}     &  h_{8}& h_{9}  \\
\end{bmatrix}\
\]
Then
\[

\begin{bmatrix}
   kx'   \\
   ky'   \\
    k
\end{bmatrix} = \begin{bmatrix}
   h_{1}x+h_{2}y+h_{3}   \\
  h_{4}x+h_{5}y+h_{6}   \\
    h_{7}x+h_{8}y+h_{9}
\end{bmatrix}
\]
Using the property that the cross product between \((x', y', 1)\) and \((kx', ky', k)\) is 0, we have
\[ \begin{bmatrix}
    i   &    j & k  \\
    h_{1}x+h_{2}y+h_{3}    &   h_{4}x+h_{5}y+h_{6} & h_{7}x+h_{8}y+h_{9}  \\
    x'     &  y' & 1  \\
\end{bmatrix}={\bf 0}
\]
This gives us three equations but only two of them are linear independent. The reason is that given
\[ \begin{bmatrix}
    i   &    j & k  \\
    a    &   b & c  \\
    u     &  v & w  \\
\end{bmatrix}={\bf 0}
\]
If \(aw-cu=0\) and \(bw-cv=0\), then \(av-bu=0\) for sure.

With two linearly independent equations, we have
\[ \begin{bmatrix}
    0    &   0 & 0 & x & y & 1 & -xy' & -yy' & -y'  \\
    x    &  y & 1 & 0 & 0 & 0 & -x'x & -x'y & -x' \\
\end{bmatrix}
\begin{bmatrix}
    h_{1} \\
   h_{2} \\
   h_{3} \\
   h_{4} \\
h_{5} \\
h_{6} \\
h_{7} \\
h_{8} \\
h_{9}
\end{bmatrix}
={\bf 0}
\]


With N pairs of correspondences, we can construct a matrix \({\bf M}\) which has 2N lines
\[ {\bf M}=\begin{bmatrix}
    0    &   0 & 0 & x_{1} & y_{1} & 1 & -x_{1}y_{1}' & -y_{1}y_{1}' & -y_{1}'  \\
    x_{1}    &  y_{1} & 1 & 0 & 0 & 0 & -x_{1}'x_{1} & -x_{1}'y_{1} & -x_{1}' \\
...\\
 0    &   0 & 0 & x_{N} & y_{N} & 1 & -x_{N}y_{N}' & -y_{N}y_{N}' & -y_{N}'  \\
    x_{N}    &  y_{N} & 1 & 0 & 0 & 0 & -x_{N}'x_{N} & -x_{N}'y_{N} & -x_{N}' \\
\end{bmatrix}
\]
The last step is SVD \({\bf USV = M}\). Then the last column of the matrix \({\bf V}\) corresponds to elements in matrix \({\bf P}\) since the last column of \({\bf V}\) is for the smallest Eigen value. It is the closest solution we can find for making
\[ {\bf M}
\begin{bmatrix}
    h_{1} \\
   h_{2} \\
   h_{3} \\
   h_{4} \\
h_{5} \\
h_{6} \\
h_{7} \\
h_{8} \\
h_{9}
\end{bmatrix}
={\bf 0}
\]

Sample Matlab source code for the whole procedure is shown below:
clear all; close all;
% to generate (x, y, 1)
srcPos = [-1483         853        4172        2572       -1196       -4241         308        4340         688       -4881
    3308         497       -2142        2537         678       -4460        2792       -3701        -306       -1629];


[M, N] = size(srcPos);
srcPos = [srcPos.' ones(N, 1)].';
PM = [1 2 0
    0 1 0
    -0.01 0.01 1];
dstTemp = PM * srcPos;
dstPos(1,:) = dstTemp(1,:) .* (1 ./ dstTemp(3,:));
dstPos(2,:) = dstTemp(2,:) .* (1 ./ dstTemp(3,:));
dstPos(3,:) = dstTemp(3,:) .* (1 ./ dstTemp(3,:));


% (x', y', 1)
dstPos = round(dstPos);
M = zeros(2*N, 9);
for i = 1:N
    M(2*i-1,:) = [0 0 0 srcPos(1,i) srcPos(2,i) 1 -srcPos(1,i)*dstPos(2,i) -srcPos(2,i)*dstPos(2,i) -dstPos(2,i)];
    M(2*i,:) = [srcPos(1,i) srcPos(2,i) 1 0 0 0 -srcPos(1,i)*dstPos(1,i) -srcPos(2,i)*dstPos(1,i) -dstPos(1,i)];
end


% To estmate matrix P
[u, s, v]= svd(M);
h = v(:,9)/v(9,9)

PM_hat = [h(1) h(2) h(3)
    h(4) h(5) h(6)
    h(7) h(8) h(9)]


To estimate

\[ {\bf P} =  \begin{bmatrix}
    1 & 2 & 0 \\
0 & 1 & 0 \\
-0.01 & 0.01 & 1
\end{bmatrix}\]

the result of above code is
\[ {\bf \hat{P}} =  \begin{bmatrix}
    1.0060  &  2.0137 & -56.3469\\
    0.0014  &  1.0073 & -14.1412\\
   -0.0100  &  0.0100   & 1.0000
\end{bmatrix}\]

The offsets of -56.3469 and -14.1412 are due to rounding error. If we change dstPos = round(dstPos) in the code to dstPos = (dstPos). Then \( {\bf \hat{P}}\) becomes

\[ {\bf \hat{P}} =  \begin{bmatrix}
        1.0000  &  2.0000 &   0.0000\\
   -0.0000  &  1.0000  &  0.0000\\
   -0.0100  &  0.0100  &  1.0000
\end{bmatrix}\]

Using least square estimation here does not yield good results since we don't know \(k\) in 

\[

\begin{bmatrix}   kx'   \\
   ky'   \\
    k
\end{bmatrix} = {\bf P} \begin{bmatrix}
   x   \\
   y   \\
    1
\end{bmatrix}
\]


(Added on 08/13/2016) Another way to solve this equation is this: since \({h_{1},...,h_{9}}\) parameters have ambiguity in scaling, we can force \({h_{9}=1}\). Then the equation can be rewritten as:
\[ {\bf M'}
\begin{bmatrix}
    h_{1} \\
   h_{2} \\
   h_{3} \\
   h_{4} \\
h_{5} \\
h_{6} \\
h_{7} \\
h_{8}
\end{bmatrix}
=\begin{bmatrix}
    y_{1}'  \\
    x_{1}' \\
...\\
 y_{N}'  \\
    x_{N}' \\
\end{bmatrix}
\]
where
\[ {\bf M'}=\begin{bmatrix}
    0    &   0 & 0 & x_{1} & y_{1} & 1 & -x_{1}y_{1}' & -y_{1}y_{1}'  \\
    x_{1}    &  y_{1} & 1 & 0 & 0 & 0 & -x_{1}'x_{1} & -x_{1}'y_{1} \\
...\\
 0    &   0 & 0 & x_{N} & y_{N} & 1 & -x_{N}y_{N}' & -y_{N}y_{N}' \\
    x_{N}    &  y_{N} & 1 & 0 & 0 & 0 & -x_{N}'x_{N} & -x_{N}'y_{N} \\
\end{bmatrix}
\]

Equation in this form can be solved by least square method.

A (Very) Useful Formula for Matrix Inversion

Here is a very useful formula for matrix inverse, I think the first time I saw it is from Prof. John Cioffi's book.

\[(A+BD^{-1}C)^{-1}=A^{-1}-A^{-1}B(D+CA^{-1}B)^{-1}CA^{-1}\]

let me give one example of how it can be used. Assuming that \(R_{uu}=h_{2}h_{2}^{h}+\lambda I\) where \(h_{2}\) is a vector. Then we have

\[R_{uu}^{-1}=\lambda^{-1}I-\frac{1}{\lambda^{2}+\lambda h_{2}^{h}h_{2}}h_{2}h_{2}^{h}\]

By using this formula, the end result turns out to be much simpler than where it starts from.

Implementation in Arduino Servo Libary for Rotation with Less Than 1 Degree Granularity

Arduino is popular open-source software/hardware platform for DIY. Using Arduino platform, one can control all kinds of device such as sensor, servo, LCD etc. Servo is a motor with feedback loop for rotation measurement. The feedback loop helps the servo to achieve precise control. If you tell a servo to rotate 90 degrees, it will do that. This makes servo an essential part for projects like robots. However, the smallest rotation the existing Arduino servo library can achieve is 1 degree. The reason is that the existing library only allows one to program integer number of degrees for rotation. Therefore, if one want to program servo for rotation less than 1 degree to achieve more smooth transition, the library has to be modified but the modification is rather straightforward. This post gives an example of how to modify the library.

The existing servo rotation function looks like this: an integer value is used as the input. First, the function will make sure that this integer is between 0 and 180 since that is the scope of the servo rotation. Then this integer will be mapped into a value between SERVO_MIN() and SERVO_MAX(). Here is how the map() function work: assuming that y = map(x, 0, 180, SERVO_MIN, SERVO_MAX), then (x - 0)/(180 - x) = (y - SERVO_MIN)/(SERVO_MAX - y). This y will be used to drive the servo operation. Below is the existing function in the Arduino servo library:

void Servo::write(int value)
{
  if(value < MIN_PULSE_WIDTH)
  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
    if(value < 0) value = 0;
    if(value > 180) value = 180;
    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());
  }
  this->writeMicroseconds(value);
}


To make sub-degree rotation possible, we create a new function with floating-point number as input.

// To write angle in floating value /Nan Zhang
void Servo::writeFloat(float value)
{
  if(value < MIN_PULSE_WIDTH)
  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
    if(value < 0.0) value = 0.0;
    if(value > 180.0) value = 180.0;
    value = map((int) (value*10), 0, 1800, SERVO_MIN(),  SERVO_MAX());
  }
  this->writeMicroseconds(value);
}

In this new function, we will first check whether this input is between 0.0 and 180.0. After that, the rounded value of (input*10) will be injected into the map function as map((int) (value*10), 0, 1800, SERVO_MIN(),  SERVO_MAX()). Essentially, this makes the smallest achievable rotation to be 0.1 degree. You may want to further increase the granularity by doing things like map((int) (value*100), 0, 18000, SERVO_MIN(),  SERVO_MAX()). But you will face physical limitation of how precise the servo can operate. For example, the default value for MAX and MIN are:

#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo

That means the best the device can do is (180)/(2400-544)=0.097 degree. Therefore, increasing granularity beyond 0.1 degree does not make sense for this servo device.

ICP Algorithm for Point Cloud Stitching

ICP stands for Iterative Closest Point algorithm. It is a well-known algorithm used to align two point clouds.

Iterative Closest Point

1. Search for correspondences.
2. Reject bad correspondences.
3. Estimate a transformation using the good correspondences.
4. Iterate.

Many tutorials are available online for ICP algorithm such as http://www.cs.technion.ac.il/~cs236329/tutorials/ICP.pdf

The popular PCL library for point cloud processing provides ICP class here: http://pointclouds.org/documentation/tutorials/iterative_closest_point.php

Here is another example of Matlab implementation for ICP: http://www.csse.uwa.edu.au/~ajmal/code/icp.m

Despite the popularity of ICP, when using it, I find that the result is not always satisfactory. The biggest problem is that there is no guarantee that ICP will converge to global optimum. In practice, it often converges to local optimum. As shown in the picture below, when point clouds A and B merge, depending on the starting positions, it is possible that ICP only merges part of the clouds.

Use Rodrigues' Rotation to Convert 3D data to 2D

In this blog, we introduce how Rodtrigues' rotation formula can be use to convert 3D data to 2D. Let us assume that we have a bunch of 3D points, and we want to project them to x/y axis plane for easier analysis. The Matlab code below implements this function.

clear all; close all;

% collect 3D points 
xyz = [-0.485 0.130 1.489
    0.008 0.134 1.464
    -0.008 0.425 1.458
    -0.465 0.414 1.470
    -0.489 0.136 1.483
    -0.465 0.412 1.470
    0.006 0.137 1.464];

% Model them as a plane in 3D using least square method
[h,l]=size(xyz);
coef = inv(xyz'*xyz)*(xyz'*ones(h,1));
coef_unit = (coef/sqrt(coef'*coef)).';

% Rodrigues' rotation formula
in0 = coef_unit;
in1 = [0 0 1];
% Reference
in0Norm = in0 ./ sqrt(in0*in0.');
in1Norm = in1 ./ sqrt(in1*in1.');
axisRaw = cross(in0Norm, in1Norm);
axis = axisRaw./sqrt(axisRaw*axisRaw.');
angle = acos(dot(in0Norm, in1Norm));
M = [axis(1)^2+(1-axis(1)^2)*cos(angle) axis(1)*axis(2)*(1-cos(angle))-axis(3)*sin(angle) axis(1)*axis(3)*(1-cos(angle))+axis(2)*sin(angle);
axis(1)*axis(2)*(1-cos(angle))+axis(3)*sin(angle) axis(2)^2+(1-axis(2)^2)*cos(angle) axis(2)*axis(3)*(1-cos(angle))-axis(1)*sin(angle);
axis(1)*axis(3)*(1-cos(angle))-axis(2)*sin(angle) axis(2)*axis(3)*(1-cos(angle))+axis(1)*sin(angle) axis(3)^2+(1-axis(3)^2)*cos(angle)];
    
% Project 3D points to 2D plane with z=0 
xyz_ortho = xyz - xyz*coef_unit.'*coef_unit;   
xyz_prj = M*xyz_ortho.';

Let us walk through the code step-by-step.

Step 1: A plane in 3D space can be characterized as \( a x + b y + c z = 1 \). Assuming that matrix \(X\) has all 3D points: \[ X = \begin{bmatrix}
    x_{0}       & y_{0} & z_{0}  \\
    x_{1}       & y_{1} & z_{1} \\
    ...              & ...        &...\\
    x_{N}       & y_{N} & z_{N}
\end{bmatrix}\]
Then least square estimator for [a b c] is \( coef = (X'X)^{-1}(X'h)\) where \(h = [1 1 ... 1]'\) and \(X'\) is the transpose of \(X\). Normalized version of coef is called coef_unit.

Step 2: Use Rodrigues' rotation formula to find the matrix \(M\) which can rotate coef_unit to [0 0 1].

Step 3: First we project all 3D points to a plane characterized by [x y z]*coef_unit = 0. This is achieved by:
xyz_ortho = xyz - xyz*coef_unit.'*coef_unit;   
The last step is to use rotation matrix \(M\) to reduce the dimension of xyz_ortho to 2D. After the rotation, all z-axis elements in xyz_prj are 0's.

${<br>}_{}$

Vector Kalman Filter

Kalman filter is one type of statistical filtering. It was invented in 1970's and has been widely used till now. There are different variations of Kalman. We focus on vector Kalman filter here.

For system model, we have two equations:
\[s[n]=\textbf{A}s[n-1]+\textbf{B}u[n] \qquad (1)\]
\[x[n]=\textbf{H}[n]s[n]+w[n] \qquad (2)\] 

\(s[n], x[n], u[n], w[n]\) are vectors; \(\textbf{A}, \textbf{B}, \textbf{H}[n]\) are matrices. \(s[n]\) is the signal and what we want to detect. Equation (1) shows that the signal in time n and time n-1 are correlated. \(s[n]\) is generated by \(s[n-1]\) plus a noise \(u[n]\). \(u[n]\) is white noise with \(N(0, \textbf{Q})\). But equation (1) alone does not give a good estimate of \(s[n]\). As time goes by, \(u[n]\) keeps adding noise to the estimate and eventually will render it unusable. That is where equation (2) comes into the picture. \(x[n]\) in equation (2) is observation. In each moment, the observation of \(x[n]\) is used to refine the estimate of \(s[n]\). \(x[n]\) is also corruptedby noise \(w[n]\), which has spectral density of \(N(0,\textbf{C}[n])\).

Now the problem comes: how should we utilize the observation? If we just have equation (2) but not equation (1), then MMSE or LS estimators are natural choices. But with equation (1), these estimators will be sub-optimal since they fail to consider the previous results of s. Kalman filter was invented to fully utilize the information given in both equations. Under Kalman filter, the estimate of s, s_hat, is derived from equation (1) but then corrected by the observation in equation (2).

The first step of Kalman filter is prediction:
\[\hat{s}[n|n-1]=\textbf{A}\hat{s}[n-1|n-1]\qquad(3)\]

The new MSE of \(\hat{s}[n|n-1]\) becomes
\[\textbf{M}[n|n-1]=\textbf{AM}[n-1|n-1]\textbf{A}'+\textbf{BQB}'\qquad(4)\]

where \(\textbf{A}\)' is the transpose of conjugate of the matrix \(\textbf{A}\). Equation (4) shows that without correction, MSE will keep growing due to \(\textbf{BQB}'\).

For correlation, we uses MMSE filter, which is the best known filter to reduce Mean Square Error. The formula for MMSE filter is
\[\textbf{K}[n]=E(x[n]s[n]')(E(x[n]x[n]')^{-1}\qquad(5)\]

where \((.)^{-1}\) means matrix inverse. Thus
\[\textbf{K}[n]=\textbf{M}[n|n-1]\textbf{H}[n]'(\textbf{H}[n]\textbf{M}[n|n-1]\textbf{H}[n]'+\textbf{C}[n])^{-1}\qquad(6)\]

Then the correction formula for Kalman becomes
\[\hat{s}[n|n]=\hat{s}[n|n-1]+\textbf{K}[n](x[n]-\textbf{H}[n]\hat{s}[n|n-1])\qquad(7)\]

\(x[n]\) is supposed to be a good estimation of \(\textbf{H}[n]\hat{s}[n|n-1]\). The difference between these two are used to correct \(\hat{s}\).

Correction should reduce MSE. The new MSE is
\[E\big((s[n]-\textbf{K}[n]x[n])(s[n]-\textbf{K}[n]x[n])'\big)\]

Based on the orthogonality rule of MMSE filter,
\[E((s[n]-\textbf{K}[n]x[n])x[n]')=0\]

Therefore, MSE can be modified to be

E((s[n]-K[n]x[n])(s[n]-K[n]x[n])')

=E((s[n]-K[n]x[n])s[n]')

=E(s[n]s[n]')-K[n]E(x[n]s[n]')

= (I-K[n]H[n])M[n|n-1]

=M[n|n]

I don't want to drive people to sleep but this is how things are. Kalman filtering is not easy. To use Kalman, you need to come out with the models of A, B, C, Q, H. Depending on the size of s and x, you may also need to inverse a large matrix. 

Steven Kay's classic book: Fundamentals of statistical signal processing -- estimation theory, has one chapter dedicated to Kalman filter. You can refer to that book for more knowledge of Kalman filtering.

Deep Dive Into PulsedLight LIDAR-Lite V2 Device

LIDAR-Lite V2 Device is produced by http://pulsedlight3d.com/  This excellent device uses laser to measure the distance. The max distance is 40m. You can use it for both indoor and outdoor. And it is quite convenient to hook it up with open-source platform like Arduino. As the name suggested, its main usage is for LIDAR sensor. You can mount it to a robot or a drone to collect surrounding information. But people also use it for other purposes like generating point cloud.

This is what we see when open the device. Inside it has two parts: the upper part is the laser diode, which is the transmitter. The lower part is receiver, which can sense the laser light. It works like radar: the upper part sends a laser beam. The beam is reflected by object and the lower part detects the reflection. By correlating the signal sent and signal received, it can identify the peak of correlation and derive the distance based on the peak location. When the object moves farther, the reflection peak will be detected later. 

The SW library of LIDAR Lite allows to retrieve the correlation values. For each correlation, it has 256 elements. The plot below includes correlation of a few distance measurements. "corr50" curve has the max distance of 4.82m (482cm). Clearly the correlation peak occurs later for larger distance.

Quaternion-Based Rotation in OpenGL

This is yet another blog about quaternion. Assuming that we have quaternion [q0, q1, q2, q3]. Can we directly use it to rotation object in OpenGL? The answer is yes. At first, let us normalize this quaternion (written with Java)

     norm = (float) Math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

    if (norm == 0)

    {

    return 0;

    }

    else

    {

    q0 = q0 / norm;

    q1 = q1 / norm;

    q2 = q2 / norm;

    q3 = q3 / norm;

    }

Then, glRotatef function in OpenGL is a natural fit for rotation using quaternion.

 

        // Rotate the object according to quaternion

        theta = (float) (Math.acos(q0) * 2);

        

        aNorm = (float) Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3);

        if (aNorm != 0)

        {

        gl.glRotatef(theta*180f/PI, q1/aNorm, q2/aNorm, q3/aNorm);

        }

        else

        {

        gl.glRotatef(theta*180f/PI, q1, q2, q3);

        }

Note that except for quaternion, glRotatef can also be used for rotation along x, y or z axis. In that way, glRotatef needs to be called three times instead of once with quaternion-based method.

    glRotatef(xRot , 1.0, 0.0, 0.0);

    glRotatef(yRot , 0.0, 1.0, 0.0);

    glRotatef(zRot , 0.0, 0.0, 1.0);

How To Find A Point's Screen Correspondence in OpenGL

To project a point, we can use gluProject function. What the function does is to project a 3D coordinate to the window screen.

GLfloat x,y,z;
double wx,wy,wz,pixel_x,pixel_y;
double modelMatrix[16];
double projMatrix[16];
GLint viewport[4];

// To find the projection
glGetDoublev(GL_MODELVIEW_MATRIX,modelMatrix);
glGetDoublev(GL_PROJECTION_MATRIX,projMatrix);
glGetIntegerv(GL_VIEWPORT,viewport); 
gluProject(x, y, z, modelMatrix, projMatrix, viewport, &wx, &wy, &wz); 

If we want to compare the screen coordinate of the point with the location of the mouse, a little bit extra processing is needed before comparing with event->x()/y(), which are mouse locations.

pixel_x = wx;
pixel_y = viewport[1] + viewport[3] - wy;
dist = (pixel_x - event->x()) * (pixel_x - event->x()) + (pixel_y - event->y()) * (pixel_y - event->y());

How To Zoom In in OpenGL + Qt

Recently I am working on a project which utilizes Qt5.3+OpenGL. I need to implement a function in which by spinning the mouse wheel, the 3D object in OpenGL view can be enlarged or shrunk. After some research, it is found that we can implement the function in two ways: glScaled or  glOrthof.

Approach 1: glScaled

glScaled function scales the x, y, z by a factor. In my implementation, all three axis are scaled by the same factor.

First, the mouse wheel function is written in such a way that wheel rotation changes a zoom factor named zoomScale. After that, it will call the paintGL() function where galScaled is embedded.

void GLWidget::wheelEvent(QWheelEvent *event)
{
QPoint numDegrees = event->angleDelta();
if (numDegrees.y() < 0)  zoomScale = zoomScale/1.1;
if (numDegrees.y() > 0)  zoomScale = zoomScale*1.1;

updateGL(); // call paintGL()
}

In paintGL() function, galScaled() is called and zoomScale is used to scale x, y, and z axis. We should note that in order to avoid unnecessary clipping, the near and far planes of OpenGL view need to be set with some redundancy.

void GLWidget::paintGL()

{

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glLoadIdentity();

    glScaled(zoomScale, zoomScale, zoomScale);

    glRotatef(xRot / 16.0, 1.0, 0.0, 0.0);

    glRotatef(yRot / 16.0, 0.0, 1.0, 0.0);

    glRotatef(zRot / 16.0, 0.0, 0.0, 1.0);

Approach 2: glOrthof

glOrthof function defines a box where the objects are shown in OpenGL. The box has left, right, up, down, near, far boundaries. Again, we starts from the wheelEvent function which calls resizeGL().

void GLWidget::wheelEvent(QWheelEvent *event)

{

QPoint numDegrees = event->angleDelta();

if (numDegrees.y() < 0)  zoomScale = zoomScale/1.1;

if (numDegrees.y() > 0)  zoomScale = zoomScale*1.1;

GLint viewport[4];

glGetIntegerv(GL_VIEWPORT,viewport); 

resizeGL(viewport[2], viewport[3]);

updateGL(); // call paintGL()

}

In resizeGL, zoomScale is used to adjust the boundaries in x and y directions. In this way, we can also implement the zoom in/out functions.

void GLWidget::resizeGL(int width, int height)

{

    int side = qMin(width, height);

int range = 1000;

    //glViewport((width - side) / 2, (height - side) / 2, side, side);

glViewport(0, 0, width, height);

    glMatrixMode(GL_PROJECTION);

    glLoadIdentity();

    glOrthof(-0.5*range*zoomScale, +0.5*range*zoomScale, -0.5*height/width*range*zoomScale, +0.5*height/width*range*zoomScale, -5*range, +5*range);

    glMatrixMode(GL_MODELVIEW);

}