《视觉SLAM十四讲》slambook/ch7/gpose_estimation_3d3d更改成只用Eigen不用非线性优化

例子运行方法：
siat@hzt:~/Documents/slambook/ch7$ ./build/pose_estimation_3d3d 1.png 2.png 1_depth.png 2_depth.png

因我具体的问题是3点到3点的ICP匹配求相机姿态，所以SVD就够了，不用非线性优化（因里面有g2o库还需要再下载一个，挺大的吧，各种不想用），自己删除了此文件中非线性相关代码，并成功编译使用SVD（前提是装OPENCV，Eigen 头文件库）。新建gpose_estimation_3_3d3d.cpp内容如下：
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include

using namespace std;
using namespace cv;

void pose_estimation_3d3d (
const vector& pts1,
const vector& pts2,
Mat& R, Mat& t
);

void bundleAdjustment(
const vector& points_3d,
const vector& points_2d,
Mat& R, Mat& t
);

// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}

virtual void computeError()
{
    const g2o::VertexSE3Expmap* pose = static_cast ( _vertices[0] );
    // measurement is p, point is p'
    _error = _measurement - pose->estimate().map( _point );
}

virtual void linearizeOplus()
{
    g2o::VertexSE3Expmap* pose = static_cast(_vertices[0]);
    g2o::SE3Quat T(pose->estimate());
    Eigen::Vector3d xyz_trans = T.map(_point);
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];

    _jacobianOplusXi(0,0) = 0;
    _jacobianOplusXi(0,1) = -z;
    _jacobianOplusXi(0,2) = y;
    _jacobianOplusXi(0,3) = -1;
    _jacobianOplusXi(0,4) = 0;
    _jacobianOplusXi(0,5) = 0;

    _jacobianOplusXi(1,0) = z;
    _jacobianOplusXi(1,1) = 0;
    _jacobianOplusXi(1,2) = -x;
    _jacobianOplusXi(1,3) = 0;
    _jacobianOplusXi(1,4) = -1;
    _jacobianOplusXi(1,5) = 0;

    _jacobianOplusXi(2,0) = -y;
    _jacobianOplusXi(2,1) = x;
    _jacobianOplusXi(2,2) = 0;
    _jacobianOplusXi(2,3) = 0;
    _jacobianOplusXi(2,4) = 0;
    _jacobianOplusXi(2,5) = -1;
}

bool read ( istream& in ) {}
bool write ( ostream& out ) const {}

protected:
Eigen::Vector3d _point;
};

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

vector pts1, pts2;

float x1=43,y1=7,z1=404;
float x2=-38,y2=6,z2=384;
float x3=-52,y3=2,z3=382;
float xo1=78,yo1=6,zo1=416;
float xo2=6,yo2=7,zo2=372;
float xo3=-7,yo3=2,zo3=367;
pts1.push_back ( Point3f (x1, y1, z1));
pts2.push_back ( Point3f (xo1, yo1,zo1));
pts1.push_back ( Point3f (x2, y2, z2));
pts2.push_back ( Point3f (xo2, yo2,zo2));
pts1.push_back ( Point3f (x3, y3, z3));
pts2.push_back ( Point3f (xo3, yo3,zo3));
cout<<"3d-3d pairs: "< Mat R, t;
pose_estimation_3d3d ( pts1, pts2, R, t );
cout<<"ICP via SVD results: "< cout<<"R = "< cout<<"t = "< cout<<"R_inv = "< cout<<"t_inv = "<<-R.t() *t<

cout<<"calling bundle adjustment"<$CSPARS{E}_{L}IBRARY)然后进入build目录开始编译：siat@hzt:/Documents/slambook/ch7$ cd build
 siat@hzt:~/Documents/slambook/ch7/build$ cmake …（注意…前有一个空格）
 siat@hzt:~/Documents/slambook/ch7/build$ cmake…
 过几分钟编译完生产可执行文件pose_estimation_3_3d3d，
 就可以运行了：
 siat@hzt:~/Documents/slambook/ch7/build$ ./pose_estimation_3_3d3d