Computing Essential and Fundamental Matrix Using OpenCV (8 Points Algorithm) With C++

Leave a reply

void findFundamentalMatrix(std::vector<cv::Point2d>&imagePointsLeftCamera,std::vector<cv::Point2d>&imagePointsRightCamera)
{
    std::vector<cv::Point3d> imagePointsLeftCameraHomogeneous,imagePointsRightCameraHomogeneous;
    cv::convertPointsToHomogeneous(imagePointsLeftCamera,imagePointsLeftCameraHomogeneous);
    cv::convertPointsToHomogeneous(imagePointsRightCamera,imagePointsRightCameraHomogeneous);
/*

   ┌       ┐ ┌f11  f12  f13┐ ┌u┐
   |u` v` 1|*|f21  f22  f23|*|v|=0
   └       ┘ └f31  f32  f33┘ └1┘

   ┌u'1u1   u'1v1   u'1   v'1u1   v'1v1   v'1   u1   v1   1┐   ┌f11┐
   |u'2u2   u'2v2   u'2   v'2u2   v'2v2   v'2   u2   v2   1|   |f12|
   |u'3u3   u'3v3   u'3   v'3u3   v'3v3   v'3   u3   v3   1|   |f13|
   |u'4u4   u'4v4   u'4   v'4u4   v'4v4   v'4   u4   v4   1|   |f21|
   |u'5u5   u'5v5   u'5   v'5u5   v'5v5   v'5   u5   v5   1| * |f22|=0
   |u'6u6   u'6v6   u'6   v'6u6   v'6v6   v'6   u6   v6   1|   |f23|
   |u'7u7   u'7v7   u'7   v'7u7   v'7v7   v'7   u7   v7   1|   |f31|
   └u'8u8   u'8v8   u'8   v'8u8   v'8v8   v'8   u8   v8   1┘   |f32|
                                                               └f33┘
*/
    double u_prime, v_prime, u,v;
    cv::Mat A=cv::Mat_<double>(imagePointsLeftCamera.size(),9);
    for(int i=0;i<imagePointsLeftCamera.size();i++)
    {
        u_prime=imagePointsLeftCamera.at(i).x;
        v_prime=imagePointsLeftCamera.at(i).y;

        u=imagePointsRightCamera.at(i).x;
        v=imagePointsRightCamera.at(i).y;

        A.at<double>(i,0)=u_prime*u;
        A.at<double>(i,1)=u_prime*v;
        A.at<double>(i,2)=u_prime;
        A.at<double>(i,3)=v_prime*u;
        A.at<double>(i,4)=v_prime*v;
        A.at<double>(i,5)=v_prime;
        A.at<double>(i,6)=u;
        A.at<double>(i,7)=v;
        A.at<double>(i,8)=1;

    }

    cv::Mat U,SingularValuesVector , VT;
    cv::Mat SigmaMatrix=cv::Mat::zeros(A.rows,A.cols,CV_64F);
    cv::SVD::compute(A.clone(), SingularValuesVector,U,VT);

//////////////////////////////////Buliding U (Buliding Square Matrix U)///////////////////////////////////

    cv::Mat completeU=cv::Mat_<double>(U.rows,U.rows);
    cv::Mat missingElementsOfU=cv::Mat::zeros(U.rows,U.rows-U.cols,CV_64F);
    cv::hconcat(U,missingElementsOfU,completeU);

//////////////////////////////////Buliding Sigma Matrix ///////////////////////////////////

    cv::Mat completeSigma=cv::Mat::zeros(completeU.cols,VT.rows,CV_64F);
    for(std::size_t i=0;i<SingularValuesVector.rows;i++)
    {
        completeSigma.at<double>(i,i)=SingularValuesVector.at<double>(i,0);
    }


//////////////////////////////////Checking A=completeU*completeSigma*Vt ///////////////////////////////////

    std::cout<< "checking A-U*Sigma*VT=0"  <<std::endl;
    std::cout<< cv::sum(A-completeU*completeSigma*VT).val[0] <<std::endl;

///////////////////////////////////Building F Matrix From F vector /////////////////////////////////////////////
    cv::Mat F_vec=VT.col(VT.cols-1  );
    std::cout<< F_vec.cols<<std::endl;
    cv::Mat F=cv::Mat(3,3,cv::DataType<double>::type);

    F.at<double>(0,0)=F_vec.at<double>(0,0);
    F.at<double>(0,1)=F_vec.at<double>(1,0);
    F.at<double>(0,2)=F_vec.at<double>(2,0);
    F.at<double>(1,0)=F_vec.at<double>(3,0);
    F.at<double>(1,1)=F_vec.at<double>(4,0);
    F.at<double>(1,2)=F_vec.at<double>(5,0);
    F.at<double>(2,0)=F_vec.at<double>(6,0);
    F.at<double>(2,1)=F_vec.at<double>(7,0);
    F.at<double>(2,2)=F_vec.at<double>(8,0);

///////////////////////////////////Computing SVD of F /////////////////////////////////////////////

    cv::SVD::compute(F.clone(), SingularValuesVector,U,VT);
    std::cout<< "F singular values" <<std::endl;
    std::cout<< SingularValuesVector <<std::endl;

///////////////////////////////////Setting The Smallest Eigen Value to Zero/////////////////////////////////////////////
    SingularValuesVector.at<double>(SingularValuesVector.rows-1,0)=0;

//////////////////////////////////Buliding U (Buliding Square Matrix U)///////////////////////////////////

    completeU=cv::Mat_<double>(U.rows,U.rows);
    missingElementsOfU=cv::Mat::zeros(U.rows,U.rows-U.cols,CV_64F);
    cv::hconcat(U,missingElementsOfU,completeU);

//////////////////////////////////Buliding Sigma Matrix ///////////////////////////////////

    completeSigma=cv::Mat::zeros(completeU.cols,VT.rows,CV_64F);
    for(std::size_t i=0;i<SingularValuesVector.rows;i++)
    {
        completeSigma.at<double>(i,i)=SingularValuesVector.at<double>(i,0);
    }
/////////////////////////////////////Building New F matrix///////////////////////////////////////

    cv::Mat NewF=completeU*completeSigma*VT;
    std::cout<< "Fundamental Matrix is:"<<std::endl;
    std::cout<< NewF<<std::endl;
}

