admin

How to use image_geometry and camera_info_manager in ROS

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

camera_info_publisher.cpp:

#include <camera_info_manager/camera_info_manager.h>
int main(int argc, char** argv)
{
    ros::init(argc, argv, "camera_info_publisher");
    ros::NodeHandle nh;

    const std::string cname="prosilica";
    const std::string url="file://${ROS_HOME}/camera_info/prosilica.yaml";
    ros::Publisher pubCameraInfo = nh.advertise<sensor_msgs::CameraInfo>("camera/camera_info", 1);
    camera_info_manager::CameraInfoManager cinfo(nh,cname,url);
    sensor_msgs::CameraInfo msgCameraInfo;
    ros::Rate loop_rate(5);
    while (nh.ok())
    {
        msgCameraInfo=cinfo.getCameraInfo();
        pubCameraInfo.publish(msgCameraInfo);
        ros::spinOnce();
        loop_rate.sleep();
    }
}

#include <camera_info_manager/camera_info_manager.h>

int main(int argc, char** argv)

{

ros::init(argc, argv, "camera_info_publisher");

ros::NodeHandle nh;

const std::string cname="prosilica";

const std::string url="file://${ROS_HOME}/camera_info/prosilica.yaml";

ros::Publisher pubCameraInfo = nh.advertise<sensor_msgs::CameraInfo>("camera/camera_info", 1);

camera_info_manager::CameraInfoManager cinfo(nh,cname,url);

sensor_msgs::CameraInfo msgCameraInfo;

ros::Rate loop_rate(5);

while (nh.ok())

{

msgCameraInfo=cinfo.getCameraInfo();

pubCameraInfo.publish(msgCameraInfo);

ros::spinOnce();

loop_rate.sleep();

}

image_geometry_demo.cpp:

#include <ros/ros.h>
#include <image_geometry/pinhole_camera_model.h>

void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)
{
    std::cout<<"===============================cameraInfoCallback===============================" <<std::endl;
    image_geometry::PinholeCameraModel cam_model_;
    cam_model_.fromCameraInfo(info_msg);
    // projection_matrix is the matrix you should use if you don't want to use project3dToPixel() and want to use opencv API
    cv::Matx34d projection_matrix=cam_model_.fullProjectionMatrix();
    std::cout<<cam_model_.project3dToPixel(cv::Point3d(-0.1392072,-0.02571392, 2.50376511) )<<std::endl;
}

int main(int argc,char ** argv)
{
    ros::init(argc,argv,"image_geometry_demo",1);
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("camera/camera_info", 1000, cameraInfoCallback);
    ros::spin();
}

#include <ros/ros.h>

#include <image_geometry/pinhole_camera_model.h>

void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)

{

std::cout<<"===============================cameraInfoCallback===============================" <<std::endl;

image_geometry::PinholeCameraModel cam_model_;

cam_model_.fromCameraInfo(info_msg);

// projection_matrix is the matrix you should use if you don't want to use project3dToPixel() and want to use opencv API

cv::Matx34d projection_matrix=cam_model_.fullProjectionMatrix();

std::cout<<cam_model_.project3dToPixel(cv::Point3d(-0.1392072,-0.02571392, 2.50376511) )<<std::endl;

}

int main(int argc,char ** argv)

{

ros::init(argc,argv,"image_geometry_demo",1);

ros::NodeHandle nh;

ros::Subscriber sub = nh.subscribe("camera/camera_info", 1000, cameraInfoCallback);

ros::spin();

}

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS image_geometry camera_info_manager roscpp)

1	find_package(catkin REQUIRED COMPONENTS image_geometry camera_info_manager roscpp)

How to use image_geometry and camera_info_manager in ROS Read More »

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 »

Publishing video file and images with ROS

Camera Calibration Based on Zhang’s Algorithm (Chess Board) Explained

Leave a Comment / Computer Vision, Image Processing, Linear Algebra, Machine Learning, Tutorials / admin

Camera Calibration Based on Zhang’s Algorithm (Chess Board) Explained Read More »

Camera Calibration Based on Direct Linear Transform Explained

2 Comments / C++, Computer Vision, Tutorials / admin

Camera Calibration Based on Direct Linear Transform Explained Read More »

Geometry of Stereo Vision explained

Leave a Comment / Computer Vision, Tutorials / admin

In this video, I explain the geometry of stereo Vision. Please have look at my other tutorial about finding the essential and fundamental matrix.

Geometry of Stereo Vision explained Read More »

The relation between the size of an object and its projection in camera fame

Leave a Comment / Computer Vision, Tutorials / admin

What is the relation between the size of an object and its projection in camera fame? In this video, I explain how far you should stay from an object to see it at your desired size at camera frame </

