opencv Archives - Robotics with ROS

Decomposing Projection Using OpenCV and C++

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

#include <opencv/cv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include "transformation.hpp"

//https://www.cnblogs.com/shengguang/p/5932522.html
void HouseHolderQR(const cv::Mat &A, cv::Mat &Q, cv::Mat &R)
{
    assert ( A.channels() == 1 );
    assert ( A.rows >= A.cols );
    auto sign = [](double value) { return value >= 0 ? 1: -1; };
    const auto totalRows = A.rows;
    const auto totalCols = A.cols;
    R = A.clone();
    Q = cv::Mat::eye ( totalRows, totalRows, A.type() );
    for ( int col = 0; col < A.cols; ++ col )
    {
        cv::Mat matAROI = cv::Mat ( R, cv::Range ( col, totalRows ), cv::Range ( col, totalCols ) );
        cv::Mat y = matAROI.col ( 0 );
        auto yNorm = norm ( y );
        cv::Mat e1 = cv::Mat::eye ( y.rows, 1, A.type() );
        cv::Mat w = y + sign(y.at<double>(0,0)) *  yNorm * e1;
        cv::Mat v = w / norm( w );
        cv::Mat vT; cv::transpose(v, vT );
        cv::Mat I = cv::Mat::eye( matAROI.rows, matAROI.rows, A.type() );
        cv::Mat I_2VVT = I - 2 * v * vT;
        cv::Mat matH = cv::Mat::eye ( totalRows, totalRows, A.type() );
        cv::Mat matHROI = cv::Mat(matH, cv::Range ( col, totalRows ), cv::Range ( col, totalRows ) );
        I_2VVT.copyTo ( matHROI );
        R = matH * R;
        Q = Q * matH;
    }
}

