哪些网站做的比较好,鞍山网站制作谁家好,wordpress个人网线,合肥哪里有做网页的地方位姿优化及其误差模型 1. calculateFeatureResidualUnitPlane 函数功能2. calculateFeatureResidualUnitPlane 函数功能实现步骤#xff1a;3. 位姿优化误差模型#xff1a; 1. calculateFeatureResidualUnitPlane 函数功能
计算特征点在单位平面上的残差#xff08;residu… 位姿优化及其误差模型 1. calculateFeatureResidualUnitPlane 函数功能2. calculateFeatureResidualUnitPlane 函数功能实现步骤3. 位姿优化误差模型 1. calculateFeatureResidualUnitPlane 函数功能
计算特征点在单位平面上的残差residual并根据给定的测量噪声进行加权处理和误差计算。
输入参数
- f特征点的方向向量。
- xyz_in_world特征点在世界坐标系下的位置。
- T_imu_worldIMU惯性测量单元相对于世界坐标系的变换。
- T_cam_imu相机相对于IMU的变换。
- measurement_sigma测量噪声的标准差。
- robust_weight鲁棒加权函数。
- unwhitened_error非白化误差的指针。
- chi2_error卡方误差的指针。
- HHessian矩阵的指针可选。
- g梯度向量的指针可选。输出参数
- unwhitened_error非白化误差即特征点投影与观测值之间的范数。
- chi2_error卡方误差即误差平方范数的加权一半。
- HHessian矩阵用于优化过程可选。
- g梯度向量用于优化过程可选。
-
void calculateFeatureResidualUnitPlane(const Eigen::Refconst 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*eR*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 指针计算非白化误差并保存即将预测误差的范数作为非白化误差。 unwhitened_error ∥ e ∥ \text{{unwhitened\_error}} \|e\| unwhitened_error∥e∥
对预测误差进行白化处理乘以信息矩阵的平方根即测量噪声的倒数得到白化误差 e。 e whitened R ⋅ e e_{\text{{whitened}}} R \cdot e ewhitenedR⋅e
其中 R R R 是信息矩阵的平方根 R 1 measurement_sigma R \frac{1}{\text{{measurement\_sigma}}} Rmeasurement_sigma1。
计算加权系数 weight使用鲁棒加权函数 robust_weight 对白化误差进行加权。 weight robust_weight ( e whitened ) \text{{weight}} \text{{robust\_weight}}(e_{\text{{whitened}}}) weightrobust_weight(ewhitened)
计算卡方误差 chi2_error即误差平方范数乘以加权系数的一半。 chi2_error 1 2 ∥ e whitened ∥ 2 ⋅ weight \text{{chi2\_error}} \frac{1}{2} \|e_{\text{{whitened}}}\|^2 \cdot \text{{weight}} chi2_error21∥ewhitened∥2⋅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 系统而有所差异。
