Ceres Solver 协方差的求解思路

他人是一面镜子，保持谦虚的态度

已于 2022-05-30 10:36:42 修改

阅读量2.9k

点赞数

分类专栏：视觉SLAM基础理论文章标签： html 算法 html5

于 2021-11-20 13:37:47 首次发布

本文链接：https://blog.csdn.net/hltt3838/article/details/121438307

版权

视觉SLAM基础理论专栏收录该内容

50 篇文章 92 订阅

订阅专栏

一、理论

(1)、系统观测值是均值为0，协方差为单位矩阵

(2)、系统观测值是均值为0，协方差不是单位矩阵

注意：这个S是观测值的协方差矩阵；比如对于GPS/INS组合的导航系统，S应该是GPS位置观测值的协方差矩阵；

二、SVD分解求矩阵伪逆

C++代码

#include <iostream>
#include <Eigen/SVD>
#include <Eigen/Core>

using namespace std;


MatrixXd Estimator::pinv(MatrixXd  A)
{
   Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);//M=USV*
    double  pinvtoler = 1.e-8; //tolerance
    int row = A.rows();
    int col = A.cols();
    int k = min(row,col);
    MatrixXd X = MatrixXd::Zero(col,row);
    MatrixXd singularValues_inv = svd.singularValues();//奇异值
    MatrixXd singularValues_inv_mat = MatrixXd::Zero(col, row);
    for (long i = 0; i<k; ++i) 
    {
        if (singularValues_inv(i) > pinvtoler)
                singularValues_inv(i) = 1.0 / singularValues_inv(i);
        else singularValues_inv(i) = 0;
    }
    for (long i = 0; i < k; ++i) 
    {
        singularValues_inv_mat(i, i) = singularValues_inv(i);
    }
    
    //X=VS+U*
    X=(svd.matrixV())*(singularValues_inv_mat)*(svd.matrixU().transpose());
     
    return X;
}



int main() {
    // 设置矩阵行数、列数
    const int ROW = 3;
    const int COL = 4;

    // 生成大小 ROW * COL 的随机矩阵
    Eigen::MatrixXd A;
    A = Eigen::MatrixXd::Random(ROW, COL);
    
    // 打印矩阵A
    cout << "矩阵A为：" << endl;
    cout << A << endl;
    
    // 打印矩阵A的伪逆矩阵
    cout << "矩阵A的伪逆为：" << endl;
    cout << pinv(A) << endl;
}

三、ceres-solver 求协方差

      Eigen::Matrix<double,6,6, Eigen::RowMajor> cov_pose = Eigen::Matrix<double,6,6, Eigen::RowMajor>::Zero();
      
           ceres::Covariance::Options cov_options;
      ceres::Covariance covariance(cov_options);
      
      std::vector<std::pair<const double*, const double*>> covariance_blocks;
      for(int i = 0; i <= WINDOW_SIZE; ++i)
      covariance_blocks.push_back(std::make_pair(para_Pose[i], para_Pose[i]));
  
     // 有时候出现 rank问题，忽略一下
      
      Ceres_cov_pose.setZero();
      Eigen::Matrix<double,6,6, Eigen::RowMajor> Ceres_cov_mid = Eigen::Matrix<double,6,6, Eigen::RowMajor>::Zero();
      
     if( covariance.Compute(covariance_blocks, &problem) )
     {
         for(int i = 0; i <= WINDOW_SIZE; ++i)
         {
             covariance.GetCovarianceBlockInTangentSpace(para_Pose[i],para_Pose[i], Ceres_cov_mid.data());  
             Ceres_cov_pose += Ceres_cov_mid; 
         }    

     }
      
     std::cout <<"cov_pose.diagonal() ="<< cov_pose.diagonal() << std::endl;

别人的思路

1. ceres 官网

官网简介链接

2. 案例

double x[3];
double y[2];

Problem problem;
problem.AddParameterBlock(x, 3);
problem.AddParameterBlock(y, 2);
<Build Problem>
<Solve Problem>

Covariance::Options options;
Covariance covariance(options);

vector<pair<const double*, const double*> > covariance_blocks;
covariance_blocks.push_back(make_pair(x, x));
covariance_blocks.push_back(make_pair(y, y));
covariance_blocks.push_back(make_pair(x, y));

CHECK(covariance.Compute(covariance_blocks, &problem));

double covariance_xx[3 * 3];
double covariance_yy[2 * 2];
double covariance_xy[3 * 2];
covariance.GetCovarianceBlock(x, x, covariance_xx)
covariance.GetCovarianceBlock(y, y, covariance_yy)
covariance.GetCovarianceBlock(x, y, covariance_xy)