int main()
{


/*
Thera are two notations for Projection Matrix

1)P=K[R|t]
                                                              ┌X┐
                                                      ┌R,t┐   |Y|
    X_Cam=R*X+t  ==> Homogeneous Coordinate ==> X_Cam=|0,1|   |Z|
                                                      └   ┘4x4└1┘4x1

                                         ┌X┐
                                ┌   ┐    |Y|
    image plane  <--x=K[I|0]3x4 |R,t|    |Z|
                                └0,1┘4x4 └1┘4x1

    P=K[R|t]

2)P=KR[I|-X0]
                                                              ┌X┐
                                                       ┌R -Rc┐|Y|
    X_Cam=R*[X_w-C]  ==> Homogeneous Coordinate ==>    |     ||Z|
                                                       └0  1 ┘└1┘
                                       ┌X┐
                                ┌R -Rc┐|Y|
    image plane  <--x=K[I|0]3x4 |0  1 ||Z|
                                └     ┘└1┘
    P=K[R|-RC]= KR[I|-C]


(1) and (2) ==> t=-RC

*/
    int numberOfPixelInHeight,numberOfPixelInWidth;
    double heightOfSensor, widthOfSensor;
    double focalLength=1.50;
    double mx, my, U0, V0;
    numberOfPixelInHeight=600;
    numberOfPixelInWidth=800;

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

    double tx,ty,tz,roll,pitch,yaw;

    tx=1.0;
    ty=2.1;
    tz=-1.4;

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

    cv::Vec3d theta;

    roll=+M_PI/4 ;
    pitch=+M_PI/10;
    yaw=-+M_PI/6;

    theta[0]=roll;
    theta[1]=pitch;
    theta[2]=yaw;
    cv::Mat rotation=eulerAnglesToRotationMatrix(theta);
    //1)P=K[R|t]
    cv::Mat R_T;
    cv::hconcat(rotation, translation, R_T);
    cv::Mat projectionMatrix=cameraMatrix*R_T;



    cv::Mat calculatedCameraMatrix,calculatedRotation,calculatedTranslation;

    cv::decomposeProjectionMatrix(projectionMatrix,calculatedCameraMatrix,calculatedRotation,calculatedTranslation);


    std::cout<<"==============================Ground Truth==============================" <<std::endl;

    std::cout<<"Rotation Matrix (Ground Truth)" <<std::endl;
    std::cout<<rotation <<std::endl;

    std::cout<<"Translation Matrix (Ground Truth)" <<std::endl;
    std::cout<<translation <<std::endl;

    std::cout<<"Camera Matrix (Ground Truth)" <<std::endl;
    std::cout<<cameraMatrix <<std::endl;

    std::cout<<"==============================Decomposing Using OpenCV==============================" <<std::endl;


    std::cout<<"Computed Rotation Matrix (OpenCV)" <<std::endl;
    std::cout<<calculatedRotation <<std::endl;

    std::cout<<"Computed Translation Matrix (OpenCV)" <<std::endl;
    //std::cout<<calculatedTranslation/calculatedTranslation.at<double>(3,0) <<std::endl;
    cv::Mat tempT=(cv::Mat_<double>(3,1)<<
                   calculatedTranslation.at<double>(0,0)/calculatedTranslation.at<double>(3,0),
                   calculatedTranslation.at<double>(1,0)/calculatedTranslation.at<double>(3,0),
                   calculatedTranslation.at<double>(2,0)/calculatedTranslation.at<double>(3,0));
    std::cout<<-rotation*tempT<<std::endl;

    std::cout<<"Computed Camera Matrix (OpenCV)" <<std::endl;
    std::cout<<calculatedCameraMatrix <<std::endl;

///////////////////////////////////////decomposing the projection matrix///////////////////////////////////////////

/*

    P=KR[I|-X0]=[H_inf3x3|h3x1]
    KR=H_inf3x3
    1)X0
    -KRX0=h3x1 => X0=-(KR)^-1*h3x1 ==>X0=-(H_inf3x3)^-1*h3x1
    2)K,R

    KR=H_inf3x3 =>(KR)^-1= H_inf3x3^-1 =>R^-1*K^-1=H_inf3x3^-1 | R^-1*K^-1=Q*R => R=Q^-1, K=R^-1
                                         H_inf3x3^-1=Q*R       |

*/


    cv::Mat H_inf3x3=(cv::Mat_<double>(3,3)<< projectionMatrix.at<double>(0,0),projectionMatrix.at<double>(0,1),projectionMatrix.at<double>(0,2)
                      ,projectionMatrix.at<double>(1,0),projectionMatrix.at<double>(1,1),projectionMatrix.at<double>(1,2)
                      ,projectionMatrix.at<double>(2,0),projectionMatrix.at<double>(2,1),projectionMatrix.at<double>(2,2));

    cv::Mat h3x1=(cv::Mat_<double>(3,1)<<projectionMatrix.at<double>(0,3),projectionMatrix.at<double>(1,3),projectionMatrix.at<double>(2,3) );



    cv::Mat Q,R;
    cv::Mat H_inf3x3_inv=H_inf3x3.inv();
    //R=Q^-1, K=R^-1


    HouseHolderQR(H_inf3x3_inv, Q, R);
    cv::Mat K=R.inv();

    std::cout<<"==============================Decomposing Using My Code==============================" <<std::endl;
    //due to homogeneity we divide it by last element
    std::cout<<"Estimated Camera Matrix\n"<<K/K.at<double>(2,2) <<std::endl;

    cv::Mat rotationMatrix=Q.inv();
    std::cout<<"Estimated Camera Rotation\n"<<rotationMatrix*-1 <<std::endl;

    std::cout<<"Estimated Camera Translation" <<std::endl;

    //t=-R*C, Q.inv()=R
    std::cout<<-1*(-Q.inv()*(-H_inf3x3.inv()*h3x1 ))<<std::endl;

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

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

#include <opencv/cv.hpp>

#include <opencv2/core.hpp>

#include <opencv2/calib3d.hpp>

#include "transformation.hpp"

//https://www.cnblogs.com/shengguang/p/5932522.html

void HouseHolderQR(const cv::Mat &A, cv::Mat &Q, cv::Mat &R)

{

assert ( A.channels() == 1 );

assert ( A.rows >= A.cols );

auto sign = [](double value) { return value >= 0 ? 1: -1; };

const auto totalRows = A.rows;

const auto totalCols = A.cols;

R = A.clone();

Q = cv::Mat::eye ( totalRows, totalRows, A.type() );

for ( int col = 0; col < A.cols; ++ col )

{

cv::Mat matAROI = cv::Mat ( R, cv::Range ( col, totalRows ), cv::Range ( col, totalCols ) );

cv::Mat y = matAROI.col ( 0 );

auto yNorm = norm ( y );

cv::Mat e1 = cv::Mat::eye ( y.rows, 1, A.type() );

cv::Mat w = y + sign(y.at<double>(0,0)) * yNorm * e1;

cv::Mat v = w / norm( w );

cv::Mat vT; cv::transpose(v, vT );

cv::Mat I = cv::Mat::eye( matAROI.rows, matAROI.rows, A.type() );

cv::Mat I_2VVT = I - 2 * v * vT;

cv::Mat matH = cv::Mat::eye ( totalRows, totalRows, A.type() );

cv::Mat matHROI = cv::Mat(matH, cv::Range ( col, totalRows ), cv::Range ( col, totalRows ) );

I_2VVT.copyTo ( matHROI );

R = matH * R;

Q = Q * matH;

}

int main()

{

Thera are two notations for Projection Matrix

1)P=K[R|t]

┌X┐

┌R,t┐ |Y|

X_Cam=R*X+t ==> Homogeneous Coordinate ==> X_Cam=|0,1| |Z|

└ ┘4x4└1┘4x1

┌X┐

┌ ┐ |Y|

image plane <--x=K[I|0]3x4 |R,t| |Z|

└0,1┘4x4 └1┘4x1

P=K[R|t]

2)P=KR[I|-X0]

┌X┐

┌R -Rc┐|Y|

X_Cam=R*[X_w-C] ==> Homogeneous Coordinate ==> | ||Z|

└0 1 ┘└1┘

┌X┐

┌R -Rc┐|Y|

image plane <--x=K[I|0]3x4 |0 1 ||Z|

└ ┘└1┘

P=K[R|-RC]= KR[I|-C]

(1) and (2) ==> t=-RC

int numberOfPixelInHeight,numberOfPixelInWidth;

double heightOfSensor, widthOfSensor;

double focalLength=1.50;

double mx, my, U0, V0;

numberOfPixelInHeight=600;

numberOfPixelInWidth=800;

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

double tx,ty,tz,roll,pitch,yaw;

tx=1.0;

ty=2.1;

tz=-1.4;

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

cv::Vec3d theta;

roll=+M_PI/4 ;

pitch=+M_PI/10;

yaw=-+M_PI/6;

theta[0]=roll;

theta[1]=pitch;

theta[2]=yaw;

cv::Mat rotation=eulerAnglesToRotationMatrix(theta);

//1)P=K[R|t]

cv::Mat R_T;

cv::hconcat(rotation, translation, R_T);

cv::Mat projectionMatrix=cameraMatrix*R_T;

cv::Mat calculatedCameraMatrix,calculatedRotation,calculatedTranslation;

cv::decomposeProjectionMatrix(projectionMatrix,calculatedCameraMatrix,calculatedRotation,calculatedTranslation);

std::cout<<"==============================Ground Truth==============================" <<std::endl;

std::cout<<"Rotation Matrix (Ground Truth)" <<std::endl;

std::cout<<rotation <<std::endl;

std::cout<<"Translation Matrix (Ground Truth)" <<std::endl;

std::cout<<translation <<std::endl;

std::cout<<"Camera Matrix (Ground Truth)" <<std::endl;

std::cout<<cameraMatrix <<std::endl;

std::cout<<"==============================Decomposing Using OpenCV==============================" <<std::endl;

std::cout<<"Computed Rotation Matrix (OpenCV)" <<std::endl;

std::cout<<calculatedRotation <<std::endl;

std::cout<<"Computed Translation Matrix (OpenCV)" <<std::endl;

//std::cout<<calculatedTranslation/calculatedTranslation.at<double>(3,0) <<std::endl;

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

calculatedTranslation.at<double>(0,0)/calculatedTranslation.at<double>(3,0),

calculatedTranslation.at<double>(1,0)/calculatedTranslation.at<double>(3,0),

calculatedTranslation.at<double>(2,0)/calculatedTranslation.at<double>(3,0));

