最近整理一下之前所做的深蓝学院视觉SLAM课程的作业，其中内容有些遗忘，就当做复习。毕竟做作业的过程中收获很大。

3-6 轨迹的描绘

程序代码：

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string trajectory_file = "./trajectory.txt";//注意路径问题，确保文件在build文件夹中

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);

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

    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;

    /// implement pose reading code
    // start your code here (5~10 lines)
    ifstream fin(trajectory_file);
//    while(!fin.eof()){
    
//        double data[8]={0};
//        for(auto& d:data)
//            fin>>d;
//        Eigen::Quaterniond q(data[7],data[4],data[5],data[6]);
//        Eigen::Vector3d t(data[1],data[2],data[3]);
//        Sophus::SE3 SE3_q(q,t);
//        poses.push_back(SE3_q);
//    }
    if(!fin)
        cerr<<"ERROR!"<<endl;
    double data[7]={
    0};
    double time=0;
    while(fin>>time){
    
        for(auto& d:data){
    
            fin>>d;
        }
        Eigen::Quaterniond q(data[6],data[3],data[4],data[5]);
        Eigen::Vector3d t(data[0],data[1],data[2]);
//        double q0=data[6],q1=data[3],q2=data[4],q3=data[5];
//        Eigen::Matrix<double,4,4> T;//可以从四元数来求得旋转矩阵，通过先求旋转向量，再用罗德里格斯公式。
//        T<<1-2*q2*q2-2*q3*q3,2*q1*q2+2*q0*q3,2*q1*q3-2*q0*q2,data[0],
//           2*q1*q2-2*q0*q3,1-2*q1*q1-2*q3*q3,2*q2*q3+2*q0*q1,data[1],
//           2*q1*q3+2*q0*q2,2*q2*q3-2*q0*q1,1-2*q1*q1-2*q2*q2,data[2],
//           0,0,0,1;
//        poses.push_back(T);
        Sophus::SE3 SE3_q(q,t);
        poses.push_back(SE3_q);
    }
    // end your code here

    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
    
    if (poses.empty()) {
    
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));


    while (pangolin::ShouldQuit() == false) {
    
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses.size() - 1; i++) {
    
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

CMakeLists.txt文件：

cmake_minimum_required(VERSION 2.8)
project(assignment3_6)
set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

add_executable(assignment3_6 "main.cpp")

find_package(Pangolin REQUIRED)
find_package( Sophus REQUIRED )
include_directories( ${
    Sophus_INCLUDE_DIRS} )
include_directories(
${
    EIGEN3_INCLUDE_DIR}
${
    Pangolin_INCLUDE_DIRS}
)


find_package(Sophus REQUIRED)

target_link_libraries(assignment3_6 ${
    Sophus_LIBRARIES})
target_link_libraries(${
    PROJECT_NAME}
${
    OpenCV_LIBS}
${
    EIGEN3_LIBS}
${
    Pangolin_LIBRARIES}
)

4-2图像去畸变

代码：

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

using namespace std;

string image_file = "../test.png";   // 请确保路径正确

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

    // 本程序需要你自己实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file,0);   // 图像是灰度图，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {
    
            double u_distorted = 0, v_distorted = 0;
            // TODO 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted) (~6 lines)
            // start your code here公式中的坐标为归一化平面
            double x=(u-cx)/fx;//这里的x，y都是归一化平面上的坐标。第一步，将像素坐标转化成归一化坐标
            double y=(v-cy)/fy;
            double x_distorted=x*(1+k1*(x*x+y*y)+k2*((x*x+y*y)*(x*x+y*y)))+2*p1*x*y+p2*(3*x*x+y*y);//利用畸变模型，求解点对应的畸变坐标
            double y_distorted=y*(1+k1*(x*x+y*y)+k2*((x*x+y*y)*(x*x+y*y)))+p1*(3*y*y+x*x)+2*p2*x*y;
            u_distorted=fx*x_distorted+cx;//第三步，求解畸变点的像素坐标
            v_distorted=fy*y_distorted+cy;
            // end your code here

            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
    
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);//第四步，将畸变图像中所求的畸变像素点对应的像素存入去畸变的图像。思想就是将畸变图像中畸变的像素点放入正确的位置。
            } else {
    
                image_undistort.at<uchar>(v, u) = 0;
            }
        }

    // 画图去畸变后图像
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}

4-3双目视差的使用

代码：
最初对于自己的困难在于，对OpenCV的一些API用法不熟悉。

#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径，如果不对，请调整
string left_file = "../left.png";
string right_file = "../right.png";
string disparity_file = "../disparity.png";

