Tutorials Archives - Page 4 of 13

Eigen Arrays, Matrices and Vectors: Definition, Initialization Resizing, Populating and Coefficient Wise Operations

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

    std::cout <<"/////////////////Definition////////////////"<<std::endl;

/*
    Definition of vectors and matrices in Eigen comes in the following form:

    Eigen::MatrixSizeType
    Eigen::VectorSizeType
    Eigen::ArraySizeType

    Size can be 2,3,4 for fixed size square matrices or X for dynamic size
    Type can be:
    i for integer,
    f for float,
    d for double,
    c for complex,
    cf for complex float,
    cd for complex double.
*/

    // Vector3f is a fixed column vector of 3 floats:
    Eigen::Vector3f objVector3f;

    // RowVector2i is a fixed row vector of 3 integer:
    Eigen::RowVector2i objRowVector2i;

    // VectorXf is a column vector of size 10 floats:
    Eigen::VectorXf objv(10);


    Eigen::Matrix4d m; // 4x4 double

    Eigen::Matrix4cd objMatrix4cd; // 4x4 double complex


    //a is a 3x3 matrix, with a static float[9] array of uninitialized coefficients,
    Eigen::Matrix3f a;

    //b is a dynamic-size matrix whose size is currently 0x0, and whose array of coefficients hasn't yet been allocated at all.
    Eigen::MatrixXf b;

    //A is a 10x15 dynamic-size matrix, with allocated but currently uninitialized coefficients.
    Eigen::MatrixXf A(10, 15);

    //V is a dynamic-size vector of size 30, with allocated but currently uninitialized coefficients.
    Eigen::VectorXf V(30);


    //Template style definition
    // Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>  &matrix

    Eigen::Matrix<double, 2, 3> my_matrix;
    my_matrix << 1, 2, 3, 4, 5, 6;

    // ArrayXf
    Eigen::Array<float, Eigen::Dynamic, 1> a1;
    // Array3f
    Eigen::Array<float, 3, 1> a2;
    // ArrayXXd
    Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic> a3;
    // Array33d
    Eigen::Array<double, 3, 3> a4;
    Eigen::Matrix3d matrix_from_array = a4.matrix();


    std::cout <<"///////////////////Initialization//////////////////"<< std::endl;

    Eigen::Matrix2d a_2d;
    a_2d.setRandom();

    a_2d.setConstant(4.3);

    Eigen::MatrixXd identity=Eigen::MatrixXd::Identity(6,6);

    Eigen::MatrixXd zeros=Eigen::MatrixXd::Zero(3, 3);

    Eigen::ArrayXXf table(10, 4);
    table.col(0) = Eigen::ArrayXf::LinSpaced(10, 0, 90);


    std::cout <<"/////////////Matrix Coefficient Wise Operations///////////////////"<< std::endl;
    int i,j;
    std::cout << my_matrix << std::endl;
    std::cout << my_matrix.transpose()<< std::endl;

    std::cout<<my_matrix.minCoeff(&i, &j)<<std::endl;
    std::cout<<my_matrix.maxCoeff(&i, &j)<<std::endl;
    std::cout<<my_matrix.prod()<<std::endl;
    std::cout<<my_matrix.sum()<<std::endl;
    std::cout<<my_matrix.mean()<<std::endl;
    std::cout<<my_matrix.trace()<<std::endl;
    std::cout<<my_matrix.colwise().mean()<<std::endl;
    std::cout<<my_matrix.rowwise().maxCoeff()<<std::endl;
    std::cout<<my_matrix.lpNorm<2>()<<std::endl;
    std::cout<<my_matrix.lpNorm<Eigen::Infinity>()<<std::endl;
    std::cout<<(my_matrix.array()>0).all()<<std::endl;// if all elemnts are positive
    std::cout<<(my_matrix.array()>2).any()<<std::endl;//if any element is greater than 2
    std::cout<<(my_matrix.array()>1).count()<<std::endl;// count the number of elements greater than 1
    std::cout << my_matrix.array() - 2 << std::endl;
    std::cout << my_matrix.array().abs() << std::endl;
    std::cout << my_matrix.array().square() << std::endl;
    std::cout << my_matrix.array() * my_matrix.array() << std::endl;
    std::cout << my_matrix.array().exp() << std::endl;
    std::cout << my_matrix.array().log() << std::endl;
    std::cout << my_matrix.array().sqrt() << std::endl;

    std::cout <<"//////////////////Block Elements Access////////////////////"<< std::endl;
    //Block of size (p,q), starting at (i,j)	matrix.block(i,j,p,q)
    Eigen::MatrixXf mat(4, 4);
    mat << 1, 2, 3, 4,
        5, 6, 7, 8,
        9, 10, 11, 12,
        13, 14, 15, 16;
    std::cout << "Block in the middle" << std::endl;
    std::cout << mat.block<2, 2>(1, 1) << std::endl;

    for (int i = 1; i <= 3; ++i)
    {
        std::cout << "Block of size " << i << "x" << i << std::endl;
        std::cout << mat.block(0, 0, i, i) << std::endl;
    }


    /////////////build matrix from vector, resizing matrix, dynamic size ////////////////
    Eigen::MatrixXd dynamicMatrix;
    int rows, cols;
    rows=3;
    cols=2;
    dynamicMatrix.resize(rows,cols);
    dynamicMatrix<<-1,7,3,4,5,1;

    //If you want a conservative variant of resize() which does not change the coefficients, use conservativeResize()
    dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);
    dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector3d(1, 4, 0);

    dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);
    dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector3d(5, -8, 6);