std::cout<<-rotation*tempT<<std::endl;

std::cout<<"Computed Camera Matrix (OpenCV)" <<std::endl;

std::cout<<calculatedCameraMatrix <<std::endl;

///////////////////////////////////////decomposing the projection matrix///////////////////////////////////////////

P=KR[I|-X0]=[H_inf3x3|h3x1]

KR=H_inf3x3

1)X0

-KRX0=h3x1 => X0=-(KR)^-1*h3x1 ==>X0=-(H_inf3x3)^-1*h3x1

2)K,R

KR=H_inf3x3 =>(KR)^-1= H_inf3x3^-1 =>R^-1*K^-1=H_inf3x3^-1 | R^-1*K^-1=Q*R => R=Q^-1, K=R^-1

H_inf3x3^-1=Q*R |

cv::Mat H_inf3x3=(cv::Mat_<double>(3,3)<< projectionMatrix.at<double>(0,0),projectionMatrix.at<double>(0,1),projectionMatrix.at<double>(0,2)

,projectionMatrix.at<double>(1,0),projectionMatrix.at<double>(1,1),projectionMatrix.at<double>(1,2)

,projectionMatrix.at<double>(2,0),projectionMatrix.at<double>(2,1),projectionMatrix.at<double>(2,2));

cv::Mat h3x1=(cv::Mat_<double>(3,1)<<projectionMatrix.at<double>(0,3),projectionMatrix.at<double>(1,3),projectionMatrix.at<double>(2,3) );

cv::Mat Q,R;

cv::Mat H_inf3x3_inv=H_inf3x3.inv();

//R=Q^-1, K=R^-1

HouseHolderQR(H_inf3x3_inv, Q, R);

cv::Mat K=R.inv();

std::cout<<"==============================Decomposing Using My Code==============================" <<std::endl;

//due to homogeneity we divide it by last element

std::cout<<"Estimated Camera Matrix\n"<<K/K.at<double>(2,2) <<std::endl;

cv::Mat rotationMatrix=Q.inv();

std::cout<<"Estimated Camera Rotation\n"<<rotationMatrix*-1 <<std::endl;

std::cout<<"Estimated Camera Translation" <<std::endl;

//t=-R*C, Q.inv()=R

std::cout<<-1*(-Q.inv()*(-H_inf3x3.inv()*h3x1 ))<<std::endl;

And the output is:

