pcl

Simulating A Virtual Camera With OpenCV and Reverse Projection From 2D to 3D

Leave a Comment / C++, Tutorials / admin

Projection of Points from 3D in the camera plane: Computed rays from the camera origin in the direction of points:

void virtualCameraSimulator(int argc, char ** argv)
{
    int numberOfPixelInHeight,numberOfPixelInWidth;
    double heightOfSensor, widthOfSensor;
    double focalLength=0.5;
    double mx, my, U0, V0;
    numberOfPixelInHeight=600;
    numberOfPixelInWidth=600;

    heightOfSensor=10;
    widthOfSensor=10;

    my=(numberOfPixelInHeight)/heightOfSensor ;
    U0=(numberOfPixelInHeight)/2 ;

    mx=(numberOfPixelInWidth)/widthOfSensor;
    V0=(numberOfPixelInWidth)/2;



    cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) <<
    focalLength*mx, 0, V0,
    0,focalLength*my,U0,
    0,0,1);


    cv::Mat distortionCoefficient= (cv::Mat_<double>(5,1) <<0, 0, 0, 0, 0);

////////////////////////////////////////////setting up virtual camera extrinsic ///////////////////////////////////////////////////////
    cv::Mat cameraRotation=cv::Mat::eye(3,3, CV_64F);
    double roll,pitch,yaw;
    cv::Vec3d theta;
    roll=0 ;
    pitch=0;
    yaw=0;

    theta[0]=roll;
    theta[1]=pitch;
    theta[2]=yaw;

    cameraRotation =eulerAnglesToRotationMatrix(theta);

    double tx,ty,tz;
    tx=0;
    ty=0;
    tz=0;
    cv::Mat cameraTranslation = (cv::Mat_<double>(3,1) <<tx,ty,tz);

//////////////////////////////////////////projecting 3D points from world into virtual camera //////////////////////////////

    std::vector<cv::Point3d> objectpoints;
    std::vector<cv::Point2d> projectedPoints;

/*
Points are in the following form (OpenCV coordinate):

                  Z
                ▲
               /
              /
             /      1   2    X
            |------------ ⯈
           1|           .
           2|       .   .   .
           3|           .
           4|           .
            | Y
            ⯆
*/


    std::string pathToPointFile(argv[1]);

    io::CSVReader<3> in(pathToPointFile);
    in.read_header(io::ignore_extra_column, "x", "y", "z");
    double x,y,z;

    pcl::PointXYZ point;
    boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > pointsInCameraCloud(new pcl::PointCloud<pcl::PointXYZ>);

    while(in.read_row(x,y,z))
    {
        objectpoints.push_back(cv::Point3d(x,y,z));
        pointsInCameraCloud->push_back(pcl::PointXYZ(x,y,z));
    }
    cv::projectPoints(objectpoints, cameraRotation, cameraTranslation, cameraMatrix, distortionCoefficient, projectedPoints);

///////////////////////////////////////////////saving the image //////////////////////////////////////////

    double U,V;

    cv::Mat cameraImage=cv::Mat::zeros(numberOfPixelInHeight,numberOfPixelInWidth,CV_8UC1);
    cv::line(cameraImage,cv::Point2d(numberOfPixelInWidth/2,0),cv::Point2d(numberOfPixelInWidth/2,numberOfPixelInHeight),cv::Scalar(255,255,255));
    cv::line(cameraImage,cv::Point2d(0,numberOfPixelInHeight/2),cv::Point2d(numberOfPixelInWidth,numberOfPixelInHeight/2),cv::Scalar(255,255,255));
    for(std::size_t i=0;i<projectedPoints.size();i++)
    {
        V=int(projectedPoints.at(i).x);
        U=int(projectedPoints.at(i).y);
        //std::cout<<U <<"," <<V  <<std::endl;
        cameraImage.at<char>(int(U),int(V))=char(255);
    }


    std::string fileName=std::string("image_")+std::to_string(focalLength)+ std::string("_.jpg");
    cv::imwrite(fileName,cameraImage);


/////////////////////////////////////////// ray tracing////////////////////////////////////////////
    std::vector<cv::Point3d> directionRays;
    reverseProjection(distortionCoefficient , cameraMatrix, projectedPoints, directionRays);