100

101

102

103

104

105

106

107

108

109

110

void findFundamentalMatrix(std::vector<cv::Point2d>&imagePointsLeftCamera,std::vector<cv::Point2d>&imagePointsRightCamera)

{

std::vector<cv::Point3d> imagePointsLeftCameraHomogeneous,imagePointsRightCameraHomogeneous;

cv::convertPointsToHomogeneous(imagePointsLeftCamera,imagePointsLeftCameraHomogeneous);

cv::convertPointsToHomogeneous(imagePointsRightCamera,imagePointsRightCameraHomogeneous);

┌ ┐ ┌f11 f12 f13┐ ┌u┐

|u` v` 1|*|f21 f22 f23|*|v|=0

└ ┘ └f31 f32 f33┘ └1┘

┌u'1u1 u'1v1 u'1 v'1u1 v'1v1 v'1 u1 v1 1┐ ┌f11┐

|u'2u2 u'2v2 u'2 v'2u2 v'2v2 v'2 u2 v2 1| |f12|

|u'3u3 u'3v3 u'3 v'3u3 v'3v3 v'3 u3 v3 1| |f13|

|u'4u4 u'4v4 u'4 v'4u4 v'4v4 v'4 u4 v4 1| |f21|

|u'5u5 u'5v5 u'5 v'5u5 v'5v5 v'5 u5 v5 1| * |f22|=0

|u'6u6 u'6v6 u'6 v'6u6 v'6v6 v'6 u6 v6 1| |f23|

|u'7u7 u'7v7 u'7 v'7u7 v'7v7 v'7 u7 v7 1| |f31|

└u'8u8 u'8v8 u'8 v'8u8 v'8v8 v'8 u8 v8 1┘ |f32|

└f33┘

double u_prime, v_prime, u,v;

cv::Mat A=cv::Mat_<double>(imagePointsLeftCamera.size(),9);

for(int i=0;i<imagePointsLeftCamera.size();i++)

{

u_prime=imagePointsLeftCamera.at(i).x;

v_prime=imagePointsLeftCamera.at(i).y;

u=imagePointsRightCamera.at(i).x;

v=imagePointsRightCamera.at(i).y;

A.at<double>(i,0)=u_prime*u;

A.at<double>(i,1)=u_prime*v;

A.at<double>(i,2)=u_prime;

A.at<double>(i,3)=v_prime*u;

A.at<double>(i,4)=v_prime*v;

A.at<double>(i,5)=v_prime;

A.at<double>(i,6)=u;

A.at<double>(i,7)=v;

A.at<double>(i,8)=1;

}

cv::Mat U,SingularValuesVector , VT;

cv::Mat SigmaMatrix=cv::Mat::zeros(A.rows,A.cols,CV_64F);

cv::SVD::compute(A.clone(), SingularValuesVector,U,VT);

//////////////////////////////////Buliding U (Buliding Square Matrix U)///////////////////////////////////

cv::Mat completeU=cv::Mat_<double>(U.rows,U.rows);

cv::Mat missingElementsOfU=cv::Mat::zeros(U.rows,U.rows-U.cols,CV_64F);

cv::hconcat(U,missingElementsOfU,completeU);

//////////////////////////////////Buliding Sigma Matrix ///////////////////////////////////

cv::Mat completeSigma=cv::Mat::zeros(completeU.cols,VT.rows,CV_64F);

for(std::size_t i=0;i<SingularValuesVector.rows;i++)

{

completeSigma.at<double>(i,i)=SingularValuesVector.at<double>(i,0);

}

//////////////////////////////////Checking A=completeU*completeSigma*Vt ///////////////////////////////////

std::cout<< "checking A-U*Sigma*VT=0" <<std::endl;

std::cout<< cv::sum(A-completeU*completeSigma*VT).val[0] <<std::endl;

///////////////////////////////////Building F Matrix From F vector /////////////////////////////////////////////

cv::Mat F_vec=VT.col(VT.cols-1 );

std::cout<< F_vec.cols<<std::endl;

cv::Mat F=cv::Mat(3,3,cv::DataType<double>::type);

F.at<double>(0,0)=F_vec.at<double>(0,0);

F.at<double>(0,1)=F_vec.at<double>(1,0);

F.at<double>(0,2)=F_vec.at<double>(2,0);

F.at<double>(1,0)=F_vec.at<double>(3,0);

F.at<double>(1,1)=F_vec.at<double>(4,0);

F.at<double>(1,2)=F_vec.at<double>(5,0);

F.at<double>(2,0)=F_vec.at<double>(6,0);

F.at<double>(2,1)=F_vec.at<double>(7,0);

F.at<double>(2,2)=F_vec.at<double>(8,0);

///////////////////////////////////Computing SVD of F /////////////////////////////////////////////

cv::SVD::compute(F.clone(), SingularValuesVector,U,VT);

std::cout<< "F singular values" <<std::endl;

std::cout<< SingularValuesVector <<std::endl;

///////////////////////////////////Setting The Smallest Eigen Value to Zero/////////////////////////////////////////////

SingularValuesVector.at<double>(SingularValuesVector.rows-1,0)=0;

//////////////////////////////////Buliding U (Buliding Square Matrix U)///////////////////////////////////

completeU=cv::Mat_<double>(U.rows,U.rows);

missingElementsOfU=cv::Mat::zeros(U.rows,U.rows-U.cols,CV_64F);

cv::hconcat(U,missingElementsOfU,completeU);

//////////////////////////////////Buliding Sigma Matrix ///////////////////////////////////

completeSigma=cv::Mat::zeros(completeU.cols,VT.rows,CV_64F);

for(std::size_t i=0;i<SingularValuesVector.rows;i++)

{

completeSigma.at<double>(i,i)=SingularValuesVector.at<double>(i,0);

}

/////////////////////////////////////Building New F matrix///////////////////////////////////////

cv::Mat NewF=completeU*completeSigma*VT;

std::cout<< "Fundamental Matrix is:"<<std::endl;

std::cout<< NewF<<std::endl;

}