==============================Ground Truth==============================
Rotation Matrix (Ground Truth)
[0.823639103546332, 0.5427868801100539, -0.1643199010764935;
 -0.4755282581475767, 0.5031184295835893, -0.7216264418079997;
 -0.3090169943749474, 0.6724985119639573, 0.6724985119639574]
Translation Matrix (Ground Truth)
[1;
 2.1;
 -1.4]
Camera Matrix (Ground Truth)
[120, 0, 400;
 0, 90, 300;
 0, 0, 1]
==============================Decomposing Using OpenCV==============================
Computed Rotation Matrix (OpenCV)
[0.823639103546332, 0.5427868801100539, -0.1643199010764936;
 -0.4755282581475769, 0.5031184295835893, -0.7216264418079997;
 -0.3090169943749474, 0.6724985119639573, 0.6724985119639574]
Computed Translation Matrix (OpenCV)
[0.9999999999999988;
 2.1;
 -1.4]
Computed Camera Matrix (OpenCV)
[120, -1.4210854715202e-14, 400;
 0, 90.00000000000001, 300;
 0, 0, 1]
==============================Decomposing Using My Code==============================
Estimated Camera Matrix
[120, -5.776629175002766e-14, 399.9999999999999;
 -5.608036291626306e-15, 89.99999999999994, 299.9999999999999;
 -1.49192016182457e-17, -1.561251128379126e-16, 1]
Estimated Camera Rotation
[0.8236391035463322, 0.5427868801100542, -0.1643199010764936;
 -0.4755282581475767, 0.5031184295835892, -0.7216264418079998;
 -0.3090169943749475, 0.6724985119639573, 0.6724985119639575]
Estimated Camera Translation
[1;
 2.099999999999999;
 -1.4]

==============================Ground Truth==============================

Rotation Matrix (Ground Truth)

[0.823639103546332, 0.5427868801100539, -0.1643199010764935;

-0.4755282581475767, 0.5031184295835893, -0.7216264418079997;

-0.3090169943749474, 0.6724985119639573, 0.6724985119639574]

Translation Matrix (Ground Truth)

[1;

2.1;

-1.4]

Camera Matrix (Ground Truth)

[120, 0, 400;

0, 90, 300;

0, 0, 1]

==============================Decomposing Using OpenCV==============================

Computed Rotation Matrix (OpenCV)

[0.823639103546332, 0.5427868801100539, -0.1643199010764936;

-0.4755282581475769, 0.5031184295835893, -0.7216264418079997;

-0.3090169943749474, 0.6724985119639573, 0.6724985119639574]

Computed Translation Matrix (OpenCV)

[0.9999999999999988;

2.1;

-1.4]

Computed Camera Matrix (OpenCV)

[120, -1.4210854715202e-14, 400;

0, 90.00000000000001, 300;

0, 0, 1]

==============================Decomposing Using My Code==============================

Estimated Camera Matrix

[120, -5.776629175002766e-14, 399.9999999999999;

-5.608036291626306e-15, 89.99999999999994, 299.9999999999999;

-1.49192016182457e-17, -1.561251128379126e-16, 1]

Estimated Camera Rotation

[0.8236391035463322, 0.5427868801100542, -0.1643199010764936;

-0.4755282581475767, 0.5031184295835892, -0.7216264418079998;

-0.3090169943749475, 0.6724985119639573, 0.6724985119639575]

Estimated Camera Translation

[1;

2.099999999999999;

-1.4]

Decomposing Projection Using OpenCV and C++ Read More »

Create OpenCV Matrix (cv::Mat, cv::Mat_ ) From Various Data Types

Leave a Comment / C++, Tutorials / admin

1) cv:: Mat_ You can create a matrix with cv:: Mat_

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

1 2	cv::Mat cameraMatrix(3, 3, cv::DataType::type); cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) <<focalLengthmx, 0, V0,0,focalLengthmy,U0,0,0,1);

2) cv::Mat or like this with cv::Mat

/*
              C1   C2    C3    C4
    CV_8U     0    8     16    24
    CV_8S     1    9     17    25
    CV_16U    2    10    18    26
    CV_16S    3    11    19    27
    CV_32S    4    12    20    28
    CV_32F    5    13    21    29
    CV_64F    6    14    22    30
*/

cv::Mat mat_CV_64FC1(number_of_rows, number_of_columns,CV_64FC1);

C1 C2 C3 C4

CV_8U 0 8 16 24

CV_8S 1 9 17 25

CV_16U 2 10 18 26

CV_16S 3 11 19 27

CV_32S 4 12 20 28

CV_32F 5 13 21 29

CV_64F 6 14 22 30

cv::Mat mat_CV_64FC1(number_of_rows, number_of_columns,CV_64FC1);

if you forgot the list of data types you use

cv::Mat A(number_of_rows, number_of_columns, cv::DataType<float>::type);

1	cv::Mat A(number_of_rows, number_of_columns, cv::DataType<float>::type);

more data types here

Create OpenCV Matrix (cv::Mat, cv::Mat_ ) From Various Data Types Read More »