/*
    you should expect this:
     1  7  1  5
     3  4  4 -8
     5  1  0  6

*/
    std::cout<< dynamicMatrix<<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

std::cout <<"/////////////////Definition////////////////"<<std::endl;

Definition of vectors and matrices in Eigen comes in the following form:

Eigen::MatrixSizeType

Eigen::VectorSizeType

Eigen::ArraySizeType

Size can be 2,3,4 for fixed size square matrices or X for dynamic size

Type can be:

i for integer,

f for float,

d for double,

c for complex,

cf for complex float,

cd for complex double.

// Vector3f is a fixed column vector of 3 floats:

Eigen::Vector3f objVector3f;

// RowVector2i is a fixed row vector of 3 integer:

Eigen::RowVector2i objRowVector2i;

// VectorXf is a column vector of size 10 floats:

Eigen::VectorXf objv(10);

Eigen::Matrix4d m; // 4x4 double

Eigen::Matrix4cd objMatrix4cd; // 4x4 double complex

//a is a 3x3 matrix, with a static float[9] array of uninitialized coefficients,

Eigen::Matrix3f a;

//b is a dynamic-size matrix whose size is currently 0x0, and whose array of coefficients hasn't yet been allocated at all.

Eigen::MatrixXf b;

//A is a 10x15 dynamic-size matrix, with allocated but currently uninitialized coefficients.

Eigen::MatrixXf A(10, 15);

//V is a dynamic-size vector of size 30, with allocated but currently uninitialized coefficients.

Eigen::VectorXf V(30);

//Template style definition

// Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> &matrix

Eigen::Matrix<double, 2, 3> my_matrix;

my_matrix << 1, 2, 3, 4, 5, 6;

// ArrayXf

Eigen::Array<float, Eigen::Dynamic, 1> a1;

// Array3f

Eigen::Array<float, 3, 1> a2;

// ArrayXXd

Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic> a3;

// Array33d

Eigen::Array<double, 3, 3> a4;

Eigen::Matrix3d matrix_from_array = a4.matrix();

std::cout <<"///////////////////Initialization//////////////////"<< std::endl;

Eigen::Matrix2d a_2d;

a_2d.setRandom();

a_2d.setConstant(4.3);

Eigen::MatrixXd identity=Eigen::MatrixXd::Identity(6,6);

Eigen::MatrixXd zeros=Eigen::MatrixXd::Zero(3, 3);

Eigen::ArrayXXf table(10, 4);

table.col(0) = Eigen::ArrayXf::LinSpaced(10, 0, 90);

std::cout <<"/////////////Matrix Coefficient Wise Operations///////////////////"<< std::endl;

int i,j;

std::cout << my_matrix << std::endl;

std::cout << my_matrix.transpose()<< std::endl;

std::cout<<my_matrix.minCoeff(&i, &j)<<std::endl;

std::cout<<my_matrix.maxCoeff(&i, &j)<<std::endl;

std::cout<<my_matrix.prod()<<std::endl;

std::cout<<my_matrix.sum()<<std::endl;