/////////////////////////////////////////// 3D visualization////////////////////////////////////////////

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    int pointSize=5;
    double 	coordinateFrameScaleSize=1;
    std::string coordinateName;
    coordinateName="World";
    int viewport = 0;


    viewer->addCoordinateSystem(coordinateFrameScaleSize,Eigen::Affine3f::Identity(),coordinateName,viewport);
    double rayScale=5;
    double r,g,b;
    r=0.0;
    g=1.0;
    b=1.0;

    for(std::size_t i=0;i<directionRays.size();i++)
    {
        std::cout<< directionRays[i].x<<","<<directionRays[i].y<<","<<directionRays[i].z <<std::endl;
        viewer->addLine(pcl::PointXYZ(0 ,0 , 0), pcl::PointXYZ(rayScale*directionRays[i].x,rayScale*directionRays[i].y,rayScale*directionRays[i].z), r, g, b, std::to_string(i+100),viewport);
    }



    int Red,Blue,Green;

    Red=255;
    Green=0;
    Blue=0;
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ >  tgt_h ( pointsInCameraCloud,Red,Green,Blue   );
    viewer->addPointCloud<pcl::PointXYZ> (pointsInCameraCloud ,tgt_h ,  std::to_string(1), 0);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::to_string(1));


    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

void virtualCameraSimulator(int argc, char ** argv)

{

int numberOfPixelInHeight,numberOfPixelInWidth;

double heightOfSensor, widthOfSensor;

double focalLength=0.5;

double mx, my, U0, V0;

numberOfPixelInHeight=600;

numberOfPixelInWidth=600;

heightOfSensor=10;

widthOfSensor=10;

my=(numberOfPixelInHeight)/heightOfSensor ;

U0=(numberOfPixelInHeight)/2 ;

mx=(numberOfPixelInWidth)/widthOfSensor;

V0=(numberOfPixelInWidth)/2;

cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) <<

focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1);

cv::Mat distortionCoefficient= (cv::Mat_<double>(5,1) <<0, 0, 0, 0, 0);

////////////////////////////////////////////setting up virtual camera extrinsic ///////////////////////////////////////////////////////

cv::Mat cameraRotation=cv::Mat::eye(3,3, CV_64F);

double roll,pitch,yaw;

cv::Vec3d theta;

roll=0 ;

pitch=0;

yaw=0;

theta[0]=roll;

theta[1]=pitch;

theta[2]=yaw;

cameraRotation =eulerAnglesToRotationMatrix(theta);

double tx,ty,tz;

tx=0;

ty=0;

tz=0;

cv::Mat cameraTranslation = (cv::Mat_<double>(3,1) <<tx,ty,tz);

//////////////////////////////////////////projecting 3D points from world into virtual camera //////////////////////////////

std::vector<cv::Point3d> objectpoints;

std::vector<cv::Point2d> projectedPoints;

Points are in the following form (OpenCV coordinate):

▲

/ 1 2 X

|------------ ⯈

1| .

2| . . .

3| .

4| .

| Y

⯆

std::string pathToPointFile(argv[1]);

io::CSVReader<3> in(pathToPointFile);

in.read_header(io::ignore_extra_column, "x", "y", "z");

double x,y,z;

pcl::PointXYZ point;

boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > pointsInCameraCloud(new pcl::PointCloud<pcl::PointXYZ>);

while(in.read_row(x,y,z))

{

objectpoints.push_back(cv::Point3d(x,y,z));

pointsInCameraCloud->push_back(pcl::PointXYZ(x,y,z));

}

cv::projectPoints(objectpoints, cameraRotation, cameraTranslation, cameraMatrix, distortionCoefficient, projectedPoints);

///////////////////////////////////////////////saving the image //////////////////////////////////////////

double U,V;

cv::Mat cameraImage=cv::Mat::zeros(numberOfPixelInHeight,numberOfPixelInWidth,CV_8UC1);

cv::line(cameraImage,cv::Point2d(numberOfPixelInWidth/2,0),cv::Point2d(numberOfPixelInWidth/2,numberOfPixelInHeight),cv::Scalar(255,255,255));

cv::line(cameraImage,cv::Point2d(0,numberOfPixelInHeight/2),cv::Point2d(numberOfPixelInWidth,numberOfPixelInHeight/2),cv::Scalar(255,255,255));

for(std::size_t i=0;i<projectedPoints.size();i++)

{

V=int(projectedPoints.at(i).x);

U=int(projectedPoints.at(i).y);

//std::cout<<U <<"," <<V <<std::endl;

cameraImage.at<char>(int(U),int(V))=char(255);

}

std::string fileName=std::string("image_")+std::to_string(focalLength)+ std::string("_.jpg");

cv::imwrite(fileName,cameraImage);

/////////////////////////////////////////// ray tracing////////////////////////////////////////////

std::vector<cv::Point3d> directionRays;