// 在panglin中画图，已写好，无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

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

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 间距
    double d = 0.573;

    // 读取图像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Mat disparity = cv::imread(disparity_file, 0); // disparity 为CV_8U,单位为像素

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // TODO 根据双目模型计算点云
    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
    

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // start your code here (~6 lines)
            // 根据双目模型计算 point 的位置
            //unsigned int disp = disparity.ptr<unsigned short > ( v )[u];
            int disp=disparity.at<short>(v,u*disparity.channels());
            //unsigned char* row_ptr = disparity.ptr<unsigned char > ( v );  // row_ptr是第y行的头指针
           // unsigned char* data_ptr = &row_ptr[ u*disparity.channels() ]; // data_ptr 指向待访问的像素数据
           if (disp==0){
    
               cout<<"disp=0"<<endl;
               continue;
           }
            point[2]=(fx*d*1000)/(disp);//计算中是以毫米为单位
            point[0]=(u-cx)*point[2]/fx;
            point[1]=(v-cy)*point[2]/fy;
            pointcloud.push_back(point);
            // end your code here
        }

    // 画出点云
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {
    

if (pointcloud.empty()) {
    
cerr << "Point cloud is empty!" << endl;
return;
}

pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
);

pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

while (pangolin::ShouldQuit() == false) {
    
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

d_cam.Activate(s_cam);
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

glPointSize(2);
glBegin(GL_POINTS);
for (auto &p: pointcloud) {
    
glColor3f(p[3], p[3], p[3]);
glVertex3d(p[0], p[1], p[2]);
}
glEnd();
pangolin::FinishFrame();
usleep(5000);   // sleep 5 ms
}
return;
}

4-5 高斯牛顿法的曲线拟合实验

该题需要注意点的有：
1.求出误差函数
2.求出误差函数的导数，对于每个待优化参数分别求导。
3.求出GN方程
4.求解线性方程组Ax=b

代码：

//
// Created by 高翔 on 2017/12/15.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
    int N = 100;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
    
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
    }

    // 开始Gauss-Newton迭代
    int iterations = 100;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

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

        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;
        cout<<iter<<endl;
        for (int i = 0; i < N; i++) {
    
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            // start your code here
            double error = 0;   // 第i个数据点的计算误差
            error = yi-exp(ae*xi*xi+be*xi+ce); // 填写计算error的表达式
            Vector3d J; // 雅可比矩阵的转置，即列向量，因为此时的fx是一维的。
            J[0] = -xi*xi*exp(ae*xi*xi+be*xi+ce);  // de/da分别对各个参数求导
            J[1] = -xi*exp(ae*xi*xi+be*xi+ce);  // de/db
            J[2] = -exp(ae*xi*xi+be*xi+ce); // de/dc

            H +=  J*J.transpose(); // GN近似的H
            b += -J*error;
            // end your code here

            cost += error * error;
        }

        // 求解线性方程 Hx=b，建议用ldlt
        // start your code here
        Vector3d dx;
        dx=H.ldlt().solve(b);//采用改进的Cholesky方法。H需为对称正定矩阵
        //cout<<dx.transpose()<<endl;
        // end your code here

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

        if (iter > 0 && cost > lastCost) {
    
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 更新abc估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << endl;
    }

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}

5-2 ORB特征点

（1）ORB提取

ORB即Oriented FAST，实际是对FAST角点添加了尺度和旋转的描述。
尺度不变性由图像金字塔来实现；旋转指添加了特征的方向信息。

代码：

// compute the angle
void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) {
    
    int half_patch_size = 8;
    for (auto &kp : keypoints) {
    
        // START YOUR CODE HERE (~7 lines)
        int x=kp.pt.x;
        int y=kp.pt.y;
        if(x<half_patch_size||y<half_patch_size||x+half_patch_size>image.cols||y+half_patch_size>image.rows)
            continue;
        double m01=0,m10=0;
        for (int v  = y-half_patch_size; v < (y + half_patch_size-1 ); ++v) {
    
            for (int u = x-half_patch_size; u < (x+half_patch_size-1);++u) {
    
                m01 += v*image.at<uchar >(v,u);//注意这里的坐标，（y，x）
                m10 += u*image.at<uchar >(v,u);//图像矩中的坐标为图像块的像素坐标
            }
        }
        kp.angle = atan2(m01,m10)*180/pi; // compute kp.angle
        //kp.angle=atan(m01/m10)*180/pi; //atan范围只是-90~90,atan2范围为-180~180
        cout<<"kp.angle="<<kp.angle<<endl;
        // END YOUR CODE HERE
    }
    return;
}

通过查看opencv官方文档，知道如何读取图像像素值。

（2）BREIF描述子

需要选取任意的两点，p，q。两点坐标为相对于keypoints的位置，先进行旋转在加到keypoints坐标上。

代码：