std::cout<<my_matrix.mean()<<std::endl;

std::cout<<my_matrix.trace()<<std::endl;

std::cout<<my_matrix.colwise().mean()<<std::endl;

std::cout<<my_matrix.rowwise().maxCoeff()<<std::endl;

std::cout<<my_matrix.lpNorm<2>()<<std::endl;

std::cout<<my_matrix.lpNorm<Eigen::Infinity>()<<std::endl;

std::cout<<(my_matrix.array()>0).all()<<std::endl;// if all elemnts are positive

std::cout<<(my_matrix.array()>2).any()<<std::endl;//if any element is greater than 2

std::cout<<(my_matrix.array()>1).count()<<std::endl;// count the number of elements greater than 1

std::cout << my_matrix.array() - 2 << std::endl;

std::cout << my_matrix.array().abs() << std::endl;

std::cout << my_matrix.array().square() << std::endl;

std::cout << my_matrix.array() * my_matrix.array() << std::endl;

std::cout << my_matrix.array().exp() << std::endl;

std::cout << my_matrix.array().log() << std::endl;

std::cout << my_matrix.array().sqrt() << std::endl;

std::cout <<"//////////////////Block Elements Access////////////////////"<< std::endl;

//Block of size (p,q), starting at (i,j) matrix.block(i,j,p,q)

Eigen::MatrixXf mat(4, 4);

mat << 1, 2, 3, 4,

5, 6, 7, 8,

9, 10, 11, 12,

13, 14, 15, 16;

std::cout << "Block in the middle" << std::endl;

std::cout << mat.block<2, 2>(1, 1) << std::endl;

for (int i = 1; i <= 3; ++i)

{

std::cout << "Block of size " << i << "x" << i << std::endl;

std::cout << mat.block(0, 0, i, i) << std::endl;

}

/////////////build matrix from vector, resizing matrix, dynamic size ////////////////

Eigen::MatrixXd dynamicMatrix;

int rows, cols;

rows=3;

cols=2;

dynamicMatrix.resize(rows,cols);

dynamicMatrix<<-1,7,3,4,5,1;

//If you want a conservative variant of resize() which does not change the coefficients, use conservativeResize()

dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);

dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector3d(1, 4, 0);

dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);

dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector3d(5, -8, 6);

you should expect this:

1 7 1 5

3 4 4 -8

5 1 0 6

std::cout<< dynamicMatrix<<std::endl;

Eigen Arrays, Matrices and Vectors: Definition, Initialization Resizing, Populating and Coefficient Wise Operations Read More »

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 »

Camera Projection Matrix with Eigen

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

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

}

Camera Projection Matrix with Eigen 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 »

How to use image_geometry and camera_info_manager in ROS

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

camera_info_publisher.cpp:

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

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

#include <camera_info_manager/camera_info_manager.h>

int main(int argc, char** argv)

{

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

ros::NodeHandle nh;

const std::string cname="prosilica";

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

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

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

sensor_msgs::CameraInfo msgCameraInfo;

ros::Rate loop_rate(5);

while (nh.ok())

{

msgCameraInfo=cinfo.getCameraInfo();

pubCameraInfo.publish(msgCameraInfo);

ros::spinOnce();

loop_rate.sleep();

}

image_geometry_demo.cpp:

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

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

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

#include <ros/ros.h>

#include <image_geometry/pinhole_camera_model.h>

void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)

{

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

image_geometry::PinholeCameraModel cam_model_;

cam_model_.fromCameraInfo(info_msg);

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

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

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

}

int main(int argc,char ** argv)

{

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

ros::NodeHandle nh;

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

ros::spin();

}

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS image_geometry camera_info_manager roscpp)

1	find_package(catkin REQUIRED COMPONENTS image_geometry camera_info_manager roscpp)

How to use image_geometry and camera_info_manager in ROS Read More »

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

Leave a Comment / C++, Tutorials / admin

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

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

    heightOfSensor=10;
    widthOfSensor=10;

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

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



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


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

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

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

    cameraRotation =eulerAnglesToRotationMatrix(theta);

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

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

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

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

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


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

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

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

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

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

    double U,V;

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


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


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

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

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


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

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



    int Red,Blue,Green;

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


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

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

void virtualCameraSimulator(int argc, char ** argv)