reverseProjection(distortionCoefficient , cameraMatrix, projectedPoints, directionRays);

/////////////////////////////////////////// 3D visualization////////////////////////////////////////////

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));

viewer->setBackgroundColor (0, 0, 0);

int pointSize=5;

double coordinateFrameScaleSize=1;

std::string coordinateName;

coordinateName="World";

int viewport = 0;

viewer->addCoordinateSystem(coordinateFrameScaleSize,Eigen::Affine3f::Identity(),coordinateName,viewport);

double rayScale=5;

double r,g,b;

r=0.0;

g=1.0;

b=1.0;

for(std::size_t i=0;i<directionRays.size();i++)

{

std::cout<< directionRays[i].x<<","<<directionRays[i].y<<","<<directionRays[i].z <<std::endl;

viewer->addLine(pcl::PointXYZ(0 ,0 , 0), pcl::PointXYZ(rayScale*directionRays[i].x,rayScale*directionRays[i].y,rayScale*directionRays[i].z), r, g, b, std::to_string(i+100),viewport);

}

int Red,Blue,Green;

Red=255;

Green=0;

Blue=0;

pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ > tgt_h ( pointsInCameraCloud,Red,Green,Blue );

viewer->addPointCloud<pcl::PointXYZ> (pointsInCameraCloud ,tgt_h , std::to_string(1), 0);

viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::to_string(1));

while (!viewer->wasStopped ())

{

viewer->spinOnce (100);

boost::this_thread::sleep (boost::posix_time::microseconds (100000));

}

points are stored in CSV file like this:

x,y,z   
2,1, 1
1,2, 1
2,2, 1 
3,2, 1 
2,3, 1
2,4, 1

x,y,z

2,1, 1

1,2, 1

2,2, 1

3,2, 1

2,3, 1

2,4, 1

The content of “reverse_projection.hpp”

/////////////////////////////////////////implementing OpenCV undistortPoints///////////////////////////////////////

    cv::Mat imagePointMatrix=cv::Mat_<double>(3,1);
    cv::Mat imageRayMatrix=cv::Mat_<double>(3,1);

    std::cout<<"===============Inverse of Camera Matrix(OpenCV)==========================="<<std::endl;
    std::cout<<cameraMatrix.inv()<<std::endl;

    std::cout<<"=====================Inverse of Camera Matrix====================="<<std::endl;

    cv::Mat inv=(cv::Mat_<double>(3,3)<< cameraMatrix.at<double>(1,1)  ,0,-cameraMatrix.at<double>(1,1)*cameraMatrix.at<double>(0,2)
                 ,0,cameraMatrix.at<double>(0,0),-cameraMatrix.at<double>(1,2)*cameraMatrix.at<double>(0,0)
                 ,0,0,cameraMatrix.at<double>(0,0)*cameraMatrix.at<double>(1,1));
    std::cout<<inv/(cameraMatrix.at<double>(1,1)*cameraMatrix.at<double>(0,0))<<std::endl;

    std::cout<<"=====================Computed Rays===================== "<<std::endl;


    for(std::size_t i=0;i<imagePointsInCamera.size();i++)
    {
        imagePointMatrix.at<double>(0,0)=imagePointsInCamera[i].x;
        imagePointMatrix.at<double>(1,0)=imagePointsInCamera[i].y;
        imagePointMatrix.at<double>(2,0)=1.0;
        imageRayMatrix=cameraMatrix.inv()*imagePointMatrix;



        std::cout<<imageRayMatrix <<std::endl;


    }
/*
    (u,v) is the input point, (u', v') is the output point
    camera_matrix=[fx 0 cx; 0 fy cy; 0 0 1]
    P=[fx' 0 cx' tx; 0 fy' cy' ty; 0 0 1 tz]
    x" = (u - cx)/fx
    y" = (v - cy)/fy
    (x',y') = undistort(x",y",dist_coeffs)
    [X,Y,W]T = R*[x' y' 1]T
    x = X/W, y = Y/W
    only performed if P=[fx' 0 cx' [tx]; 0 fy' cy' [ty]; 0 0 1 [tz]] is specified
    u' = x*fx' + cx'
    v' = y*fy' + cy',
*/

    std::vector<cv::Point2d> imagePointsInCameraNormalizedCoordinate;
    cv::undistortPoints(imagePointsInCamera,imagePointsInCameraNormalizedCoordinate,cameraMatrix,distortionCoefficient);
    std::vector<cv::Point3d> imagePointsInCameraNormalizedCoordinateHomogeneous;
    cv::convertPointsToHomogeneous(imagePointsInCameraNormalizedCoordinate,imagePointsInCameraNormalizedCoordinateHomogeneous);
    cv::Mat imagePointsInCameraNormalizedCoordinateHomogeneousMatrix=cv::Mat(imagePointsInCameraNormalizedCoordinateHomogeneous);
    imagePointsInCameraNormalizedCoordinateHomogeneousMatrix=imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.reshape(1).t();
    double a,b,c;
    std::cout<<"=====================OpenCV Rays=====================" <<std::endl;

    for(int i=0;i<imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.cols;i++)
    {
        a=imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.at<double>(0,i);
        b=imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.at<double>(1,i);
        c=imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.at<double>(2,i);
        std::cout<<a <<std::endl;
        std::cout<<b <<std::endl;
        std::cout<<c <<std::endl;
        directionRays.push_back(cv::Point3d(a,b,c));

    }
