Sunday, May 7, 2017

Resizable Window for Pygame + OpenGL

Pygame + OpenGL provides a good platform for implementing OpenGL in Python language. It is straightforward to make the OpenGL window resizable. In the code below, we first detect the event of pygame video size change. When this event occurs, the display is reset with the width and height of the new window. The OpenGL perspective is also changed accordingly.

            if event.type == pygame.VIDEORESIZE:
                # For window size change
                surface = pygame.display.set_mode((event.w, event.h), DOUBLEBUF|OPENGL|RESIZABLE)
                gluPerspective(45, (1.0*event.w/event.h), 0.1, 50.0)
                glTranslatef(0.0,0.0, -5)

A sample Python program is listed below which shows a cube in OpenGL window which can be resized:


import pygame
from pygame.locals import *
import math

from OpenGL.GL import *
from OpenGL.GLU import *

verticies = (
    (1, -1, -1),
    (1, 1, -1),
    (-1, 1, -1),
    (-1, -1, -1),
    (1, -1, 1),
    (1, 1, 1),
    (-1, -1, 1),
    (-1, 1, 1)
    )

edges = (
    (0,1),
    (0,3),
    (0,4),
    (2,1),
    (2,3),
    (2,7),
    (6,3),
    (6,4),
    (6,7),
    (5,1),
    (5,4),
    (5,7)
    )


def Cube():
    glBegin(GL_LINES)
    for edge in edges:
        for vertex in edge:
            glVertex3fv(verticies[vertex])
    glEnd()
    #print(repr(verticies[0][0])+','+repr(verticies[0][1]));
 
    glBegin( GL_POINTS );
    glColor3f(1,1,1); 
    for i in range(0,8):
         glVertex3f(verticies[i][0], verticies[i][1], verticies[i][2]);
    glEnd();


def main():
    pygame.init()
 
    display = (1000,750)
    surface = pygame.display.set_mode(display, DOUBLEBUF|OPENGL|RESIZABLE)
    gluPerspective(45, (1.0*display[0]/display[1]), 0.1, 50.0)
    glTranslatef(0.0,0.0, -5)

    while True:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                quit()
            if event.type == pygame.VIDEORESIZE:
                # For window size change
                surface = pygame.display.set_mode((event.w, event.h), DOUBLEBUF|OPENGL|RESIZABLE)
                gluPerspective(45, (1.0*event.w/event.h), 0.1, 50.0)
                glTranslatef(0.0,0.0, -5)

        glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
        Cube()

        pygame.display.flip()
        pygame.time.wait(10)


main()

Thursday, April 13, 2017

Using Mouse for Object Zoom In/Zoom Out/Rotation in Python + OpenGL

OpenGL is supported in multiple languages including Python. A common task performed in OpenGL is to zoom in, zoom out and rotate the 3D object. In this blog, we provide an example of how to carry out these tasks. We utilize a Python package name pyname in our code.

In our code, when one rolls up the wheel, the object will be zoomed in; when one rolls down the wheel, the object will be zoomed out. Zoom in/out is implemented by the function of glScaled. When the scale is larger than 1, the object will be enlarged; when the scale is smaller than 1, the object will be shrink. The code snippet for zoom function is:


    if event.type == pygame.MOUSEBUTTONDOWN and event.button == 4: # wheel rolled up
        glScaled(1.05, 1.05, 1.05);
    elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 5: # wheel rolled down
        glScaled(0.95, 0.95, 0.95);


Rotation function is designed in this way: when you move the mouse  to a direction while pressing the left button, the object rotates to that direction. Since the movement of mouse in the monitor is 2D, the first step is to calculate the mouse movement in the x-axis and y-axis directions. pygame allows to record the 2D location of the mouse. The delta between the current location and the previous location gives dx and dy, which indicates the horizontal and vertical directions of movement:
        x, y = event.pos;
        dx = x - lastPosX;
        dy = y - lastPosY;

As the next step of implementing rotation, model view matrix needs to be retrieved. Since the 3D object keeps rotating, model view matrix tells the current orientation.

            modelView = (GLfloat * 16)()
            mvm = glGetFloatv(GL_MODELVIEW_MATRIX, modelView)

The model view matrix is a 4x4 matrix. The first 3 rows by 3 columns are the rotation matrix which is also a unitary matrix. This rotation matrix, Q, tells the orientation of the object. In order to achieve the desired rotation effect, elements of the rotation matrix need to be multiplied with dx and dy. Assuming Q = [q0|q1|q2], since dy represents the rotation around x-axis and dx represents the rotation around y-axis, q0*dy+q1*dx is combined rotation vector. sqrt(dx^2+dy^2) is the magnitude of rotation. The code is as below:

            temp = (GLfloat * 3)();
            temp[0] = modelView[0]*dy + modelView[1]*dx;
            temp[1] = modelView[4]*dy + modelView[5]*dx;
            temp[2] = modelView[8]*dy + modelView[9]*dx;
            norm_xy = math.sqrt(temp[0]*temp[0] + temp[1]*temp[1] + temp[2]*temp[2]);
            glRotatef(math.sqrt(dx*dx+dy*dy), temp[0]/norm_xy, temp[1]/norm_xy, temp[2]/norm_xy);


