I'm currently trying to run a pnp solver for the optimization of a camera pose: Given fixed 3D points in space, their corresponding image 2D image projections and the camera intrinsics matrix. I verify the reprojection of the points given the output pose matrix (extrinsics). The pose estimation is done with Opencv pnp solver and g2o code below.
Opencv seems fine, however I cannot make sense of the output pose camera matrix from g2o and the reprojection is off. Any suggestions or advice would be most helpful! Thank you in advance!
cv::Point2f projectPoint(const cv::Point3f &p, const cv::Mat &K) { return cv::Point2f ((p.x * K.at<float>(0,0)/p.z + K.at<float>(0,2)),(p.y * K.at<float>(1,1)/p.z + K.at<float>(1,2))); }
const float fx = 520.9;
const float cx = 325.1;
const float fy = 521.0;
const float cy = 249.7;
cv::Mat distMatrix = cv::Mat::zeros(4, 1, cv::DataType<float>::type);
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
K.at<float>(0,0) = fx;
K.at<float>(1,1) = fy;
K.at<float>(0,2) = cx;
K.at<float>(1,2) = cy;
std::vector<cv::Point2f> imagePts;
std::vector<cv::Point3f> modelPts;
modelPts.clear();
modelPts.push_back(cv::Point3f (-4,2,1));
modelPts.push_back(cv::Point3f (1,2,3));
modelPts.push_back(cv::Point3f (1,3,2));
modelPts.push_back(cv::Point3f (2,1,1));
modelPts.push_back(cv::Point3f (-1,4,2));
modelPts.push_back(cv::Point3f (7,0,3));
imagePts.clear();
imagePts.push_back(projectPoint(cv::Point3f(2, 3, 1), K));
imagePts.push_back(projectPoint(cv::Point3f(2, -2, 3), K));
imagePts.push_back(projectPoint(cv::Point3f(3, -2, 2), K));
imagePts.push_back(projectPoint(cv::Point3f(1, -3, 1), K));
imagePts.push_back(projectPoint(cv::Point3f(4, 0, 2), K));
imagePts.push_back(projectPoint(cv::Point3f(0, -8, 3), K));
cv::Mat rVector = (cv::Mat_<float>(3, 1, CV_32F) << 0, 0, 0);
cv::Mat tVector= (cv::Mat_<float>(3, 1, CV_32F) << 0, 0, 0);
std::vector<cv::Point2f> projectedPts;
cv::solvePnP(modelPts, imagePts, K, distMatrix, rVector, tVector);
cv::Mat R;
cv::Rodrigues(rVector, R);
cv::Mat poseMatrix = cv::Mat::eye(4, 4, CV_32F);
R.copyTo(poseMatrix(cv::Rect(0, 0, 3, 3)));
tVector.copyTo(poseMatrix(cv::Rect(3, 0, 1, 3)));
std::cout << "opencv Pose Matrix:" << poseMatrix << std::endl;
cv::projectPoints(modelPts, rVector, tVector, K, distMatrix, projectedPts);
for(unsigned int i = 0; i < projectedPts.size(); ++i)
{
std::cout << "Image point: " << imagePts[i] << " Projected to " << projectedPts[i] << std::endl;
}
cv::Mat initialPose = cv::Mat::eye(4,4,CV_32F);
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(true);
std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
linearSolver = std::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>>();
std::unique_ptr<g2o::BlockSolver_6_3> solver_ptr (new g2o::BlockSolver_6_3(std::move(linearSolver)));
g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));
optimizer.setAlgorithm(solver);
int vertexIdx = 0;
g2o::VertexSE3Expmap *camPose = new g2o::VertexSE3Expmap();
camPose->setId(vertexIdx++);
camPose->setEstimate(toSE3Quat(initialPose));
camPose->setFixed(false);
optimizer.addVertex(camPose);
for(size_t i = 0; i<modelPts.size(); i++)
{
g2o::VertexPointXYZ *point3D = new g2o::VertexPointXYZ();
point3D->setId(vertexIdx++);
point3D->setEstimate(Eigen::Vector3d (modelPts[i].x,modelPts[i].y,modelPts[i].z));
point3D->setMarginalized(true);
point3D->setFixed(true);
optimizer.addVertex(point3D);
}
g2o::CameraParameters *camParams = new g2o::CameraParameters(fx,Eigen::Vector2d(cx,cy),0);
camParams->setId(0);
optimizer.addParameter(camParams);
int edgeIdx = 0;
for(size_t j = 0; j<imagePts.size();j++)
{
g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
edge->setId(edgeIdx++);
edge->setVertex(0,dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex((edgeIdx+1))));
edge->setVertex(1,camPose);
edge->setMeasurement(Eigen::Vector2d (imagePts[j].x,imagePts[j].y));
edge->setParameterId(0,0);
edge->setInformation(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);
edgeIdx++;
}
optimizer.initializeOptimization();
optimizer.optimize(50);
cv::Mat estimatedPose = toCvMat(camPose->estimate());
std::cout << "g2o Pose Matrix:" << estimatedPose << std::endl;
cv::Rodrigues(estimatedPose(cv::Rect(0, 0, 3, 3)).clone(), rVector);
tVector = estimatedPose(cv::Rect(3, 0, 1, 3)).clone();
projectedPts.clear();
cv::projectPoints(modelPts, rVector, tVector, K, distMatrix, projectedPts);
for(unsigned int i = 0; i < projectedPts.size(); ++i)
{
std::cout << "Image point: " << imagePts[i] << " Projected to " << projectedPts[i] << std::endl;
}
Here is the output:
opencv Pose Matrix:[-4.3711392e-08, 1, -2.1225477e-09, -3.3296445e-08;
-1, -4.3711392e-08, -1.954343e-08, -0.99999988;
-1.954343e-08, 2.1225468e-09, 1, -2.6714115e-08;
0, 0, 0, 1]
Image point: [1366.9, 1812.7] Projected to [1366.9, 1812.7]
Image point: [672.367, -97.6333] Projected to [672.367, -97.6333]
Image point: [1106.45, -271.3] Projected to [1106.45, -271.3]
Image point: [846, -1313.3] Projected to [846, -1313.3]
Image point: [1366.9, 249.7] Projected to [1366.9, 249.7]
Image point: [325.1, -1139.63] Projected to [325.1, -1139.63]
5.26354e-315 0 0.0078125
0.0078125 0 0
0 5.26354e-315 0
0
5.26354e-315
0.0078125
iteration= 0 chi2= 4067601.149988 time= 0.000186321 cumTime= 0.000186321 edges= 3 schur= 1 lambda= 729726137812.847412 levenbergIter= 1
iteration= 1 chi2= 4067542.112396 time= 5.067e-05 cumTime= 0.000236991 edges= 3 schur= 1 lambda= 243242045937.615784 levenbergIter= 1
iteration= 2 chi2= 4067365.044704 time= 5.0601e-05 cumTime= 0.000287592 edges= 3 schur= 1 lambda= 81080681979.205261 levenbergIter= 1
iteration= 3 chi2= 4066834.247042 time= 4.5993e-05 cumTime= 0.000333585 edges= 3 schur= 1 lambda= 27026893993.068420 levenbergIter= 1
iteration= 4 chi2= 4065245.493651 time= 4.5915e-05 cumTime= 0.0003795 edges= 3 schur= 1 lambda= 9008964664.356140 levenbergIter= 1
iteration= 5 chi2= 4060511.745107 time= 4.5845e-05 cumTime= 0.000425345 edges= 3 schur= 1 lambda= 3002988221.452046 levenbergIter= 1
iteration= 6 chi2= 4046596.643980 time= 7.0317e-05 cumTime= 0.000495662 edges= 3 schur= 1 lambda= 1000996073.817349 levenbergIter= 1
iteration= 7 chi2= 4007263.388196 time= 4.9268e-05 cumTime= 0.00054493 edges= 3 schur= 1 lambda= 333665357.939116 levenbergIter= 1
iteration= 8 chi2= 3907315.614434 time= 4.8244e-05 cumTime= 0.000593174 edges= 3 schur= 1 lambda= 111221785.979705 levenbergIter= 1
iteration= 9 chi2= 3708895.525299 time= 5.842e-05 cumTime= 0.000651594 edges= 3 schur= 1 lambda= 37403922.773461 levenbergIter= 1
iteration= 10 chi2= 3448304.525415 time= 5.0854e-05 cumTime= 0.000702448 edges= 3 schur= 1 lambda= 18048977.409428 levenbergIter= 1
iteration= 11 chi2= 3250108.499856 time= 5.1256e-05 cumTime= 0.000753704 edges= 3 schur= 1 lambda= 7740155.963523 levenbergIter= 1
iteration= 12 chi2= 3049503.538679 time= 5.0348e-05 cumTime= 0.000804052 edges= 3 schur= 1 lambda= 2580051.987841 levenbergIter= 1
iteration= 13 chi2= 2698181.645287 time= 5.1083e-05 cumTime= 0.000855135 edges= 3 schur= 1 lambda= 860017.329280 levenbergIter= 1
iteration= 14 chi2= 1896741.985008 time= 4.8692e-05 cumTime= 0.000903827 edges= 3 schur= 1 lambda= 286672.443093 levenbergIter= 1
iteration= 15 chi2= 1119208.224301 time= 4.9394e-05 cumTime= 0.000953221 edges= 3 schur= 1 lambda= 131166.495391 levenbergIter= 1
iteration= 16 chi2= 389895.012406 time= 5.0266e-05 cumTime= 0.00100349 edges= 3 schur= 1 lambda= 68451.067197 levenbergIter= 1
iteration= 17 chi2= 27090.961786 time= 4.972e-05 cumTime= 0.00105321 edges= 3 schur= 1 lambda= 22817.022399 levenbergIter= 1
iteration= 18 chi2= 721.009229 time= 4.9587e-05 cumTime= 0.00110279 edges= 3 schur= 1 lambda= 7605.674133 levenbergIter= 1
iteration= 19 chi2= 3.346215 time= 4.8429e-05 cumTime= 0.00115122 edges= 3 schur= 1 lambda= 2535.224711 levenbergIter= 1
iteration= 20 chi2= 0.002573 time= 5.1063e-05 cumTime= 0.00120229 edges= 3 schur= 1 lambda= 845.074904 levenbergIter= 1
iteration= 21 chi2= 0.000000 time= 5.0643e-05 cumTime= 0.00125293 edges= 3 schur= 1 lambda= 563.383269 levenbergIter= 1
iteration= 22 chi2= 0.000000 time= 5.1876e-05 cumTime= 0.00130481 edges= 3 schur= 1 lambda= 375.588846 levenbergIter= 1
iteration= 23 chi2= 0.000000 time= 4.8687e-05 cumTime= 0.00135349 edges= 3 schur= 1 lambda= 250.392564 levenbergIter= 1
iteration= 24 chi2= 0.000000 time= 4.9374e-05 cumTime= 0.00140287 edges= 3 schur= 1 lambda= 166.928376 levenbergIter= 1
iteration= 25 chi2= 0.000000 time= 5.6584e-05 cumTime= 0.00145945 edges= 3 schur= 1 lambda= 111.285584 levenbergIter= 1
iteration= 26 chi2= 0.000000 time= 0.00026502 cumTime= 0.00172447 edges= 3 schur= 1 lambda= 2431070.678171 levenbergIter= 6
iteration= 27 chi2= 0.000000 time= 0.000125269 cumTime= 0.00184974 edges= 3 schur= 1 lambda= 12965710.283579 levenbergIter= 3
iteration= 28 chi2= 0.000000 time= 4.9929e-05 cumTime= 0.00189967 edges= 3 schur= 1 lambda= 8643806.855719 levenbergIter= 1
iteration= 29 chi2= 0.000000 time= 9.0831e-05 cumTime= 0.0019905 edges= 3 schur= 1 lambda= 69150454.845753 levenbergIter= 2
g2o Pose Matrix:[0.68579429, 0.44715682, 0.57422727, -2.0300806;
-0.71916682, 0.29522252, 0.62900138, 0.15114786;
0.11173744, -0.84433079, 0.52404225, 0.64115191;
0, 0, 0, 1]
Image point: [1366.9, 1812.7] Projected to [2099, -2030.58]
Image point: [672.367, -97.6333] Projected to [1366.9, 1813]
Image point: [1106.45, -271.3] Projected to [-490.13, -871.739]
Image point: [846, -1313.3] Projected to [672.367, -97.7001]
Image point: [1366.9, 249.7] Projected to [261.079, -708.225]
Image point: [325.1, -1139.63] Projected to [1106.45, -271.4]