/*
The inverse of a 3x3 matrix:
| a11 a12 a13 |-1
| a21 a22 a23 |    =  1/DET * A^-1
| a31 a32 a33 |

with A^-1  =

|  a33a22-a32a23  -(a33a12-a32a13)   a23a12-a22a13 |
|-(a33a21-a31a23)   a33a11-a31a13  -(a23a11-a21a13)|
|  a32a21-a31a22  -(a32a11-a31a12)   a22a11-a21a12 |

and DET  =  a11(a33a22-a32a23) - a21(a33a12-a32a13) + a31(a23a12-a22a13)

Camera Matrix:

|fx 0 cx|
|0 fy cy|
|0 0  1 |

Rays are  A^-1*p:

 1    |fy 0   -fycx|  |u|
----- |0  fx -cy*fx| *|v| = [ (u- cx)/fx, (v-cx)/fy, 1]
fx*fy |0  0   fy*fx|  |1|

*/

/////////////////////////////////////////implementing OpenCV undistortPoints///////////////////////////////////////

cv::Mat imagePointMatrix=cv::Mat_<double>(3,1);

cv::Mat imageRayMatrix=cv::Mat_<double>(3,1);

std::cout<<"===============Inverse of Camera Matrix(OpenCV)==========================="<<std::endl;

std::cout<<cameraMatrix.inv()<<std::endl;

std::cout<<"=====================Inverse of Camera Matrix====================="<<std::endl;

cv::Mat inv=(cv::Mat_<double>(3,3)<< cameraMatrix.at<double>(1,1) ,0,-cameraMatrix.at<double>(1,1)*cameraMatrix.at<double>(0,2)

,0,cameraMatrix.at<double>(0,0),-cameraMatrix.at<double>(1,2)*cameraMatrix.at<double>(0,0)

,0,0,cameraMatrix.at<double>(0,0)*cameraMatrix.at<double>(1,1));

std::cout<<inv/(cameraMatrix.at<double>(1,1)*cameraMatrix.at<double>(0,0))<<std::endl;

std::cout<<"=====================Computed Rays===================== "<<std::endl;

for(std::size_t i=0;i<imagePointsInCamera.size();i++)

{

imagePointMatrix.at<double>(0,0)=imagePointsInCamera[i].x;

imagePointMatrix.at<double>(1,0)=imagePointsInCamera[i].y;

imagePointMatrix.at<double>(2,0)=1.0;

imageRayMatrix=cameraMatrix.inv()*imagePointMatrix;

std::cout<<imageRayMatrix <<std::endl;

}

(u,v) is the input point, (u', v') is the output point

camera_matrix=[fx 0 cx; 0 fy cy; 0 0 1]

P=[fx' 0 cx' tx; 0 fy' cy' ty; 0 0 1 tz]

x" = (u - cx)/fx

y" = (v - cy)/fy

(x',y') = undistort(x",y",dist_coeffs)

[X,Y,W]T = R*[x' y' 1]T

x = X/W, y = Y/W

only performed if P=[fx' 0 cx' [tx]; 0 fy' cy' [ty]; 0 0 1 [tz]] is specified

u' = x*fx' + cx'

v' = y*fy' + cy',

std::vector<cv::Point2d> imagePointsInCameraNormalizedCoordinate;

cv::undistortPoints(imagePointsInCamera,imagePointsInCameraNormalizedCoordinate,cameraMatrix,distortionCoefficient);

std::vector<cv::Point3d> imagePointsInCameraNormalizedCoordinateHomogeneous;

cv::convertPointsToHomogeneous(imagePointsInCameraNormalizedCoordinate,imagePointsInCameraNormalizedCoordinateHomogeneous);