Decomposing Projection Using OpenCV and C++

Leave a reply

#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]

Camera Projection Matrix with Eigen

Leave a reply

void projectFromCameraCoordinateToCameraPlane( Eigen::Matrix3Xd &pointsInCameraCoordinate, Eigen::Matrix3d &cameraIntrinsicMatrix,Eigen::Matrix2Xd &pointsInCameraPlane)
{
/*
Intrinsic camera Matrix is something like this:

focalLength  ===> unit is mm
mx=  ==>unit is Pixel/mm
U0=  ===> unit is Pixel

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

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

Gamma=0

X,Y,Z,W are homogeneous position of point in camera coordinate

┌ ┐	 ┌                         	 ┐┌ ┐
v`	 |f*mx		Gamma		V0	0||X|	
u`	=|0			f*my		U0	0||Y|
w`	 |0			0			 1	0||Z|
└ ┘	 └                         	 ┘└W┘

									 ▲y
									 |	
			----►V					 |	
			|	 ____________________|_____________________
			|	|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
		U	▼	|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|____________► x
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|
				|__|__|__|__|__|__|__|__|__|__|__|__|__|__|



To project points from camera coordinate to camera plane:

V`=X*f*mx + V0*Z 
U`=Y*f*my + U0*Z
W`=Z

U=U`/W` ,V=V`/W`


V=f*mx*X/Z + V0 
U=f*my*Y/Z + U0

(U,V) is the index of corresponding point in the image
*/
	
	Eigen::Matrix3Xd pointsInCameraPlaneHomogeneous(pointsInCameraCoordinate.rows(),pointsInCameraCoordinate.cols());	

	pointsInCameraPlaneHomogeneous=cameraIntrinsicMatrix*pointsInCameraCoordinate;
	std::cout<<"pointsInCameraPlaneHomogeneous"<<std::endl;	
 	std::cout<<pointsInCameraPlaneHomogeneous<<std::endl;	
	for(int i=0;i<pointsInCameraPlaneHomogeneous.cols();i++  )
	{
		pointsInCameraPlaneHomogeneous(0,i)=pointsInCameraPlaneHomogeneous(0,i)/pointsInCameraPlaneHomogeneous(2,i);
		pointsInCameraPlaneHomogeneous(1,i)=pointsInCameraPlaneHomogeneous(1,i)/pointsInCameraPlaneHomogeneous(2,i);
	}
	pointsInCameraPlane=pointsInCameraPlaneHomogeneous.block(0,0,pointsInCameraPlaneHomogeneous.rows()-1,pointsInCameraPlaneHomogeneous.cols());
}