Computing Fundamental Matrix and Drawing Epipolar Lines for Stereo Vision Cameras in OpenCV

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

Following my other post, you can extract the equation for epipolar lines. Basically choosing one point in one image and using fundamental matrix, we will get a line in the other image:

    /*
    FM_7POINT   7-point algorithm
    FM_8POINT   8-point algorithm
    FM_LMEDS    least-median algorithm. 7-point algorithm is used.
    FM_RANSAC   ANSAC algorithm. It needs at least 15 points. 7-point algorithm is used
    */	

    cv::Mat fundamentalMatrix= cv::findFundamentalMat(imagePointsLeftCamera, imagePointsRightCamera, cv::FM_8POINT);
    std::vector<cv::Vec3d> leftLines, rightLines;
    cv::computeCorrespondEpilines(imagePointsLeftCamera, 1, fundamentalMatrix, rightLines);
    cv::computeCorrespondEpilines(imagePointsRightCamera, 2, fundamentalMatrix, leftLines);
	
    cv::Mat leftImageRGB(leftImage.size(), CV_8UC3);
    cv::cvtColor(leftImage, leftImageRGB, CV_GRAY2RGB);
	
    cv::Mat rightImageRGB(rightImage.size(), CV_8UC3);
    cv::cvtColor(rightImage, rightImageRGB, CV_GRAY2RGB);

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

    for(std::size_t i=0;i<rightLines.size();i=i+1)
    {
        cv::Vec3d l=rightLines.at(i);
	double a=l.val[0];
        double b=l.val[1];
        double c=l.val[2];
        std::cout<<"------------------------a,b,c Using OpenCV (ax+by+c=0)------------------------------"<<std::endl;
        std::cout<< a <<", "<<b <<", "<<c <<std::endl;
        std::cout<<"------------------------calculating a,b,c (ax+by+c=0) ------------------------------"<<std::endl;
        
        imagePointLeftCameraMatrix.at<double>(0,0)=imagePointsLeftCamera[i].x;
        imagePointLeftCameraMatrix.at<double>(1,0)=imagePointsLeftCamera[i].y;
        imagePointLeftCameraMatrix.at<double>(2,0)=1;
        cv::Mat rightLineMatrix=fundamentalMatrix*imagePointLeftCameraMatrix;
        
        std::cout<< rightLineMatrix.at<double>(0,0) <<", "<<rightLineMatrix.at<double>(0,1) <<", "<<rightLineMatrix.at<double>(0,2) <<std::endl;

        /////////////////////////////////drawing the line on the image/////////////////////////////////
        /*ax+by+c=0*/
        double x0,y0,x1,y1;
        x0=0;
        y0=(-c-a*x0)/b;
	x1=rightImageRGB.cols;
        y1=(-c-a*x1)/b;

	std::cout<<"error: "<< a*imagePointsRightCamera.at(i).x+ b*imagePointsRightCamera.at(i).y +c<<std::endl;
	cv::line(rightImageRGB, cvPoint(x0,y0), cvPoint(x1,y1), cvScalar(0,255,0), 1);
    }

    cv::imwrite("leftImageEpipolarLine.jpg",leftImageRGB);

FM_7POINT 7-point algorithm

FM_8POINT 8-point algorithm

FM_LMEDS least-median algorithm. 7-point algorithm is used.

FM_RANSAC ANSAC algorithm. It needs at least 15 points. 7-point algorithm is used

cv::Mat fundamentalMatrix= cv::findFundamentalMat(imagePointsLeftCamera, imagePointsRightCamera, cv::FM_8POINT);

std::vector<cv::Vec3d> leftLines, rightLines;

cv::computeCorrespondEpilines(imagePointsLeftCamera, 1, fundamentalMatrix, rightLines);

cv::computeCorrespondEpilines(imagePointsRightCamera, 2, fundamentalMatrix, leftLines);

cv::Mat leftImageRGB(leftImage.size(), CV_8UC3);

cv::cvtColor(leftImage, leftImageRGB, CV_GRAY2RGB);

cv::Mat rightImageRGB(rightImage.size(), CV_8UC3);

cv::cvtColor(rightImage, rightImageRGB, CV_GRAY2RGB);

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

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

{

cv::Vec3d l=rightLines.at(i);

double a=l.val[0];

double b=l.val[1];

double c=l.val[2];

std::cout<<"------------------------a,b,c Using OpenCV (ax+by+c=0)------------------------------"<<std::endl;

std::cout<< a <<", "<<b <<", "<<c <<std::endl;

std::cout<<"------------------------calculating a,b,c (ax+by+c=0) ------------------------------"<<std::endl;

imagePointLeftCameraMatrix.at<double>(0,0)=imagePointsLeftCamera[i].x;

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

imagePointLeftCameraMatrix.at<double>(2,0)=1;

cv::Mat rightLineMatrix=fundamentalMatrix*imagePointLeftCameraMatrix;

std::cout<< rightLineMatrix.at<double>(0,0) <<", "<<rightLineMatrix.at<double>(0,1) <<", "<<rightLineMatrix.at<double>(0,2) <<std::endl;

/////////////////////////////////drawing the line on the image/////////////////////////////////

/*ax+by+c=0*/

double x0,y0,x1,y1;

x0=0;

y0=(-c-a*x0)/b;

x1=rightImageRGB.cols;

y1=(-c-a*x1)/b;

std::cout<<"error: "<< a*imagePointsRightCamera.at(i).x+ b*imagePointsRightCamera.at(i).y +c<<std::endl;

cv::line(rightImageRGB, cvPoint(x0,y0), cvPoint(x1,y1), cvScalar(0,255,0), 1);

}

