Pose estimation for augmented reality
homography-dlt-opencv.cpp

[Include]

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
cv::Mat homography_dlt(const std::vector< cv::Point2d > &x1, const std::vector< cv::Point2d > &x2)
{
int npoints = (int)x1.size();
cv::Mat A(2*npoints, 9, CV_64F, cv::Scalar(0));
// We need here to compute the SVD on a (n*2)*9 matrix (where n is
// the number of points). if n == 4, the matrix has more columns
// than rows. The solution is to add an extra line with zeros
if (npoints == 4)
A.resize(2*npoints+1, cv::Scalar(0));
// Since the third line of matrix A is a linear combination of the first and second lines
// (A is rank 2) we don't need to implement this third line
for(int i = 0; i < npoints; i++) { // Update matrix A using eq. 23
A.at<double>(2*i,3) = -x1[i].x; // -xi_1
A.at<double>(2*i,4) = -x1[i].y; // -yi_1
A.at<double>(2*i,5) = -1; // -1
A.at<double>(2*i,6) = x2[i].y * x1[i].x; // yi_2 * xi_1
A.at<double>(2*i,7) = x2[i].y * x1[i].y; // yi_2 * yi_1
A.at<double>(2*i,8) = x2[i].y; // yi_2
A.at<double>(2*i+1,0) = x1[i].x; // xi_1
A.at<double>(2*i+1,1) = x1[i].y; // yi_1
A.at<double>(2*i+1,2) = 1; // 1
A.at<double>(2*i+1,6) = -x2[i].x * x1[i].x; // -xi_2 * xi_1
A.at<double>(2*i+1,7) = -x2[i].x * x1[i].y; // -xi_2 * yi_1
A.at<double>(2*i+1,8) = -x2[i].x; // -xi_2
}
// Add an extra line with zero.
if (npoints == 4) {
for (int i=0; i < 9; i ++) {
A.at<double>(2*npoints,i) = 0;
}
}
cv::Mat w, u, vt;
cv::SVD::compute(A, w, u, vt);
double smallestSv = w.at<double>(0, 0);
unsigned int indexSmallestSv = 0 ;
for (int i = 1; i < w.rows; i++) {
if ((w.at<double>(i, 0) < smallestSv) ) {
smallestSv = w.at<double>(i, 0);
indexSmallestSv = i;
}
}
cv::Mat h = vt.row(indexSmallestSv);
if (h.at<double>(0, 8) < 0) // tz < 0
h *=-1;
cv::Mat _2H1(3, 3, CV_64F);
for (int i = 0 ; i < 3 ; i++)
for (int j = 0 ; j < 3 ; j++)
_2H1.at<double>(i,j) = h.at<double>(0, 3*i+j);
return _2H1;
}
int main()
{
std::vector< cv::Point2d > x1; // Points projected in the image plane linked to camera 1
std::vector< cv::Point2d > x2; // Points projected in the image plane linked to camera 2
std::vector< cv::Point3d > wX; // 3D points in the world plane
// Ground truth pose used to generate the data
cv::Mat c1tw_truth = (cv::Mat_<double>(3,1) << -0.1, 0.1, 1.2); // Translation vector
cv::Mat c1rw_truth = (cv::Mat_<double>(3,1) << CV_PI/180*(5), CV_PI/180*(0), CV_PI/180*(45)); // Rotation vector
cv::Mat c1Rw_truth(3,3,cv::DataType<double>::type); // Rotation matrix
cv::Rodrigues(c1rw_truth, c1Rw_truth);
cv::Mat c2tc1 = (cv::Mat_<double>(3,1) << 0.01, 0.01, 0.2); // Translation vector
cv::Mat c2rc1 = (cv::Mat_<double>(3,1) << CV_PI/180*(0), CV_PI/180*(3), CV_PI/180*(5)); // Rotation vector
cv::Mat c2Rc1(3,3,cv::DataType<double>::type); // Rotation matrix
cv::Rodrigues(c2rc1, c2Rc1);
// Input data: 3D coordinates of at least 4 coplanar points
double L = 0.2;
wX.push_back( cv::Point3d( -L, -L, 0) ); // wX_0 (-L, -L, 0)^T
wX.push_back( cv::Point3d( 2*L, -L, 0) ); // wX_1 ( L, -L, 0)^T
wX.push_back( cv::Point3d( L, L, 0) ); // wX_2 ( L, L, 0)^T
wX.push_back( cv::Point3d( -L, L, 0) ); // wX_3 (-L, L, 0)^T
// Input data: 2D coordinates of the points on the image plane
for(int i = 0; i < wX.size(); i++) {
// Compute 3D points coordinates in the camera frame 1
cv::Mat c1X = c1Rw_truth*cv::Mat(wX[i]) + c1tw_truth; // Update c1X, c1Y, c1Z
// Compute 2D points coordinates in image plane from perspective projection
x1.push_back( cv::Point2d( c1X.at<double>(0, 0)/c1X.at<double>(2, 0), // x1 = c1X/c1Z
c1X.at<double>(1, 0)/c1X.at<double>(2, 0) ) ); // y1 = c1Y/c1Z
// Compute 3D points coordinates in the camera frame 2
cv::Mat c2X = c2Rc1*cv::Mat(c1X) + c2tc1; // Update c2X, c2Y, c2Z
// Compute 2D points coordinates in image plane from perspective projection
x2.push_back( cv::Point2d( c2X.at<double>(0, 0)/c2X.at<double>(2, 0), // x2 = c2X/c2Z
c2X.at<double>(1, 0)/c2X.at<double>(2, 0) ) ); // y2 = c2Y/c2Z
}
cv::Mat _2H1 = homography_dlt(x1, x2);
std::cout << "2H1 (computed with DLT):\n" << _2H1 << std::endl;
return 0;
}