void transformPoints(Eigen::Matrix3Xd &points, Eigen::Matrix3d &rotationMatrix, Eigen::Vector3d &translationVector,Eigen::Matrix3Xd &transformedPoints)
{
	transformedPoints=(rotationMatrix*points).colwise() + translationVector ;
}

void projectFromCameraCoordinateToCameraPlane_Example()
{
	
/*
 OpenCV coordinate system
                  ▲
                 /
                /
              Z/
              /
             /
------------------------------► X
            |
            |
            |
            |
          Y |
            ▼
*/	
	
	int numberOfPixelInHeight,numberOfPixelInWidth;
	double heightOfSensor, widthOfSensor;
	double focalLength=1.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;
	
	double L = 0.2;
	Eigen::Matrix3Xd controlPointsInWorldCoordinate(3,6);
	Eigen::Matrix3Xd controlPointsInCameraCoordinate(3,6);

	
	Eigen::Matrix3d cameraIntrinsicMatrix;
	cameraIntrinsicMatrix<<focalLength*mx, 0, V0,
						   0,focalLength*my,U0,
						   0,0,1;

	controlPointsInWorldCoordinate.col(0)= Eigen::Vector3d(-L, -L, 0);
	controlPointsInWorldCoordinate.col(1)= Eigen::Vector3d(2 * L, -L, 0.2);
	controlPointsInWorldCoordinate.col(2)= Eigen::Vector3d(L, L, 0.2);
	controlPointsInWorldCoordinate.col(3)= Eigen::Vector3d(-L, L, 0);
	controlPointsInWorldCoordinate.col(4)= Eigen::Vector3d(-2 * L, L, 0);
	controlPointsInWorldCoordinate.col(5)= Eigen::Vector3d(0, 0, 0.5);
	
	
	Eigen::Matrix3d rotationMatrix;
	rotationMatrix<< 1,0,0
					,0,1,0
					,0,0,1;

	Eigen::Vector3d translationVector(3,1);
	translationVector<<-0.1, 0.1, 1.2;
	
	transformPoints(controlPointsInWorldCoordinate, rotationMatrix, translationVector, controlPointsInCameraCoordinate );

	Eigen::Matrix2Xd pointsInCameraPlane(2,controlPointsInCameraCoordinate.cols());
	projectFromCameraCoordinateToCameraPlane(controlPointsInCameraCoordinate,cameraIntrinsicMatrix,pointsInCameraPlane);
    
    
    std::vector<cv::Point3d> cvPointsInCameraCoordinate;
    std::vector<cv::Point2d> cvpointsInCameraPlane;
    cvPointsInCameraCoordinate.push_back(cv::Point3d(-L, -L, 0));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(2 * L, -L, 0.2));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(L, L, 0.2));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(-L, L, 0));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(-2 * L, L, 0));
    cvPointsInCameraCoordinate.push_back(cv::Point3d(0, 0, 0.5));


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


    cv::Mat rvec= cv::Mat::eye(3,3, CV_64F);
    cv::Mat tvec=(cv::Mat_<double>(3,1)<<-0.1, 0.1, 1.2);
    cv::projectPoints(cvPointsInCameraCoordinate,rvec,tvec,cameraMatrix,cv::noArray(),cvpointsInCameraPlane);
	
    std::cout<<"cameraIntrinsicMatrix" <<std::endl;
    std::cout<<cameraIntrinsicMatrix <<std::endl;

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

	
	
    Eigen::MatrixXd image(numberOfPixelInHeight,numberOfPixelInWidth);
    image=MatrixXd::Zero(numberOfPixelInHeight,numberOfPixelInWidth);
    int U,V;
    for(int i=0;i<pointsInCameraPlane.cols();i++)
    {
        U=int(pointsInCameraPlane(0,i));
        V=int(pointsInCameraPlane(1,i));
        std::cout<<U<<","<<V <<std::endl;
        image(U,V)=255;
        std::cout<<cvpointsInCameraPlane.at(i)<<std::endl;

    }
    Mat dst;
    eigen2cv(image, dst);
    std::string fileName=std::string("eigen_camera_projection_result_focal_")+std::to_string(focalLength)+ std::string("_.jpg");
    cv::imwrite(fileName,dst);
}

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

