高博SLAM基础课第五讲——PnP非线性优化

这一题很重要

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>
#include "sophus/se3.h"

using namespace std;

typedef vector<Vector3d, Eigen::aligned_allocator<Vector3d>> VecVector3d;
typedef vector<Vector2d, Eigen::aligned_allocator<Vector3d>> VecVector2d;
typedef Matrix<double, 6, 1> Vector6d;

string p3d_file = "p3d.txt";
string p2d_file = "p2d.txt";

int main(int argc, char **argv) {

    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d K;
    double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
    K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

    // load points in to p3d and p2d 
    // START YOUR CODE HERE
    ifstream f3d(p3d_file);
    ifstream f2d(p2d_file);
    double point_3d[3];
    while(!f3d.eof()){
        for(int i=0;i<3;i++)f3d>>point_3d[i];
        p3d.push_back(Vector3d(point_3d[0],point_3d[1],point_3d[2]));
    }
    double point_2d[2];
    while(!f2d.eof()){
        for(int i=0;i<2;i++)f2d>>point_2d[i];
        p2d.push_back(Vector2d(point_2d[0],point_2d[1]));
    }
    // END YOUR CODE HERE
    assert(p3d.size() == p2d.size());
    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    Sophus::SE3 T_esti; // estimated pose

    for (int iter = 0; iter < iterations; iter++) {

        Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();

        cost = 0;
        // compute cost
        for (int i = 0; i < nPoints; i++) {
            // compute cost for p3d[I] and p2d[I]
            // START YOUR CODE HERE 

            Vector3d P_ = T_esti * Vector3d(p3d[i][0], p3d[i][1], p3d[i][2]);
            double x = P_[0];
            double y = P_[1];
            double z = P_[2];
            double z2 = z * z;
            double x2 = x * x;
            double y2 = y * y;
            Vector3d u = K * P_;
            u = u / u[2];

            Vector2d e(p2d[i][0] - u[0], p2d[i][1] - u[1]);
            cost += e.squaredNorm() / 2;
//        // END YOUR CODE HERE
//
//        // compute jacobian
            Matrix<double, 2, 6> J;
//            // START YOUR CODE HERE

            J(0,0)=fx/z;
            J(0,1)=0;
            J(0,2)=-fx*x/z2;
            J(0,3)=-fx*x*y/z2;
            J(0,4)=fx+fx*x2/z2;
            J(0,5)=-fx*y/z;

            J(1,0)=0;
            J(1,1)=fy/z;
            J(1,2)=-fy*y/z2;
            J(1,3)=-fy-fy*y2/z2;
            J(1,4)=fy*x*y/z2;
            J(1,5)=fy*x/z;
            J=-J;//注意这里1
// END YOUR CODE HERE

            H += J.transpose() * J;
            b += -J.transpose() * e;
        }

    // solve dx 
        Vector6d dx;

        // START YOUR CODE HERE 
            dx=H.ldlt().solve(b);
        // END YOUR CODE HERE

        if (isnan(dx[0])) {
            //cout << "result is nan!" << endl;
            break;
        }

       if (iter > 0 && cost >= lastCost) {
            // cost increase, update is not good
            //cout << "break!!!!"<<"cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // update your estimation
        // START YOUR CODE HERE 
        T_esti = Sophus::SE3::exp(dx)*T_esti;
        // END YOUR CODE HERE
        
        lastCost = cost;
        //cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
    }
    clock_t end=clock();


    cout << "estimated pose: \n" << T_esti.matrix() << endl<<"Time use: "<<(end-start)*1000/CLOCKS_PER_SEC<<"ms"<<endl;
    return 0;
}