【SVO】位姿优化及其误差模型,akg k77

文件名：【SVO】位姿优化及其误差模型,akg k77 【SVO】位姿优化及其误差模型

位姿优化及其误差模型 1. calculateFeatureResidualUnitPlane 函数功能2. calculateFeatureResidualUnitPlane 函数功能实现步骤：3. 位姿优化误差模型：

1. calculateFeatureResidualUnitPlane 函数功能

计算特征点在单位平面上的残差（residual），并根据给定的测量噪声进行加权处理和误差计算。

输入参数：- `f`：特征点的方向向量。- `xyz_in_world`：特征点在世界坐标系下的位置。- `T_imu_world`：IMU（惯性测量单元）相对于世界坐标系的变换。- `T_cam_imu`：相机相对于IMU的变换。- `measurement_sigma`：测量噪声的标准差。- `robust_weight`：鲁棒加权函数。- `unwhitened_error`：非白化误差的指针。- `chi2_error`：卡方误差的指针。- `H`：Hessian矩阵的指针（可选）。- `g`：梯度向量的指针（可选）。输出参数：- `unwhitened_error`：非白化误差，即特征点投影与观测值之间的范数。- `chi2_error`：卡方误差，即误差平方范数的加权一半。- `H`：Hessian矩阵，用于优化过程（可选）。- `g`：梯度向量，用于优化过程（可选）。- void calculateFeatureResidualUnitPlane(const Eigen::Ref<const BearingVector> &f,const Position &xyz_in_world, const Transformation &T_imu_world,const Transformation &T_cam_imu, double measurement_sigma,const PoseOptimizer::RobustWeightFunction &robust_weight, double *unwhitened_error,double *chi2_error, PoseOptimizer::HessianMatrix *H, PoseOptimizer::GradientVector *g) {const Vector3d xyz_in_imu(T_imu_world * xyz_in_world);const Vector3d xyz_in_cam(T_cam_imu * xyz_in_imu);// Prediction error.Eigen::Vector2d e = vk::project2(f) - vk::project2(xyz_in_cam);if (unwhitened_error)*unwhitened_error = e.norm();// Whiten error: R*e, where R is the square root of information matrix// (1/sigma).double R = 1.0 / measurement_sigma;e *= R;// M-estimator weightingdouble weight = robust_weight.weight(e.norm());// Compute log-likelihood : 1/(2*sigma^2)*(z-h(x))^2 = 1/2*e'R'*R*e*chi2_error = 0.5 * e.squaredNorm() * weight;if (H && g) {// compute jacobianPoseOptimizer::Matrix26d J_proj;Frame::jacobian_xyz2uv_imu(T_cam_imu, xyz_in_imu, J_proj);J_proj *= R;H->noalias() += J_proj.transpose() * J_proj * weight;g->noalias() -= J_proj.transpose() * e * weight;}} 2. calculateFeatureResidualUnitPlane 函数功能实现步骤： 将特征点投影到相机坐标系下，得到 xyz_in_cam。计算预测误差 e，即将特征点在相机坐标系下的投影减去 f（特征点在像素坐标系下的观测值）。如果传入了 unwhitened_error 指针，计算非白化误差并保存，即将预测误差的范数作为非白化误差。

$$\text{unwhitened\_error}=\Vert e\Vert$$

对预测误差进行白化处理，乘以信息矩阵的平方根（即测量噪声的倒数），得到白化误差 e。

$$e_{\text{whitened}}=R\cdot e$$

其中 $R$ 是信息矩阵的平方根，$R=\frac{1}{\text{measurement\_sigma}}$。

计算加权系数 weight，使用鲁棒加权函数 robust_weight 对白化误差进行加权。

$$\text{weight}=\text{robust\_weight}(e_{\text{whitened}})$$

计算卡方误差 chi2_error，即误差平方范数乘以加权系数的一半。

$$\text{chi2\_error}=\frac{1}{2}\Vert e_{\text{whitened}}{\Vert }^2\cdot \text{weight}$$

如果传入了 H 和 g 指针，计算雅可比矩阵 J_proj，并根据 J_proj、白化误差 e 和加权系数 weight 来更新 H 矩阵和 g 向量。 3. 位姿优化误差模型：

enum class ErrorType { kUnitPlane, kBearingVectorDiff, kImagePlane }; 用于表示错误类型。它定义了三个枚举值：kUnitPlane、kBearingVectorDiff 和 kImagePlane。

kUnitPlane：表示单位平面误差。在 SVO 中，相机位姿估计通常使用单位平面误差来优化。该误差指的是特征点在三维空间中的投影点与相机成像平面上的对应点之间的差异。

kBearingVectorDiff：表示方向向量差异误差。在 SVO 中，为了提高位姿估计的准确性，常常利用相机的方向向量进行优化。该误差指的是两个方向向量之间的差异。

kImagePlane：表示图像平面误差。在 SVO 中，为了实现视觉里程计（Visual Odometry），需要匹配和跟踪图像中的特征点。该误差指的是特征点在当前帧和参考帧之间的图像平面投影差异。

通过定义这些错误类型的枚举值，可以在不同的阶段和过程中标识和处理不同类型的误差。这有助于优化相机位姿以及实现高精度和鲁棒性的视觉里程计算法。请注意，以上提到的内容仅针对常见情况，具体实现和使用方式可能因不同的 SVO 系统而有所差异。