The relation between the size of an object and its projection in camera fame Read More »

Vanishing points in computer vision explained

Leave a Comment / Computer Vision, Tutorials / admin

In this tutorial, I explain the vanishing points in computer vision.

Vanishing points in computer vision explained Read More »

Equation of Line and Plane in 3D Space Explained

Leave a Comment / Computer Vision, Tutorials / admin

The equation of Line and plane in 3D Space:

Equation of Line and Plane in 3D Space Explained Read More »

How to develop GUI Application with PyQt (python Qt)

Leave a Comment / C++, Python, Tutorials / admin

There are two main methods for developing GUI application with qt: 1) Adding all widgets in your code (your cpp or python code) 2) Creating qt UI files, adding widgets there and load everything into your application. 1)Adding all widgets in your code Here is the snippet for adding all widgets and their slots in code:

import sys
print(sys.version)
import warnings
warnings.filterwarnings('ignore')


from PyQt5 import QtCore, QtWidgets
from PyQt5.QtWidgets import QMainWindow, QLabel, QSlider
from PyQt5.QtCore import QSize, Qt, pyqtSlot,pyqtSignal 



class HelloWindow(QMainWindow):
    
    pushButtonClicked = pyqtSignal()
    valueUpdating = pyqtSignal(int)
    
    
    def pushButtonClicked(self):
        self.title.setText("PyQt updated!")
            
    def valueUpdating(self, value):
        self.title.setText(str(value))
    
    def __init__(self):
        QMainWindow.__init__(self)

        self.setMinimumSize(QSize(640, 480))    
        self.setWindowTitle("Hello world") 

        self.title = QLabel("PyQt!", self)
        self.title.setAlignment(QtCore.Qt.AlignCenter) 


        self.pushButton = QtWidgets.QPushButton(self)
        self.pushButton.setGeometry(QtCore.QRect(140, 200, 99, 27))#x,y, width, height
        self.pushButton.setObjectName("pushButton")
        self.pushButton.setText("Click Me!")
        self.pushButton.clicked.connect(self.pushButtonClicked)
        
        self.slider = QtWidgets.QSlider(self)
        self.slider.setGeometry(QtCore.QRect(100, 20, 100, 30))
        self.slider.setFocusPolicy(Qt.StrongFocus)
        self.slider.setTickPosition(QSlider.TicksBothSides)
        self.slider.setSingleStep(1)
        self.slider.setPageStep(1)
        self.slider.setMaximum(100)
        self.slider.setMinimum(0)
        self.slider.setOrientation(Qt.Horizontal)
        
        self.slider.valueChanged.connect(self.valueUpdating)
        


if __name__ == "__main__":
    app = QtWidgets.QApplication(sys.argv)
    mainWin = HelloWindow()
    mainWin.show()
    sys.exit( app.exec_() )

import sys

print(sys.version)

import warnings

warnings.filterwarnings('ignore')

from PyQt5 import QtCore, QtWidgets

from PyQt5.QtWidgets import QMainWindow, QLabel, QSlider

from PyQt5.QtCore import QSize, Qt, pyqtSlot,pyqtSignal

class HelloWindow(QMainWindow):

pushButtonClicked = pyqtSignal()

valueUpdating = pyqtSignal(int)

def pushButtonClicked(self):

self.title.setText("PyQt updated!")

def valueUpdating(self, value):

self.title.setText(str(value))

def __init__(self):

QMainWindow.__init__(self)

self.setMinimumSize(QSize(640, 480))

self.setWindowTitle("Hello world")

self.title = QLabel("PyQt!", self)

self.title.setAlignment(QtCore.Qt.AlignCenter)

self.pushButton = QtWidgets.QPushButton(self)

self.pushButton.setGeometry(QtCore.QRect(140, 200, 99, 27))#x,y, width, height

self.pushButton.setObjectName("pushButton")

self.pushButton.setText("Click Me!")

self.pushButton.clicked.connect(self.pushButtonClicked)

self.slider = QtWidgets.QSlider(self)

self.slider.setGeometry(QtCore.QRect(100, 20, 100, 30))

self.slider.setFocusPolicy(Qt.StrongFocus)

self.slider.setTickPosition(QSlider.TicksBothSides)

self.slider.setSingleStep(1)

self.slider.setPageStep(1)

self.slider.setMaximum(100)

self.slider.setMinimum(0)

self.slider.setOrientation(Qt.Horizontal)

self.slider.valueChanged.connect(self.valueUpdating)

if __name__ == "__main__":

app = QtWidgets.QApplication(sys.argv)

mainWin = HelloWindow()

mainWin.show()

sys.exit( app.exec_() )

How to develop GUI Application with PyQt (python Qt) 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.