cv::imwrite("leftImageEpipolarLine.jpg",leftImageRGB);

Computing Fundamental Matrix and Drawing Epipolar Lines for Stereo Vision Cameras in OpenCV Read More »

Creating a Simulated Stereo Vision Cameras With OpenCV and C++

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

n this tutorial, I created an ellipsoid in 3D space and create two cameras in right and left and captured the image:

cv::Mat leftCameraRotation,rightCameraRotation;
	double rollLeft, pitchLeft,  yawLeft,rollRight, pitchRight,  yawRight ,txLeft,tyLeft,tzLeft,txRight,tyRight,tzRight;

	cv::Vec3d thetaLeft,thetaRight;

	rollLeft=0 ;
	pitchLeft=+M_PI/10;
	yawLeft=0;
	
	thetaLeft[0]=rollLeft;
	thetaLeft[1]=pitchLeft;
	thetaLeft[2]=yawLeft;
	
	rollRight=0;
	pitchRight= -M_PI/10;
	yawRight= 0;
		
	thetaRight[0]=rollRight;
	thetaRight[1]=pitchRight;
	thetaRight[2]=yawRight;
	
	txLeft=-1;
	tyLeft=0.0;
	tzLeft=-4.0;
		
	txRight=1.0;
	tyRight=0.0;
	tzRight=-4.0;
	
	leftCameraRotation =eulerAnglesToRotationMatrix(thetaLeft);
	rightCameraRotation =eulerAnglesToRotationMatrix(thetaRight);
		
	cv::Mat leftCameraTranslation = (cv::Mat_<double>(3,1) <<txLeft,tyLeft,tzLeft);
	cv::Mat rightCameraTranslation = (cv::Mat_<double>(3,1) <<txRight,tyRight,tzRight);
	
	std::vector<cv::Point3d> objectPointsInWorldCoordinate;
	double X,Y,Z,radius;
	

////////////////////////////////////////creating ellipsoid in the world coordinate///////////////////////	
	double phiStepSize,thetaStepSize;
	phiStepSize=0.7;
	thetaStepSize=0.6;
	double a,b,c;
	a=2;
	b=3;
	c=1.6;
	for(double phi=-M_PI;phi<M_PI;phi=phi+phiStepSize)
	{
		for(double theta=-M_PI/2;theta<M_PI/2;theta=theta+thetaStepSize)
		{
			X=a*cos(theta)*cos(phi);
			Y=b*cos(theta)*sin(phi);
			Z=c*sin(theta);
			objectPointsInWorldCoordinate.push_back(cv::Point3d(X, Y, Z));
		}
	}

	int numberOfPixelInHeight,numberOfPixelInWidth;
	double heightOfSensor, widthOfSensor;
	double focalLength=2.0;
	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 K = (cv::Mat_<double>(3,3) <<
	focalLength*mx, 0, V0,
	0,focalLength*my,U0,
	0,0,1);
	
	std::vector<cv::Point2d> imagePointsLeftCamera,imagePointsRightCamera;
	cv::projectPoints(objectPointsInWorldCoordinate, leftCameraRotation, leftCameraTranslation, K, cv::noArray(), imagePointsLeftCamera);
	cv::projectPoints(objectPointsInWorldCoordinate, rightCameraRotation, rightCameraTranslation, K, cv::noArray(), imagePointsRightCamera);

////////////////////////////////////////////////storing images from right and left camera//////////////////////////////////////////////

	std::string fileName;
	cv::Mat rightImage,leftImage;
	int U,V;
	leftImage=cv::Mat::zeros(numberOfPixelInHeight,numberOfPixelInWidth,CV_8UC1);

	for(std::size_t i=0;i<imagePointsLeftCamera.size();i++)
	{
		V=int(imagePointsLeftCamera.at(i).x);
		U=int(imagePointsLeftCamera.at(i).y);
		leftImage.at<char>(U,V)=255;
	}

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

	rightImage=cv::Mat::zeros(numberOfPixelInHeight,numberOfPixelInWidth,CV_8UC1);
	for(std::size_t i=0;i<imagePointsRightCamera.size();i++)
	{
		V=int(imagePointsRightCamera.at(i).x);
		U=int(imagePointsRightCamera.at(i).y);
		rightImage.at<char>(U,V)=255;
	}

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