{

int numberOfPixelInHeight,numberOfPixelInWidth;

double heightOfSensor, widthOfSensor;

double focalLength=0.5;

double mx, my, U0, V0;

numberOfPixelInHeight=600;

numberOfPixelInWidth=600;

heightOfSensor=10;

widthOfSensor=10;

my=(numberOfPixelInHeight)/heightOfSensor ;

U0=(numberOfPixelInHeight)/2 ;

mx=(numberOfPixelInWidth)/widthOfSensor;

V0=(numberOfPixelInWidth)/2;

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

focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1);

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

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

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

double roll,pitch,yaw;

cv::Vec3d theta;

roll=0 ;

pitch=0;

yaw=0;

theta[0]=roll;

theta[1]=pitch;

theta[2]=yaw;

cameraRotation =eulerAnglesToRotationMatrix(theta);

double tx,ty,tz;

tx=0;

ty=0;

tz=0;

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

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

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

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

Points are in the following form (OpenCV coordinate):

▲

/ 1 2 X

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

1| .

2| . . .

3| .

4| .

| Y

⯆

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

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

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

double x,y,z;

pcl::PointXYZ point;

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

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

{

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

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

}

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

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

double U,V;

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

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

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

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

{

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

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

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

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

}

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

cv::imwrite(fileName,cameraImage);

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

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

reverseProjection(distortionCoefficient , cameraMatrix, projectedPoints, directionRays);

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

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

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

int pointSize=5;

double coordinateFrameScaleSize=1;

std::string coordinateName;

coordinateName="World";

int viewport = 0;

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

double rayScale=5;

double r,g,b;

r=0.0;

g=1.0;

b=1.0;

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

{

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

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

}

int Red,Blue,Green;

Red=255;

Green=0;

Blue=0;

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

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

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

while (!viewer->wasStopped ())

{

viewer->spinOnce (100);

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

}

points are stored in CSV file like this:

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

x,y,z

2,1, 1

1,2, 1

2,2, 1

3,2, 1

2,3, 1

2,4, 1

The content of “reverse_projection.hpp”

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

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

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

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

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

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


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



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


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

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

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

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

with A^-1  =

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

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

Camera Matrix:

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

Rays are  A^-1*p:

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

*/

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

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

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

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

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

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

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

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

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

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

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

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

{

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

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

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

imageRayMatrix=cameraMatrix.inv()*imagePointMatrix;

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

}

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

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

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

x" = (u - cx)/fx

y" = (v - cy)/fy

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

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

x = X/W, y = Y/W

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

u' = x*fx' + cx'

v' = y*fy' + cy',

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

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

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

cv::convertPointsToHomogeneous(imagePointsInCameraNormalizedCoordinate,imagePointsInCameraNormalizedCoordinateHomogeneous);

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

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

double a,b,c;

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

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

{

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

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

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

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

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

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

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

}

The inverse of a 3x3 matrix:

| a11 a12 a13 |-1

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

| a31 a32 a33 |

with A^-1 =

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

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

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

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

Camera Matrix:

|fx 0 cx|

|0 fy cy|

|0 0 1 |

Rays are A^-1*p:

1 |fy 0 -fycx| |u|

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

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

The content of “transformation.hpp“

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

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

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

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


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

    return R;

}

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

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

}

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

    assert(isRotationMatrix(R));

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

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

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

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

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

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

#include <opencv2/opencv.hpp>

#include <Eigen/Eigen>

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

{

// Calculate rotation about x axis

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

1, 0, 0,

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

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

);

// Calculate rotation about y axis

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

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

0, 1, 0,

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

);

// Calculate rotation about z axis

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

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

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

0, 0, 1);

// Combined rotation matrix

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

return R;

}

bool isRotationMatrix(cv::Mat &R)

{

cv::Mat Rt;

cv::transpose(R, Rt);

cv::Mat shouldBeIdentity = Rt * R;

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

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

}

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

{

assert(isRotationMatrix(R));

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

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

float x, y, z;

if (!singular)

{

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

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

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

}

else

{

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

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

z = 0;

}

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

}

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

{

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

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

{

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

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

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

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

outputPoints.push_back(pointInCameraCoordinate);

}

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

{

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

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

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

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

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

return rotationMatrix;

}

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

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

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

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

Camera Calibration Based on Direct Linear Transform Explained

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

Camera Calibration Based on Direct Linear Transform Explained Read More »

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.