cv::Mat imagePointsInCameraNormalizedCoordinateHomogeneousMatrix=cv::Mat(imagePointsInCameraNormalizedCoordinateHomogeneous);

imagePointsInCameraNormalizedCoordinateHomogeneousMatrix=imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.reshape(1).t();

double a,b,c;

std::cout<<"=====================OpenCV Rays=====================" <<std::endl;

for(int i=0;i<imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.cols;i++)

{

a=imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.at<double>(0,i);

b=imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.at<double>(1,i);

c=imagePointsInCameraNormalizedCoordinateHomogeneousMatrix.at<double>(2,i);

std::cout<<a <<std::endl;

std::cout<<b <<std::endl;

std::cout<<c <<std::endl;

directionRays.push_back(cv::Point3d(a,b,c));

}

The inverse of a 3x3 matrix:

| a11 a12 a13 |-1

| a21 a22 a23 | = 1/DET * A^-1

| a31 a32 a33 |

with A^-1 =

| a33a22-a32a23 -(a33a12-a32a13) a23a12-a22a13 |

|-(a33a21-a31a23) a33a11-a31a13 -(a23a11-a21a13)|

| a32a21-a31a22 -(a32a11-a31a12) a22a11-a21a12 |

and DET = a11(a33a22-a32a23) - a21(a33a12-a32a13) + a31(a23a12-a22a13)

Camera Matrix:

|fx 0 cx|

|0 fy cy|

|0 0 1 |

Rays are A^-1*p:

1 |fy 0 -fycx| |u|

----- |0 fx -cy*fx| *|v| = [ (u- cx)/fx, (v-cx)/fy, 1]

fx*fy |0 0 fy*fx| |1|

The content of “transformation.hpp“

#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>

cv::Mat eulerAnglesToRotationMatrix(cv::Vec3d &theta)
{
    // Calculate rotation about x axis
    cv::Mat R_x = (cv::Mat_<double>(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]),   cos(theta[0])
               );

    // Calculate rotation about y axis
    cv::Mat R_y = (cv::Mat_<double>(3,3) <<
               cos(theta[1]),    0,      sin(theta[1]),
               0,               1,      0,
               -sin(theta[1]),   0,      cos(theta[1])
               );

    // Calculate rotation about z axis
    cv::Mat R_z = (cv::Mat_<double>(3,3) <<
               cos(theta[2]),    -sin(theta[2]),      0,
               sin(theta[2]),    cos(theta[2]),       0,
               0,               0,                  1);


    // Combined rotation matrix
    cv::Mat R = R_z * R_y * R_x;

    return R;

}

bool isRotationMatrix(cv::Mat &R)
{
    cv::Mat Rt;
    cv::transpose(R, Rt);
    cv::Mat shouldBeIdentity = Rt * R;
    cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type());

    return  cv::norm(I, shouldBeIdentity) < 1e-6;

}

cv::Vec3d rotationMatrixToEulerAngles(cv::Mat &R)
{

    assert(isRotationMatrix(R));

    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );

    bool singular = sy < 1e-6; // If

    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    return cv::Vec3f(x, y, z);
}

void transformPoints(cv::Mat rotation,cv::Mat translation, std::vector<cv::Point3d> &inputPoints,std::vector<cv::Point3d> &outputPoints)
{

    std::vector<cv::Point3d> pointsInLeftCamera;
    for(std::size_t i=0;i<inputPoints.size();i++)
    {
        cv::Point3d X=cv::Point3d(inputPoints[i].x ,inputPoints[i].y, inputPoints[i].z );
        cv::Mat X_mat=cv::Mat(X);
        cv::Mat pointInCameraMatrix=rotation*X_mat+translation;
        cv::Point3d pointInCameraCoordinate= cv::Point3d( pointInCameraMatrix.at<double>(0,0),pointInCameraMatrix.at<double>(1,0), pointInCameraMatrix.at<double>(2,0) );
        outputPoints.push_back(pointInCameraCoordinate);
    }
}

Eigen::Matrix3d eulerAnglesToRotationMatrix(double roll, double pitch,double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
    Eigen::Matrix3d rotationMatrix = q.matrix();
    return rotationMatrix;
}

#include <opencv2/opencv.hpp>

#include <Eigen/Eigen>

cv::Mat eulerAnglesToRotationMatrix(cv::Vec3d &theta)

