激光SLAM里程计标定 —— 直接线性方法

// 构建超定方程组
// 此时A和b是已经分配好大小，A的维度为3n*9，b的维度为3n*1
    A.middleRows(now_len*3,3)<< Odom(0),Odom(1),Odom(2),0,0,0,0,0,0,
	                0,0,0,Odom(0),Odom(1),Odom(2),0,0,0,
	                0,0,0,0,0,0,Odom(0),Odom(1),Odom(2);  
    b.segment<3>(now_len*3) << scan(0),scan(1),scan(2);

// 求解线性最小二乘
// 利用ldlt分解得到的是9*1的向量，需要将其转成3*3的矩阵
    //correct_matrix = (A.transpose()*A).inverse()*A.transpose()*b;
    Eigen::VectorXd correct_vector = A.ldlt().solve(b);
    // 通过Map函数将9维向量转成矩阵，但是得到的顺序为矫正矩阵的转置
    correct_matrix = Eigen::Map<Eigen::Matrix3d>(correct_vector.data());
    // 因此需要转换后的矩阵进行转置得到矫正矩阵
    correct_matrix = correct_matrix.transpose();

在运行时出现如下错误：

calib_odom_node: /usr/include/eigen3/Eigen/src/Cholesky/LDLT.h:437: Eigen::LDLT<MatrixType, _UpLo>& Eigen::LDLT<MatrixType, UpLo>::compute(const Eigen::EigenBase<OtherDerived>&) [with InputType = Eigen::Matrix<double, -1, -1>; _MatrixType = Eigen::Matrix<double, -1, -1>; int _UpLo = 1]: Assertion `a.rows()==a.cols()' failed.
[OdometryNode-1] process has died [pid 7978, exit code -6, cmd /home/like/Laser slam /HW2/odom_ws/devel/lib/calib_odom/calib_odom_node __name:=OdometryNode __log:=/home/like/.ros/log/af318c8a-d0b9-11ea-ba9b-db22f206bda5/OdometryNode-1.log].
log file: /home/like/.ros/log/af318c8a-d0b9-11ea-ba9b-db22f206bda5/OdometryNode-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

可能是LDLT求解维度的问题，现在尝试先对A矩阵分块使用LDLT：
Eigen高阶操作总结 — 子矩阵、块操作

    Eigen::Matrix3d matrix1 = A.leftCols(3);
    Eigen::Matrix3d matrix2;
    for(int i = 0;i<3;i++){
        matrix2.col(i) = A.col(3+i);
    }
    Eigen::Matrix3d matrix3 = A.rightCols(3);
    Eigen::Vector3d correct_vector1 = matrix1.ldlt().solve(b);
    Eigen::Vector3d correct_vector2 = matrix2.ldlt().solve(b);
    Eigen::Vector3d correct_vector3 = matrix3.ldlt().solve(b);
    correct_matrix.row(0) = correct_vector1.transpose();
    correct_matrix.row(1) = correct_vector2.transpose();
    correct_matrix.row(2) = correct_vector3.transpose();

出现如下错误：

calib_odom_node: /usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:258: void Eigen::PlainObjectBase<Derived>::resize(Eigen::Index, Eigen::Index) [with Derived = Eigen::Matrix<double, 3, 3>; Eigen::Index = long int]: Assertion `(!(RowsAtCompileTime!=Dynamic) || (rows==RowsAtCompileTime)) && (!(ColsAtCompileTime!=Dynamic) || (cols==ColsAtCompileTime)) && (!(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic) || (rows<=MaxRowsAtCompileTime)) && (!(ColsAtCompileTime==Dynamic && MaxColsAtCompileTime!=Dynamic) || (cols<=MaxColsAtCompileTime)) && rows>=0 && cols>=0 && "Invalid sizes when resizing a matrix or array."' failed.
[OdometryNode-2] process has died [pid 4529, exit code -6, cmd /home/like/Laser slam /HW2/odom_ws/devel/lib/calib_odom/calib_odom_node __name:=OdometryNode __log:=/home/like/.ros/log/bf6c35be-d14f-11ea-b81a-000c29807a7f/OdometryNode-2.log].
log file: /home/like/.ros/log/bf6c35be-d14f-11ea-b81a-000c29807a7f/OdometryNode-2*.log

可能是向量对矩阵赋值出了问题，发现是分块矩阵维度定义错了，将其改成igen::MatrixXd。
最终问题应该是IDLT对分解矩阵的是有要求的！而且使用.col或者.row进行赋值也是错误的！

折腾那么久之后换成QR分解！

顺利解决！被自己蠢到，下图绿色就是校正后的轨迹，发现和激光扫描的轨迹已经很接近了！