ceres 如何进行后验估计

ceres 求landmark的协方差矩阵

以下代码是自己在colmap 中实现的

/// 计算每个3D points的协方差
 for (const image_t image_id :config_.Images()) {
   Image& image = reconstruction->Image(image_id);
   for (const Point2D& point2D : image.Points2D()) {
     if (!point2D.HasPoint3D()) {
       continue;
     }
       Point3D& point3D = reconstruction->Point3D(point2D.Point3DId());
       Eigen::Matrix<double,3,3,Eigen::RowMajor> cov_tvec =Eigen::Matrix<double,3,3,Eigen::RowMajor>::Zero();
       ceres::Covariance::Options cov_options;
       ceres::Covariance covariance(cov_options);
       std::vector<std::pair<const double *,const double *> >covariance_blocks;
//       covariance_blocks.push_back(std::make_pair(image.Tvec().data(),image.Tvec().data()));
       covariance_blocks.push_back(std::make_pair(point3D.XYZ().data(),point3D.XYZ().data()));
       covariance.Compute(covariance_blocks,problem_.get());
     // 在切空间get 协方差矩阵
       covariance.GetCovarianceBlockInTangentSpace(point3D.XYZ().data(),point3D.XYZ().data(),cov_tvec.data());
       std::cout<<"image_id:"<<image_id<<" point3D_id: "<<point2D.Point3DId()<<" covariance:\n"<<cov_tvec<<"\n";
   }
 }

ceres 四元数求导

• 关于manifold 和tangent space
  许多优化问题,特别是传感器融合算法,都是在manifold 空间建模,如以四元数为代表的方向传感器.但是在优化 Manifold 上的变量时需要注意，Manifold 上变量是 over parameterized，即 Manifold 上变量的维度大于其自由度。这就造成变量建立的误差方之间程存在约束,如果直接求导会很困难,故要转换在切空间求导.
  ceres LocalParameterization 上原话如下:

Working in the tangent space reduces not just the computational complexity of the optimization algorithm, but also improves the numerical behaviour of the algorithm.

• 对四元数求导的代码如下:

// ceres local_parameterization.cc 源码
bool QuaternionParameterization::Plus(const double* x,
                                      const double* delta,
                                      double* x_plus_delta) const {
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0) {
    const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
    double q_delta[4];
    q_delta[0] = cos(norm_delta);
    q_delta[1] = sin_delta_by_delta * delta[0];
    q_delta[2] = sin_delta_by_delta * delta[1];
    q_delta[3] = sin_delta_by_delta * delta[2];
    QuaternionProduct(q_delta, x, x_plus_delta);
  } else {
    for (int i = 0; i < 4; ++i) {
      x_plus_delta[i] = x[i];
    }
  }
  return true;
}
bool QuaternionParameterization::ComputeJacobian(const double* x,
                                                 double* jacobian) const {
  jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
  jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
  jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
  jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT
  return true;
}
//////轴角到四元数,可以看到QuaternionParameterization::Plus操作的半轴角,需要注意
template<typename T>
inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
  const T& a0 = angle_axis[0];
  const T& a1 = angle_axis[1];
  const T& a2 = angle_axis[2];
  const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;

  // For points not at the origin, the full conversion is numerically stable.
  if (theta_squared > T(0.0)) {
    const T theta = sqrt(theta_squared);
    const T half_theta = theta * T(0.5);
    const T k = sin(half_theta) / theta;
    quaternion[0] = cos(half_theta);
    quaternion[1] = a0 * k;
    quaternion[2] = a1 * k;
    quaternion[3] = a2 * k;
  } else {
    // At the origin, sqrt() will produce NaN in the derivative since
    // the argument is zero.  By approximating with a Taylor series,
    // and truncating at one term, the value and first derivatives will be
    // computed correctly when Jets are used.
    const T k(0.5);
    quaternion[0] = T(1.0);
    quaternion[1] = a0 * k;
    quaternion[2] = a1 * k;
    quaternion[3] = a2 * k;
  }
}