100

101

102

103

104

105

106

107

108

109

110

cv::Mat leftCameraRotation,rightCameraRotation;

double rollLeft, pitchLeft, yawLeft,rollRight, pitchRight, yawRight ,txLeft,tyLeft,tzLeft,txRight,tyRight,tzRight;

cv::Vec3d thetaLeft,thetaRight;

rollLeft=0 ;

pitchLeft=+M_PI/10;

yawLeft=0;

thetaLeft[0]=rollLeft;

thetaLeft[1]=pitchLeft;

thetaLeft[2]=yawLeft;

rollRight=0;

pitchRight= -M_PI/10;

yawRight= 0;

thetaRight[0]=rollRight;

thetaRight[1]=pitchRight;

thetaRight[2]=yawRight;

txLeft=-1;

tyLeft=0.0;

tzLeft=-4.0;

txRight=1.0;

tyRight=0.0;

tzRight=-4.0;

leftCameraRotation =eulerAnglesToRotationMatrix(thetaLeft);

rightCameraRotation =eulerAnglesToRotationMatrix(thetaRight);

cv::Mat leftCameraTranslation = (cv::Mat_<double>(3,1) <<txLeft,tyLeft,tzLeft);

cv::Mat rightCameraTranslation = (cv::Mat_<double>(3,1) <<txRight,tyRight,tzRight);

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

double X,Y,Z,radius;

////////////////////////////////////////creating ellipsoid in the world coordinate///////////////////////

double phiStepSize,thetaStepSize;

phiStepSize=0.7;

thetaStepSize=0.6;

double a,b,c;

a=2;

b=3;

c=1.6;

for(double phi=-M_PI;phi<M_PI;phi=phi+phiStepSize)

{

for(double theta=-M_PI/2;theta<M_PI/2;theta=theta+thetaStepSize)

{

X=a*cos(theta)*cos(phi);

Y=b*cos(theta)*sin(phi);

Z=c*sin(theta);

objectPointsInWorldCoordinate.push_back(cv::Point3d(X, Y, Z));

}

int numberOfPixelInHeight,numberOfPixelInWidth;

double heightOfSensor, widthOfSensor;

double focalLength=2.0;

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 K = (cv::Mat_<double>(3,3) <<

focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1);

std::vector<cv::Point2d> imagePointsLeftCamera,imagePointsRightCamera;

cv::projectPoints(objectPointsInWorldCoordinate, leftCameraRotation, leftCameraTranslation, K, cv::noArray(), imagePointsLeftCamera);

cv::projectPoints(objectPointsInWorldCoordinate, rightCameraRotation, rightCameraTranslation, K, cv::noArray(), imagePointsRightCamera);

////////////////////////////////////////////////storing images from right and left camera//////////////////////////////////////////////

std::string fileName;

cv::Mat rightImage,leftImage;

int U,V;

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

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

{

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

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

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

}

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

cv::imwrite(fileName,leftImage);

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

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

{

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

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

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

}

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

cv::imwrite(fileName,rightImage);

Creating a Simulated Stereo Vision Cameras With OpenCV and C++ 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 »

Stereo Camera Calibration with ROS and OpenCV

5 Comments / Computer Vision, Image Processing, ROS, Tutorials / admin

In this tutorial, I’m gonna show you stereo camera calibration with ROS and OpenCV. So you need a pair of cameras, I bought a pair of this USB webcam which is okay for this task. 1)Save the following text under “stereo_usb_cam_stream_publisher.launch”

<launch>

<arg name="video_device_right" default="/dev/video1" />
<arg name="video_device_left" default="/dev/video2" />

<arg name="image_width" default="640" />
<arg name="image_height" default="480" />

<arg name="right_image_namespace" default="stereo" />
<arg name="left_image_namespace" default="stereo" />

<arg name="right_image_node" default="right" />
<arg name="left_image_node" default="left" />

<arg name="image_topic_name_right" default="image_raw" />
<arg name="camera_info_topic_name_right" default="camera_info" />

<arg name="image_topic_name_left" default="image_raw" />
<arg name="camera_info_topic_name_left" default="camera_info" />



<arg name="left_camera_name" default="left" />
<arg name="right_camera_name" default="right" />

<arg name="left_camera_frame_id" default="left_camera" />
<arg name="right_camera_frame_id" default="right_camera" />

<arg name="framerate" default="30" />

<arg name="camera_info" default="false" />
<arg name="camera_info_path" default="stereo_camera_info" />
	  
