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

// 构建超定方程组
// 此时A和b是已经分配好大小，A的维度为3n*9，b的维度为3n*1A.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分解！

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

激光SLAM里程计标定 —— 直接线性方法相关推荐

LIOM: 一种新的激光惯性里程计和建图方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:robot L | 来源:知乎 https://zhuanlan.zhihu.com/p/137 ...
东北大学 | 一种适用于大规模公路环境的鲁棒激光惯性里程计和建图系统
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达标题: A Robust Laser-Inertial Odometry and Mapping Me ...
激光SLAM理论与实践第二次作业（里程计标定）
1.章节: 1.激光SLAM理论与实践-第五期第一次作业(矩阵坐标变换) 2.激光SLAM理论与实践-第五期第二次作业(里程计标定) 3.激光SLAM理论与实践-第五期第三次作业(去运动畸变) ...
里程计标定：直接线性方法
里程计标定:直接线性方法通过线性最小二乘的方式标定里程计信息,使其提高精度. 航迹推算已知里程计信息,上一帧底盘位姿和当前底盘位姿: last_pose=[xlastylastθlast]now_ ...
最新开源Faster-LIO：快速激光IMU里程计
作者丨半闲居士@知乎来源丨https://zhuanlan.zhihu.com/p/468628910 编辑丨点云PCL 文章:Faster-LIO: Lightweight Tightly Cou ...
DEMO: 一种单目视觉里程计的深度增强方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:robot L | 来源:知乎 https://zhuanlan.zhihu.com/p/149 ...
Point-LIO：鲁棒高带宽激光惯性里程计
点击进入->3D视觉工坊学习交流群标题:Point-LIO: Robust High-Bandwidth LiDAR-Inertial Odometry 作者:He et al., HKU M ...
ROS导航小车3 odom里程计标定（仅作个人记录）
检测机器人是否需要标定参考链接 1.打开一个新的terminal,运行rviz rviz窗口打开后,将fixed frame选择为odom 关闭其他所有勾选,只保留grid和tf(如没有g ...
轮式里程计与激光雷达进行标定1--线性最小二乘直接线性方法
轮式里程计与激光雷达进行标定--线性最小二乘直接线性方法轮式里程计定义轮式里程计与激光SLAM的关系轮式里程计标定前言线性最小二乘直接线性方法里程计标定 Code Result 轮式里程计定 ...

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

激光SLAM里程计标定 —— 直接线性方法相关推荐

最新文章

热门文章