void projectFromCameraCoordinateToCameraPlane( Eigen::Matrix3Xd &pointsInCameraCoordinate, Eigen::Matrix3d &cameraIntrinsicMatrix,Eigen::Matrix2Xd &pointsInCameraPlane)

{

Intrinsic camera Matrix is something like this:

focalLength ===> unit is mm

mx= ==>unit is Pixel/mm

U0= ===> unit is Pixel

my=-(numberOfPixelInHeight)/heightOfSensor ;

U0=(numberOfPixelInHeight)/2 ;

mx=(numberOfPixelInWidth)/heightOfSensor;

V0=(numberOfPixelInWidth)/2;

Gamma=0

X,Y,Z,W are homogeneous position of point in camera coordinate

┌ ┐ ┌ ┐┌ ┐

v` |f*mx Gamma V0 0||X|

u` =|0 f*my U0 0||Y|

w` |0 0 1 0||Z|

└ ┘ └ ┘└W┘

▲y

----►V |

| ____________________|_____________________

| |__|__|__|__|__|__|__|__|__|__|__|__|__|__|

U ▼ |__|__|__|__|__|__|__|__|__|__|__|__|__|__|

|__|__|__|__|__|__|__|__|__|__|__|__|__|__|

|__|__|__|__|__|__|__|__|__|__|__|__|__|__|____________► x

|__|__|__|__|__|__|__|__|__|__|__|__|__|__|

To project points from camera coordinate to camera plane:

V`=X*f*mx + V0*Z

U`=Y*f*my + U0*Z

W`=Z

U=U`/W` ,V=V`/W`

V=f*mx*X/Z + V0

U=f*my*Y/Z + U0

(U,V) is the index of corresponding point in the image

Eigen::Matrix3Xd pointsInCameraPlaneHomogeneous(pointsInCameraCoordinate.rows(),pointsInCameraCoordinate.cols());

pointsInCameraPlaneHomogeneous=cameraIntrinsicMatrix*pointsInCameraCoordinate;

std::cout<<"pointsInCameraPlaneHomogeneous"<<std::endl;

std::cout<<pointsInCameraPlaneHomogeneous<<std::endl;

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

{

pointsInCameraPlaneHomogeneous(0,i)=pointsInCameraPlaneHomogeneous(0,i)/pointsInCameraPlaneHomogeneous(2,i);

pointsInCameraPlaneHomogeneous(1,i)=pointsInCameraPlaneHomogeneous(1,i)/pointsInCameraPlaneHomogeneous(2,i);

}

pointsInCameraPlane=pointsInCameraPlaneHomogeneous.block(0,0,pointsInCameraPlaneHomogeneous.rows()-1,pointsInCameraPlaneHomogeneous.cols());

}

void transformPoints(Eigen::Matrix3Xd &points, Eigen::Matrix3d &rotationMatrix, Eigen::Vector3d &translationVector,Eigen::Matrix3Xd &transformedPoints)

{

transformedPoints=(rotationMatrix*points).colwise() + translationVector ;

}

void projectFromCameraCoordinateToCameraPlane_Example()

