VIO标定开源工具Kalibr源码笔记

本文主要是介绍VIO标定开源工具Kalibr源码笔记，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

Kalibr

1. 构造样条，与秦开怀老师的公式比较，秦老师的仅仅分析标量，coeff在最后，U在最前：
   $$r=coeff\ast BasisMatrix\ast U$$

2. coeff需要多个列构成。

3. aslam_backend实现优化。

4. knots即节点，是控制分段的点。

5. Bspline表示IMU的bias，BsplinePose表示Pose。

6. std::pair<double, int> BSpline::ComputeTIndex(double)返回，时刻t所在样条段的时长，和该段对应的索引。

7. std::pair<double, int> BSpline::computeUAndTIndex(double t)返回u和索引。

8. Ceres中CostFunction即factor：ceres::Problem::AddResidualBlock()。

9. 为了估计IMU和相机的时延，需要从IMU的Pose样条取出多段，即不单是采样时刻所在的一段，

int bufferLeft, bufferRight;
//bufferL/R是BasisMatrix的下标，
bufferRight = poseSpline.segmentIndex(frameTime + config_.timeOffsetPadding_);
bufferLeft = poseSpline.segmentIndex(frameTime - config_.timeOffsetPadding_);
// leftCoeff是coeff的多个列的下标，
Eigen::VectorXi leftCoeff = poseSpline.localVvCoefficientVectorIndices(
(poseSpline.timeInterval(bufferLeft).first +
poseSpline.timeInterval(bufferLeft).second) /
2.0);
Eigen::VectorXi rightCoeff = poseSpline.localVvCoefficientVectorIndices(
(poseSpline.timeInterval(bufferRight).first +
poseSpline.timeInterval(bufferRight).second) /
2.0);
// fill the vector with all the indices
int l = leftCoeff(0);
int r = rightCoeff(rightCoeff.size() - 1);

9. 在重投影误差中，coeff的列数为，
   int valid_coeff = basisMatrices_.size() + pose_splineOrder_ - 1;

根据采样时刻不断更新样条段，并构造样条方程。

int valid_coeff = basisMatrices_.size() + pose_splineOrder_ - 1;
Eigen::MatrixXd pose_coeffs(6, valid_coeff);
for (int i = 0; i < valid_coeff; i++, index++) {Eigen::Map<const Eigen::Matrix<double, 6, 1>> coeff_i(parameters[index]);pose_coeffs.col(i) = coeff_i;
}
std::vector<double>::const_iterator it =std::upper_bound(knots_.begin(), knots_.end(), observationTime);
int segment_id = it - knots_.begin() - 1;
Eigen::MatrixXd coeff = pose_coeffs.middleCols(segment_id, pose_splineOrder_);
Eigen::MatrixXd Basic_matrix = basisMatrices_[segment_id];
Eigen::VectorXd pose_Bt_u_ = Basic_matrix.transpose() * u;
Eigen::VectorXd value = coeff * pose_Bt_u_;
Eigen::Vector3d twb = value.head<3>();
Eigen::Matrix3d Rwb = math::expM(-value.tail<3>());

10. kalibr使用的旋转矩阵转李代数，即轴角，添加了负号。

Eigen::Vector3d RotationVector::rotationMatrixToParameters(const Eigen::Matrix3d& C) const {Eigen::Vector3d p;// Sometimes, because of roundoff error, the value of tr ends up outside// the valid range of arccos. Truncate to the valid range.double tr =std::max(-1.0, std::min((C(0, 0) + C(1, 1) + C(2, 2) - 1.0) * 0.5, 1.0));double a = acos(tr);if (fabs(a) < 1e-14) {return Eigen::Vector3d::Zero();}p[0] = (C(2, 1) - C(1, 2));p[1] = (C(0, 2) - C(2, 0));p[2] = (C(1, 0) - C(0, 1));double n2 = p.norm();if (fabs(n2) < 1e-14) return Eigen::Vector3d::Zero();
//符号double scale = -a / n2;p = scale * p;return p;
}

IMU bias

class BSplineSegmentMotionError : public ceres::CostFunction {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWBSplineSegmentMotionError(bsplines::BSpline biasSpline, const int segment,const Eigen::MatrixXd &W) {bias_splineOrder_ = biasSpline.splineOrder();Eigen::MatrixXd Q = biasSpline.segmentQuadraticIntegral(W, segment, 1); // 1 only for bias random walk errorEigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(Q);double eps = 1e-10;Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));Eigen::VectorXd S_inv =Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));Eigen::VectorXd S_sqrt = S.cwiseSqrt();Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();S_sqrt_Pt_ = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();/* S_inv_sqrt_Pt_ = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); */}virtual bool Evaluate(double const *const *parameters, double *residuals,double **jacobians) const {Eigen::VectorXd bias_coeffs(3 * bias_splineOrder_);for (int i = 0; i < bias_splineOrder_; i++) {Eigen::Map<const Eigen::Vector3d> coeff_i(parameters[i]);bias_coeffs.segment<3>(3 * i) = coeff_i;}Eigen::Map<Eigen::VectorXd>(residuals, 3 * bias_splineOrder_) =S_sqrt_Pt_ * bias_coeffs;if (jacobians) {for (int i = 0; i < bias_splineOrder_; i++) {if (jacobians[i]) {Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>jacobian_i(jacobians[i], 3 * bias_splineOrder_, 3);jacobian_i = S_sqrt_Pt_.middleCols<3>(3 * i);}}}return true;}void setVariableResidualDim(const std::vector<int> &parameter_block_sizes,const int residualDim) {/* *mutable_parameter_block_sizes() = parameter_block_sizes; */for (auto &isize : parameter_block_sizes)mutable_parameter_block_sizes()->push_back(isize);set_num_residuals(residualDim);}protected:Eigen::MatrixXd S_sqrt_Pt_;int bias_splineOrder_;
};

Ceres Problem问题构建

1. ceres::Solver::Problem::AddParameterBlock(double *, int) ，优化参数，参数个数

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
poseCoefficientVecs(6, poseSpline.numVvCoefficients());
for (int i = 0; i < poseCoefficientVecs.cols(); i++) {
poseCoefficientVecs.col(i) = poseSpline.fixedSizeVvCoefficientVector<6>(i);
problem.AddParameterBlock(&poseCoefficientVecs(0, i), 6);
}

2. 声明std::vector<double*> localParameters ，std::vector<int> parameter_block_sizes，

Eigen::VectorXi dvidxs1 =
poseSpline.localVvCoefficientVectorIndices(timestamp);
for (int i = 0; i < dvidxs1.size(); i++) {localParameters.emplace_back(&poseCoefficientVecs(0, dvidxs1[i]));parameter_block_sizes.emplace_back(6);
}

3. 声明factor

BSplineGyroscopeError *factor = new BSplineGyroscopeError(imu_ptr->gyro_, gyro_noise, poseSpline, biasSpline, timestamp);

factor中定义

void setVariableResidualDim(const std::vector<int> parameter_block_sizes, const int residualDim) {
/* mutable_parameter_block_sizes() = parameter_block_sizes; */
for (auto &isize : parameter_block_sizes) {mutable_parameter_block_sizes()->push_back(isize);
}
set_num_residuals(residualDim);
}

4. ceres::Solver::Problem::AddResidualBlock(<factor>, <loss_function>, <local_parameters>)
5. ceres::Solve()

这篇关于VIO标定开源工具Kalibr源码笔记的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---