{

// Calculate rotation about x axis

cv::Mat R_x = (cv::Mat_<double>(3,3) <<

1, 0, 0,

0, cos(theta[0]), -sin(theta[0]),

0, sin(theta[0]), cos(theta[0])

);

// Calculate rotation about y axis

cv::Mat R_y = (cv::Mat_<double>(3,3) <<

cos(theta[1]), 0, sin(theta[1]),

0, 1, 0,

-sin(theta[1]), 0, cos(theta[1])

);

// Calculate rotation about z axis

cv::Mat R_z = (cv::Mat_<double>(3,3) <<

cos(theta[2]), -sin(theta[2]), 0,

sin(theta[2]), cos(theta[2]), 0,

0, 0, 1);

// Combined rotation matrix

cv::Mat R = R_z * R_y * R_x;

return R;

}

bool isRotationMatrix(cv::Mat &R)

{

cv::Mat Rt;

cv::transpose(R, Rt);

cv::Mat shouldBeIdentity = Rt * R;

cv::Mat I = cv::Mat::eye(3,3, shouldBeIdentity.type());

return cv::norm(I, shouldBeIdentity) < 1e-6;

}

cv::Vec3d rotationMatrixToEulerAngles(cv::Mat &R)

{

assert(isRotationMatrix(R));

float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) + R.at<double>(1,0) * R.at<double>(1,0) );

bool singular = sy < 1e-6; // If

float x, y, z;

if (!singular)

{

x = atan2(R.at<double>(2,1) , R.at<double>(2,2));

y = atan2(-R.at<double>(2,0), sy);

z = atan2(R.at<double>(1,0), R.at<double>(0,0));

}

else

{

x = atan2(-R.at<double>(1,2), R.at<double>(1,1));

y = atan2(-R.at<double>(2,0), sy);

z = 0;

}

return cv::Vec3f(x, y, z);

}

void transformPoints(cv::Mat rotation,cv::Mat translation, std::vector<cv::Point3d> &inputPoints,std::vector<cv::Point3d> &outputPoints)

{

std::vector<cv::Point3d> pointsInLeftCamera;

for(std::size_t i=0;i<inputPoints.size();i++)

{

cv::Point3d X=cv::Point3d(inputPoints[i].x ,inputPoints[i].y, inputPoints[i].z );

cv::Mat X_mat=cv::Mat(X);

cv::Mat pointInCameraMatrix=rotation*X_mat+translation;

cv::Point3d pointInCameraCoordinate= cv::Point3d( pointInCameraMatrix.at<double>(0,0),pointInCameraMatrix.at<double>(1,0), pointInCameraMatrix.at<double>(2,0) );

outputPoints.push_back(pointInCameraCoordinate);

}

Eigen::Matrix3d eulerAnglesToRotationMatrix(double roll, double pitch,double yaw)

{

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());

Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());

Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;

Eigen::Matrix3d rotationMatrix = q.matrix();

return rotationMatrix;

}

Simulating A Virtual Camera With OpenCV and Reverse Projection From 2D to 3D Read More »

Eigenvectors of PCL pointcloud (moment of inertia)

Leave a Comment / C++, Computer Vision, Tutorials / admin

The snippet below shows how to computer PCA eigenvectors and eigenvalues (in this case could be interpreted as the moment of inertia).

#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/ply_io.h>

template <typename PointT>
void computeMomentOfInertia(boost::shared_ptr<pcl::PointCloud<PointT> > cloud_ptr)
{
	pcl::MomentOfInertiaEstimation <PointT> feature_extractor;
	feature_extractor.setInputCloud (cloud_ptr);
	feature_extractor.compute ();

	std::vector <float> moment_of_inertia;
	std::vector <float> eccentricity;


	pcl::PointXYZRGB min_point_AABB;
	pcl::PointXYZRGB max_point_AABB;
	pcl::PointXYZRGB min_point_OBB;
	pcl::PointXYZRGB max_point_OBB;
	pcl::PointXYZRGB position_OBB;


	Eigen::Matrix3f rotational_matrix_OBB;
	float major_value, middle_value, minor_value;
	Eigen::Vector3f major_vector, middle_vector, minor_vector;
	Eigen::Vector3f mass_center;

	feature_extractor.getMomentOfInertia (moment_of_inertia);
	feature_extractor.getEccentricity (eccentricity);
	feature_extractor.getAABB (min_point_AABB, max_point_AABB);
	feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
	feature_extractor.getEigenValues (major_value, middle_value, minor_value);
	feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
	feature_extractor.getMassCenter (mass_center);

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	viewer->setBackgroundColor (0, 0, 0);
	viewer->addCoordinateSystem (0.1);
	viewer->initCameraParameters ();

	pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
	pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
	pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
	pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));

	viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
	viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
	viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
	viewer->addPointCloud<pcl::PointXYZRGB> (cloud_ptr, "Moment Of Inertia (PCA Eigen Vector Axes)");
	
	while (!viewer->wasStopped ())
	{
		viewer->spinOnce ();
	}
}