// compute the descriptor
void computeORBDesc(const cv::Mat &image, vector<cv::KeyPoint> &keypoints, vector<DescType> &desc) {
    
    for (auto &kp: keypoints) {
    
        DescType d(256, false);
        for (int i = 0; i < 256; i++) {
    
            // START YOUR CODE HERE (~7 lines)先旋转，再恢复像素值的真值
            auto cos_= double(cos(kp.angle*pi/180));
            auto sin_= double(sin(kp.angle*pi/180));
            //pattern像素点如何取，得到的像素点坐标是以keypoints为基准取的,ORB_pattern指的是选取像素点坐标，并对该坐标进行旋转
//            cv::Point2f p_r(cos_*ORB_pattern[4*i]-sin_*ORB_pattern[4*i+1],
//                            sin_*ORB_pattern[4*i]+cos_*ORB_pattern[4*i+1]);

            cv::Point2f p_r(cos_*ORB_pattern[4*i]-sin_*ORB_pattern[4*i+1],
                            sin_*ORB_pattern[4*i]+cos_*ORB_pattern[4*i+1]);
            cv::Point2f q_r(cos_*ORB_pattern[4*i+2]-sin_*ORB_pattern[4*i+3],
                            sin_*ORB_pattern[4*i+2]+cos_*ORB_pattern[4*i+3]);
            //得到p，q的真实像素坐标，真实keypoints坐标加上相对坐标
            cv::Point2f p(kp.pt+p_r);
            cv::Point2f q(kp.pt+q_r);//先旋转后求真值
            // if kp goes outside, set d.clear()
            if(p.x<0||p.x>image.cols||p.y<0||p.y>image.rows||q.x<0||q.x>image.cols||q.y<0||q.y>image.rows){
    
                d.clear();
                break;
            }
            //判断像素值大小,bool值或者也可用uchar
            d[i] = image.at<uchar >(p)>image.at<uchar >(q)?0:1;
            // END YOUR CODE HERE
        }
        desc.push_back(d);
    }

    int bad = 0;
    for (auto &d: desc) {
    
        if (d.empty()) bad++;
    }
    cout << "bad/total: " << bad << "/" << desc.size() << endl;
    return;
}

（3）暴力匹配Brute-force Match

主要的点在于：
1.汉明距离为每一位的异或相加
2.最终的匹配对类型构建为DMatch
代码：

//brute-force matching
void bfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches) {
    
    int d_max = 50;
    //暴力匹配时 注意规避 描述符为空的点；
    // START YOUR CODE HERE (~12 lines)
    // find matches between desc1 and desc2.
    for (int i = 0; i < desc1.size(); ++i) {
    
        if(desc1[i].empty())
            continue;
        int d_min=256,index=-1;
        for (int j = 0; j < desc2.size(); ++j) {
    
            if(desc2[j].empty())
                continue;
            int d=0;
            for (int k = 0; k < 256; ++k) {
    
                d+=desc1[i][k]^desc2[j][k];
            }

            if(d<d_min){
    
                d_min=d;//将此时的d和对应的keyboard下标保存
                index=j;
            }
        }
        if(d_min<=d_max){
    
           cv::DMatch match(i,index,d_min);
            matches.push_back(match);
        }
    }
    // END YOUR CODE HERE
    for (auto &m: matches) {
    
        cout << m.queryIdx << ", " << m.trainIdx << ", " << m.distance << endl;
    }
    return;
}

5-3 分解矩阵

难点主要在如何利用Eigen里的一些api接口。

代码：

//
// Created by 高翔 on 2017/12/19.
// 本程序演示如何从Essential矩阵计算R,t
//
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sophus/so3.h>

using namespace Eigen;


using namespace std;

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

    // 给定Essential矩阵
    Matrix3d E;
    E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
            0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
            -0.006788487241438284, -0.5815434272915686, -0.01438258684486258;

    // 待计算的R,t
    Matrix3d R;
    Vector3d t;

    // SVD and fix singular values
    // START YOUR CODE HERE
    JacobiSVD<Matrix3d> svd(E,ComputeFullU|ComputeFullV);
    Vector3d sigma=svd.singularValues();//求得奇异值sigma
    Matrix3d SIGMA;
    SIGMA<<(sigma(0,0)+sigma(1,0))/2,0,0,
    0,(sigma(1,0)+sigma(2,0)/2),0,
    0,0,0;//为使E满足条件
    cout<<"SIGMA="<<SIGMA<<endl;

    // END YOUR CODE HERE
    // set t1, t2, R1, R2 
    // START YOUR CODE HERE
    Matrix3d t_wedge1;
    Matrix3d t_wedge2;

    Matrix3d R1;
    Matrix3d R2;
    Matrix3d U=svd.matrixU();
    Matrix3d V=svd.matrixV();
    Matrix3d R_z1=AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();
    Matrix3d R_z2=AngleAxisd(-M_PI/2,Vector3d(0,0,1)).toRotationMatrix();

    t_wedge1=U*R_z1*SIGMA*U.transpose();
    t_wedge2=U*R_z2*SIGMA*U.transpose();
    R1=U*R_z1.transpose()*V.transpose();
    R2=U*R_z2.transpose()*V.transpose();

    // END YOUR CODE HERE

    cout << "R1 = " << R1 << endl;
    cout << "R2 = " << R2 << endl;
    cout << "t1 = " << Sophus::SO3::vee(t_wedge1) << endl;
    cout << "t2 = " << Sophus::SO3::vee(t_wedge2) << endl;

    // check t^R=E up to scale
    Matrix3d tR = t_wedge1 * R1;
    cout << "t^R = " << tR << endl;

    return 0;
}

