セレスは事後推定をどのように実行しますか
ceres ランドマークの共分散行列を求める
次のコードは自分で 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 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;
}
}