int main(int argc, char **argv)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudrgb_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PLYReader PLYFileReader;
	const int offset=0;
	PLYFileReader.read< pcl::PointXYZRGB >(argv[1],*cloudrgb_ptr,offset);
	//pcl::io::loadPCDFile(argv[1],*cloudrgb_ptr);
	computeMomentOfInertia(cloudrgb_ptr);
	return 0;
}

#include <pcl/features/moment_of_inertia_estimation.h>

#include <pcl/point_cloud.h>

#include <pcl/io/pcd_io.h>

#include <pcl/visualization/cloud_viewer.h>

#include <pcl/io/ply_io.h>

template <typename PointT>

void computeMomentOfInertia(boost::shared_ptr<pcl::PointCloud<PointT> > cloud_ptr)

{

pcl::MomentOfInertiaEstimation <PointT> feature_extractor;

feature_extractor.setInputCloud (cloud_ptr);

feature_extractor.compute ();

std::vector <float> moment_of_inertia;

std::vector <float> eccentricity;

pcl::PointXYZRGB min_point_AABB;

pcl::PointXYZRGB max_point_AABB;

pcl::PointXYZRGB min_point_OBB;

pcl::PointXYZRGB max_point_OBB;

pcl::PointXYZRGB position_OBB;

Eigen::Matrix3f rotational_matrix_OBB;

float major_value, middle_value, minor_value;

Eigen::Vector3f major_vector, middle_vector, minor_vector;

Eigen::Vector3f mass_center;

feature_extractor.getMomentOfInertia (moment_of_inertia);

feature_extractor.getEccentricity (eccentricity);

feature_extractor.getAABB (min_point_AABB, max_point_AABB);

feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);

feature_extractor.getEigenValues (major_value, middle_value, minor_value);

feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);

feature_extractor.getMassCenter (mass_center);

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));

viewer->setBackgroundColor (0, 0, 0);

viewer->addCoordinateSystem (0.1);

viewer->initCameraParameters ();

pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));

pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));

pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));

pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));

viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");

viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");

viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");

viewer->addPointCloud<pcl::PointXYZRGB> (cloud_ptr, "Moment Of Inertia (PCA Eigen Vector Axes)");

while (!viewer->wasStopped ())

{

viewer->spinOnce ();

}

int main(int argc, char **argv)

{

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudrgb_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);

pcl::PLYReader PLYFileReader;

const int offset=0;

PLYFileReader.read< pcl::PointXYZRGB >(argv[1],*cloudrgb_ptr,offset);

//pcl::io::loadPCDFile(argv[1],*cloudrgb_ptr);

computeMomentOfInertia(cloudrgb_ptr);

return 0;

}

Eigenvectors of PCL pointcloud (moment of inertia) Read More »

A GUI ROS-package for cropping pcl pointcloud with dynamic reconfigure

2 Comments / C++, Computer Vision, Image Processing, ROS, Tutorials / admin

This ROS package enables you to crop the scene from Kinect (input topic type: PCL pointcloud). You can even enable fitting a plane to remove the ground from the scene and by adjusting correct parameter you can get the desired object from the scene. code available on my Github.

A GUI ROS-package for cropping pcl pointcloud with dynamic reconfigure Read More »

How to create octomap tree from mesh

2 Comments / C++, ROS, Tutorials / admin

I was looking for a way to create an octomap tree from arbitrary mesh file. Well at first I tried to convert my data into PCL pointcloud and then convert them into an octomap tree. But the problem was, for instance when you a cuboid mesh, you only have 8 vertex, which gives you 8 point and the walls between

How to create octomap tree from mesh Read More »

Autonomous navigation of two wheels differential drive robot in Gazebo

1 Comment / ROS / admin

Two wheels differential drive robot (with two caster wheels). List of installed sensors: • Velodyne VLP-16. • Velodyne HDL-32E. • Hokuyo Laser scanner. • IMU. • Microsoft Kinect/Asus Xtion Pro. • RGB Camera. You can manually control the robot with Joystick controller for mapping robot environment. Autonomous navigation is possible by setting goal pose.

Autonomous navigation of two wheels differential drive robot in Gazebo Read More »

Converting sensor_msgs::PCLPointCloud2 to sensor_msgs::PointCloud and reverse

Leave a Comment / C++, Computer Vision, Image Processing, ROS / admin

#include <sensor_msgs/point_cloud_conversion.h>

