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_NEW

  BSplineSegmentMotionError(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 error

    Eigen::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()