{

OpenCV coordinate system

▲

------------------------------► X

Y |

▼

int numberOfPixelInHeight,numberOfPixelInWidth;

double heightOfSensor, widthOfSensor;

double focalLength=1.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;

double L = 0.2;

Eigen::Matrix3Xd controlPointsInWorldCoordinate(3,6);

Eigen::Matrix3Xd controlPointsInCameraCoordinate(3,6);

Eigen::Matrix3d cameraIntrinsicMatrix;

cameraIntrinsicMatrix<<focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1;

controlPointsInWorldCoordinate.col(0)= Eigen::Vector3d(-L, -L, 0);

controlPointsInWorldCoordinate.col(1)= Eigen::Vector3d(2 * L, -L, 0.2);

controlPointsInWorldCoordinate.col(2)= Eigen::Vector3d(L, L, 0.2);

controlPointsInWorldCoordinate.col(3)= Eigen::Vector3d(-L, L, 0);

controlPointsInWorldCoordinate.col(4)= Eigen::Vector3d(-2 * L, L, 0);

controlPointsInWorldCoordinate.col(5)= Eigen::Vector3d(0, 0, 0.5);

Eigen::Matrix3d rotationMatrix;

rotationMatrix<< 1,0,0

,0,1,0

,0,0,1;

Eigen::Vector3d translationVector(3,1);

translationVector<<-0.1, 0.1, 1.2;

transformPoints(controlPointsInWorldCoordinate, rotationMatrix, translationVector, controlPointsInCameraCoordinate );

Eigen::Matrix2Xd pointsInCameraPlane(2,controlPointsInCameraCoordinate.cols());

projectFromCameraCoordinateToCameraPlane(controlPointsInCameraCoordinate,cameraIntrinsicMatrix,pointsInCameraPlane);

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

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

cvPointsInCameraCoordinate.push_back(cv::Point3d(-L, -L, 0));

cvPointsInCameraCoordinate.push_back(cv::Point3d(2 * L, -L, 0.2));

cvPointsInCameraCoordinate.push_back(cv::Point3d(L, L, 0.2));

cvPointsInCameraCoordinate.push_back(cv::Point3d(-L, L, 0));

cvPointsInCameraCoordinate.push_back(cv::Point3d(-2 * L, L, 0));

cvPointsInCameraCoordinate.push_back(cv::Point3d(0, 0, 0.5));

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

focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1);

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

cv::Mat tvec=(cv::Mat_<double>(3,1)<<-0.1, 0.1, 1.2);

cv::projectPoints(cvPointsInCameraCoordinate,rvec,tvec,cameraMatrix,cv::noArray(),cvpointsInCameraPlane);

std::cout<<"cameraIntrinsicMatrix" <<std::endl;

std::cout<<cameraIntrinsicMatrix <<std::endl;

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

Eigen::MatrixXd image(numberOfPixelInHeight,numberOfPixelInWidth);

image=MatrixXd::Zero(numberOfPixelInHeight,numberOfPixelInWidth);

int U,V;

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

{

U=int(pointsInCameraPlane(0,i));

V=int(pointsInCameraPlane(1,i));

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

image(U,V)=255;

std::cout<<cvpointsInCameraPlane.at(i)<<std::endl;

}

Mat dst;

eigen2cv(image, dst);

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

cv::imwrite(fileName,dst);

}

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

Leave a reply

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

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

Leave a reply

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

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

Leave a reply

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

Point cloud of 3D Ellipsoid
Image from right camera.
Image from left camera.

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

How to use image_geometry and camera_info_manager in ROS

Leave a reply

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)

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

Leave a reply

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;

}

Publishing video file and images with ROS

Leave a reply

If you have video/image file on your disk and you want to publish it without coding, you can use the following nodes:

Publishing Images

Here are some examples to publish an image:

rosrun image_publisher image_publisher <path_to_image_file>

1	rosrun image_publisher image_publisher <path_to_image_file>

And more Advanced launch file:

<!--
Example of run:
roslaunch image_publisher.launch image_file:=image.png
-->
<launch>

<arg name="image_file" default="/opt/ros/kinetic/share/turtlesim/images/kinetic.png" />
<node pkg="image_publisher" type="image_publisher" name="image_publisher" args="$(arg image_file)">
<param name="frame_id" value="camera_frame" />
<param name="publish_rate" value="1" />
<param name="camera_info_url" value="file:///$(env HOME)/.ros/camera_info/camera.yaml" />
</node>
</launch>

<!--

Example of run:

roslaunch image_publisher.launch image_file:=image.png

-->

</node>

</launch>

Publishing Videos

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

Leave a reply