Saturday, July 30, 2016

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}\]

Sunday, July 17, 2016

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.

Monday, June 6, 2016

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.

Saturday, June 4, 2016

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.




Wednesday, May 25, 2016

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.