For reference, an example Python program is provided below. To run this program, the user needs to install pygame Python package.

import pygame
from pygame.locals import *
import math

from OpenGL.GL import *
from OpenGL.GLU import *

lastPosX = 0;
lastPosY = 0;
zoomScale = 1.0;
dataL = 0;
xRot = 0;
yRot = 0;
zRot = 0;

verticies = (
    (1, -1, -1),
    (1, 1, -1),
    (-1, 1, -1),
    (-1, -1, -1),
    (1, -1, 1),
    (1, 1, 1),
    (-1, -1, 1),
    (-1, 1, 1)
    )

edges = (
    (0,1),
    (0,3),
    (0,4),
    (2,1),
    (2,3),
    (2,7),
    (6,3),
    (6,4),
    (6,7),
    (5,1),
    (5,4),
    (5,7)
    )


def Cube():
    glBegin(GL_LINES)
    for edge in edges:
        for vertex in edge:
            glVertex3fv(verticies[vertex])
    glEnd()
    #print(repr(verticies[0][0])+','+repr(verticies[0][1]));
 
    glBegin( GL_POINTS );
    glColor3f(1,1,1); 
    for i in range(0,8):
         glVertex3f(verticies[i][0], verticies[i][1], verticies[i][2]);
    glEnd();
 
def mouseMove(event):
    global lastPosX, lastPosY, zoomScale, xRot, yRot, zRot;
 
    if event.type == pygame.MOUSEBUTTONDOWN and event.button == 4: # wheel rolled up
        glScaled(1.05, 1.05, 1.05);
    elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 5: # wheel rolled down
        glScaled(0.95, 0.95, 0.95);
 
    if event.type == pygame.MOUSEMOTION:
        x, y = event.pos;
        dx = x - lastPosX;
        dy = y - lastPosY;
        
        mouseState = pygame.mouse.get_pressed();
        if mouseState[0]:

            modelView = (GLfloat * 16)()
            mvm = glGetFloatv(GL_MODELVIEW_MATRIX, modelView)
   
   # To combine x-axis and y-axis rotation
            temp = (GLfloat * 3)();
            temp[0] = modelView[0]*dy + modelView[1]*dx;
            temp[1] = modelView[4]*dy + modelView[5]*dx;
            temp[2] = modelView[8]*dy + modelView[9]*dx;
            norm_xy = math.sqrt(temp[0]*temp[0] + temp[1]*temp[1] + temp[2]*temp[2]);
            glRotatef(math.sqrt(dx*dx+dy*dy), temp[0]/norm_xy, temp[1]/norm_xy, temp[2]/norm_xy);

        lastPosX = x;
        lastPosY = y;
        


def main():
    pygame.init()
 
    display = (1000,750)
    pygame.display.set_mode(display, DOUBLEBUF|OPENGL, RESIZABLE)

    gluPerspective(45, (1.0*display[0]/display[1]), 0.1, 50.0)
    glTranslatef(0.0,0.0, -5)
 

    while True:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                quit()
            mouseMove(event);

        glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
        Cube()
        pygame.display.flip()
        pygame.time.wait(10)


main()

How to Run N Nearest Neighbor Search in Python using kdtree Package

Given S points scattered in a K-dimension space, N nearest neighbor search algorithm finds out for certain point, which N out of S points are its closest neighbors. To implement N nearest neighbor searching algorithm, a kd tree needs to be constructed for all these S points. Searching in the internet, the most popular way to set up kd tree in Python is to use the scipy package. However, I keeps failing on installing scipy package due to the lack of Lapack package. Lapack is a linear algorithm library. Instead, I find another way. Probably some other folks also have the issue of scipy installation. Therefore, let me what I learnt.

My solution is to install the kdtree package. The package can be found in https://github.com/stefankoegl/kdtree. It is pretty straightforward to use. Let me share my example code of N nearest neighbor search algorithm. All points here are in a 3D dimension with N = 2. The average distance to its nearest neighbors is printed out for each point.



import kdtree
import math
import numpy

data = ((1, 0, 0),(2, 0, 0), (2, 0, 0), (3, 0, 0));

# Set up kd tree
myTree = kdtree.create(dimensions=3)
for i in range(0, len(data)):
  tempData = data[i];
  myTree.add((float(tempData[0]), float(tempData[1]), float(tempData[2])));

# Nearest neighbor search
minDist = []
NN = 2; # Number of neighbors to search
for i in range(0, len(data)):
   searchResult = myTree.search_knn(data[i], NN+1); # Find two closest neighbors including itself
   sumDist = 0;
   for p in range(0, NN):
     sumDist = sumDist + math.sqrt(searchResult[p+1][1]);
   avgDist = sumDist/NN;
   print avgDist

Saturday, October 15, 2016

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.



Thursday, September 22, 2016

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);


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.