5-4 用Gauss-Newton 实现BA问题

首先对于PnP问题，已知世界坐标系下3D点坐标和对应的2D像素坐标。因为所要估计的便主要是相机位姿T，so必然需要世界坐标系点和像素坐标点对应关系。

对于BA（Bundle Adjustment），个人的理解是：Bundle：束，捆；Adjustment：调整；即将参数放在一起进行优化。
对于PnP的BA问题，即最小化Reprojection error-重投影误差问题。将相机坐标系下3D点的位置和相机位姿放在一起进行优化。
插一句，一般PnP问题都是先利用三角形相似及吴消元法来求解一个初始估计相机位姿，再利用BA优化。

需要用李代数来表示相机位姿，为的是求解优化问题时，需要对误差函数进行求导，求Jacobi，而李代数可以构建无约束的优化问题，可以直接求导。
将重投影误差求和，构建最小二乘问题，寻找最好的相机位姿，使得目标函数最小化。

可以利用G-N、L-M等算法进行优化，但需要先线性化：

需要知道每个误差项关于优化变量的导数。
该问题困难的地方在于求导函数J：
其中e为像素坐标的误差（2维），x为相机位姿（6维，R，t），所以这里的J为2*6的矩阵。

对李代数进行左乘扰动，并采用链式法则来进行求导：

其中P‘为相机坐标系下空间点

易得
第二项导数：

其中误差项的负号，是因为误差是由观测值减预测值定义的，这样求导后第一项为0.（PS：这个问题以前竟然没想明白，真的蠢。）

当然除了优化位姿，还希望优化特征点空间位置：（虽然是已知的，但传感器采回来的点也不够精确。）

难点在于：
1.求解误差表达式
2.求解导数
3.利用左乘扰动更新数据
代码：

//
// Created by xiang on 12/21/17.
//

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

using namespace Eigen;

#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <arpa/nameser.h>

#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 fin_2(p2d_file);
    if(!fin_2)
        cerr<<"Error,file is empty"<<endl;
    while (!fin_2.eof()){
    
        double data[2]={
    0};
        for(auto& d:data)
            fin_2>>d;
        Vector2d p2(data[0],data[1]);
        p2d.push_back(p2);
    }

    ifstream fin_3(p3d_file);
    if(!fin_3)
        cerr<<"Error,file is empty"<<endl;
    while(!fin_3.eof()){
    
        double data[3]={
    0};
        for (auto& d:data)
            fin_3>>d;
        Vector3d p3(data[0],data[1],data[2]);
        p3d.push_back(p3);
    }
    // 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
            Vector4d P=T_esti.matrix()*Vector4d(p3d[i](0,0),p3d[i](1,0),p3d[i](2,0),1);//将世界坐标系下转换到相机坐标系下3D点
            //Vector3d u=K*Vector3d(P(0,0),P(1,0),P(2,0));//像素坐标点
            //Vector2d e=p2d[i]-Vector2d(u(0,0)/u(2,0),u(1,0)/u(2,0));//误差，p2d-归一化平面点
            Vector3d u=K*Vector3d(P(0,0),P(1,0),P(2,0))/P(2,0);//和上式same，因为s=Z即u(2,0)=P(2,0)
            Vector2d e=p2d[i]-Vector2d(u(0,0),u(1,0));
            cost+=e.squaredNorm()/2;//求解2范数平方的1/2

            // END YOUR CODE HERE

            // compute jacobian
            Matrix<double, 2, 6> J;
            // START YOUR CODE HERE
            double x=P(0,0);
            double y=P(1,0);
            double z=P(2,0);

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

            J(1,0)=0;
            J(1,1)=-fy/z;
            J(1,2)=fy*y/(z*z);
            J(1,3)=fy+fy*y*y/(z*z);
            J(1,4)=-fy*x*y/(z*z);
            J(1,5)=-fy*x/z;
            // 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);//改进的Cholesky分解法
        // 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 << "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;
    }

    cout << "estimated pose: \n" << T_esti.matrix() << endl;
    return 0;
}