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;
}
}