Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose(); // sqrt_info.setIdentity(); residual = sqrt_info * residual; 因为真正的优化项是Mahalanobis 距离d = r^{T} * P_{b_{k+1}}^{b_{k...
applyOnTheLeft(sqrt_information_.template cast<T>()); return true; } static ceres::CostFunction* Create(const Eigen::Vector3d& pts_i, const Eigen::Vector3d& pts_j) { return new ceres:: AutoDiffCostFunction<ProjectionPlaneFactorAutoDiff, 2, 7, 7, 7, 1>( new ProjectionPlaneFactorAutoDiff...
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose(); //sqrt_info.setIdentity(); residual = sqrt_info * residual; 1. 2. 3. 4. 5. 6. 7. 计算残差中使用 (设置信息矩阵) AI检测代码解析 ...
tangent_base:正切平面上的任意两个正交基(在构造函数中通过计算?被赋值); 静态数据成员sqrt_info和sum_t:在何时被赋值的呢? class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> { public: ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); v...
静态数据成员sqrt_info和sum_t:在何时被赋值的呢? class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1> { public: ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j); virtual bool Evaluate(double const *const *parameters, double *residuals, ...
首先创建一个 MarginalizationInfo 的指针。 然后不断地向里面添加 ResidualBlockInfo,ResidualBlockInfo和每个具体的残差关联起来,包括MarginalizationFactor, IMUFactor, VisualFactor 添加完成之后调用 preMarginalize() 最终调用marginalize()进行边缘化 下面我们进行逐个分析。
代码 IMUFactor::Evaluate() 中residual 还乘以一个信息矩阵 sqrt_info,这是因为真正的 优化项其实是Mahalanobis 距离: = −1 ,P 是协方差,又因为Ceres 只接受最小二乘 ( ) −1 −1 优化,也就是min , 所以把 做LLT 分解,即 = , 则有: 11 ( ) ( ) = = ′ 令 = 作为新的优化误差, 这样...
The Tmp_V.x and Tmp_V.z, as below My IMU not very good, so I think this result is normal. But, when the initialStructure() completed, the jacobian_speedbias_i is bad. The Assertion data as below. So, I found the residual and sqrt_info are all too large. ...
if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) { ROS_WARN("numerical unstable in preintegration"); //std::cout << sqrt_info << std::endl; ROS_BREAK(); } so time to switch to TX2. I just wonder is there a bug in Eigen past 3.2.9 or the...
Mahalanobis距离其实相当于一个残差加权,协方差大的加权小, 协方差小的加权大, 着重优化那些比较确定的残差. 若写成“sqrt_info.setIdentity()”相当于不加权。 4)协方差 上面提到的IMU协方差P为公式(17)推导的IMU预积分中迭代出来的IMU增量误差的协方差。