sensor_msgs::PointCloud2 point_cloud2;
sensor_msgs::PointCloud point_cloud;

sensor_msgs::convertPointCloudToPointCloud2(point_cloud, point_cloud2);
sensor_msgs::convertPointCloud2ToPointCloud(point_cloud2, point_cloud);

#include <sensor_msgs/point_cloud_conversion.h>

sensor_msgs::PointCloud2 point_cloud2;

sensor_msgs::PointCloud point_cloud;

sensor_msgs::convertPointCloudToPointCloud2(point_cloud, point_cloud2);

sensor_msgs::convertPointCloud2ToPointCloud(point_cloud2, point_cloud);

Converting sensor_msgs::PCLPointCloud2 to sensor_msgs::PointCloud and reverse Read More »

Converting pcl::PCLPointCloud2 to pcl::PointCloud and reverse

3 Comments / Computer Vision, Image Processing, ROS / admin

#include <pcl/conversions.h>

1	#include <pcl/conversions.h>

pcl::PCLPointCloud2 point_cloud2;
pcl::PointCloud<pcl::PointXYZ> point_cloud;

pcl::fromPCLPointCloud2( point_cloud2, point_cloud);

pcl::toPCLPointCloud2(point_cloud, point_cloud2);

pcl::PCLPointCloud2 point_cloud2;

pcl::PointCloud<pcl::PointXYZ> point_cloud;

pcl::fromPCLPointCloud2( point_cloud2, point_cloud);

pcl::toPCLPointCloud2(point_cloud, point_cloud2);

more about pcl point cloud and conversion

Converting pcl::PCLPointCloud2 to pcl::PointCloud and reverse Read More »

Assembling Laser scans into PCL point cloud Using Gazebo and ROS

3 Comments / Computer Vision, ROS, Tutorials / admin

For this work, first I loaded the RRBot in Gazebo and launched Its joints controller, then I sent a periodic signal to the robot such that the laser scanner mounted on the robot swings. In the following, I assembled the incoming laser scans with the transformation from tf and created PCL point cloud. Install the necessary

Assembling Laser scans into PCL point cloud Using Gazebo and ROS Read More »

Real time object recognition and 6DOF pose estimation with PCL pointcloud and ROS

2 Comments / C++, Computer Vision, Machine Learning, ROS, Tutorials / admin

Real-time object recognition and 6DOF pose estimation with PCL pointcloud and ROS. This approach is based on normal coherence

Real time object recognition and 6DOF pose estimation with PCL pointcloud and ROS Read More »

Real-time object recognition and 6DOF pose estimation based on Linemod algorithm with ROS and PCL pointcloud

5 Comments / Computer Vision, Machine Learning, ROS, Tutorials / admin

In this tutorial, I’m gonna show you how to do object recognition and 6DOF pose estimation in real-time based on Linemod algorithm with ROS and PCL pointcloud. First, you need to install ork:

sudo apt-get install ros-indigo-object-recognition-ros-visualization ros-indigo-object-recognition-msgs ros-indigo-object-recognition-core

1	sudo apt-get install ros-indigo-object-recognition-ros-visualization ros-indigo-object-recognition-msgs ros-indigo-object-recognition-core

Then add the model of your object for tracking to Couch DB:

 rosrun object_recognition_core object_add.py -n coke -d "A universal can of coke" --commit

1	rosrun object_recognition_core object_add.py -n coke -d "A universal can of coke" --commit

rosrun object_recognition_core mesh_add.py <YOUR_OBJECT_ID_FROM_PREVIOUS_STEP> /home/behnam/ork_tutorials/data/coke.obj --commit

1	rosrun object_recognition_core mesh_add.py <YOUR_OBJECT_ID_FROM_PREVIOUS_STEP> /home/behnam/ork_tutorials/data/coke.obj --commit

You need to install Kinect driver, if you don’t

Real-time object recognition and 6DOF pose estimation based on Linemod algorithm with ROS and PCL pointcloud Read More »

Cookie	Duration	Description
cookielawinfo-checbox-analytics	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Analytics".
cookielawinfo-checbox-functional	11 months	The cookie is set by GDPR cookie consent to record the user consent for the cookies in the category "Functional".
cookielawinfo-checbox-others	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Other.
cookielawinfo-checkbox-necessary	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookies is used to store the user consent for the cookies in the category "Necessary".
cookielawinfo-checkbox-performance	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Performance".
viewed_cookie_policy	11 months	The cookie is set by the GDPR Cookie Consent plugin and is used to store whether or not user has consented to the use of cookies. It does not store any personal data.