<node ns="$(arg right_image_namespace)" name="$(arg right_image_node)" pkg="usb_cam" type="usb_cam_node" output="screen" >
	<param name="video_device" value="$(arg video_device_right)" />
	<param name="image_width" value="$(arg image_width)" />
	<param name="image_height" value="$(arg image_height)"/>
	<param name="pixel_format" value="yuyv" />
	<param name="io_method" value="mmap"/>
	<remap from="/usb_cam/image_raw" to="$(arg image_topic_name_right)" />
	<param name="framerate" value="$(arg framerate)"/> 
	<!-- if camera_info is available, we will use it-->
	<param name="camera_frame_id" value="$(arg right_camera_frame_id)" if="$(arg camera_info)" /> 
	<param name="camera_info_url" value="file://${ROS_HOME}/$(arg camera_info_path)/right.yaml" if="$(arg camera_info)"/>
	<param name="camera_name" value="narrow_stereo/$(arg right_camera_name)" if="$(arg camera_info)"/> 
	<remap from="/usb_cam/camera_info" to="$(arg camera_info_topic_name_right)" if="$(arg camera_info)"/>

</node>

<node ns="$(arg left_image_namespace)" name="$(arg left_image_node)" pkg="usb_cam" type="usb_cam_node" output="screen" >
	<param name="video_device" value="$(arg video_device_left)" />
	<param name="image_width" value="$(arg image_width)" />
	<param name="image_height" value="$(arg image_height)"/>
	<param name="pixel_format" value="yuyv" />
	<param name="io_method" value="mmap"/>
	<param name="framerate" value="$(arg framerate)"/> 
	<remap from="/usb_cam/image_raw" to="$(arg image_topic_name_left)"/>
	<!-- if camera_info is available, we will use it-->
	<param name="camera_frame_id" value="$(arg left_camera_frame_id)" if="$(arg camera_info)"/> 
	<param name="camera_name" value="narrow_stereo/$(arg left_camera_name)" if="$(arg camera_info)"/> 
	<param name="camera_info_url" value="file://${ROS_HOME}/$(arg camera_info_path)/left.yaml" if="$(arg camera_info)"/> 
	<remap from="/usb_cam/camera_info" to="$(arg camera_info_topic_name_left)" if="$(arg camera_info)"/>
</node>
</launch>

</node>

</node>

</launch>

2)Then run the following node to publish both cameras.

roslaunch stereo_usb_cam_stream_publisher.launch video_device_right:=/dev/video1 video_device_left:=/dev/video2 image_width:=640 image_height:=480 left_camera_name:=left right_camera_name:=right

1	roslaunch stereo_usb_cam_stream_publisher.launch video_device_right:=/dev/video1 video_device_left:=/dev/video2 image_width:=640 image_height:=480 left_camera_name:=left right_camera_name:=right

3)Now call the calibration node:

Stereo Camera Calibration with ROS and OpenCV Read More »

How to use OpenCV 3 Contribution Modules

Leave a Comment / C++, Tutorials / admin

OpenCV Contribution Modules are developed code from the community of OpenCV and are not very stable but after they got stable they will be part f OpenCV. Anyhow, there are a lot of cool modules there (fuzzy logic, sfm, cnn, …) that made me have look at them and try them. gflags

git clone https://github.com/gflags/gflags
cd gflags
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=ON -DCMAKE_INSTALL_PREFIX:PATH=~/usr 
cmake --build build -j8
cmake --install build

git clone https://github.com/gflags/gflags

cd gflags

cmake -S . -B build -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=ON -DCMAKE_INSTALL_PREFIX:PATH=~/usr

cmake --build build -j8

cmake --install build

2) glog

How to use OpenCV 3 Contribution Modules Read More »

Computing OpticalFlow with OpenCV

1 Comment / C++, Computer Vision, Tutorials / admin

There are two main approaches for computing optical flow, computing optical flow for all pixel or computing optical flow for corners. 1. Computing optical flow for all pixel 2. Computing optical flow for corners

Computing OpticalFlow with OpenCV Read More »

Finding Homography Matrix using Singular-value Decomposition and RANSAC in OpenCV and Matlab

3 Comments / C++, Computer Vision, Image Processing, Linear Algebra, Matlab, Tutorials / admin

Solving a Homography problem leads to solving a set of homogeneous linear equations such below: \begin{equation} \left( \begin{array}{ccccccccc} -x1 & -y1 & -1 & 0 & 0 & 0 & x1*xp1 & y1*xp1 & xp1\\ 0 & 0 & 0 & -x1 & -y1 & -1 & x1*yp1 & y1*yp1 & yp1\\ -x2 & -y2 &

Finding Homography Matrix using Singular-value Decomposition and RANSAC in OpenCV and Matlab Read More »

Peak Signal-to-Noise Ratio (PSNR) in Image using OpenCV and Matlab

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

Peak signal-to-noise ratio (PSNR) shows the ratio between the maximum possible power of a signal and the power of the same image with noise. PSNR is usually expressed in logarithmic decibel scale. $ MSE =1/m*n \sum_{i=0}^{m-1} \sum_{j=0}^{n-1} [ Image( i,j) -NoisyImage( i,j) ] ^2 $ $ PSNR =10* \log_{10} \Bigg(MAXI^2/MSE\Bigg) $ MSE is Mean Square Error and MAXI is

Peak Signal-to-Noise Ratio (PSNR) in Image using OpenCV and Matlab 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.