ceres-solver无脑搞定协方差小技巧

研究惯性导航的小伙伴都比较关心一件事，那就是协方差怎么来，或者说信息矩阵如何得到，当然协方差的用处我就不多说了。
我之前的做法，都是自己临时拼一个信息矩阵，之后求个逆，最近发现了一个更为简单的方法。
ceres-solver中提供了Covariance Estimation方法类，用于得到最优解的协方差。
理论我就不多写了，省得论文查重过不了。。
总的来说调用很简单。
比如pose的协方差如何得到呢？
pose的维度是6，所以协方差是6*6那么大，因此写两行：

Eigen::Matrix<double, 6, 6, Eigen::RowMajor> cov_pose = Eigen::Matrix<double, 6, 6, Eigen::RowMajor>::Zero();


// 然后正常solve你的problem
...

// solve完成后，请打代码
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(pose.data(), pose.data())); //你要算pose的协方差，请把要算的pose的数据指针这样排好
//要算什么就加入什么，当然也可以无脑都搞，或许就有点慢而已
covariance.Compute(covariance_blocks, &problem);
covariance.GetCovarianceBlockInTangentSpace(pose.data(),pose.data(), cov_pose.data()); //这里得调用切空间那个接口，因为pose的协方差定义在切空间上

于是你就得到了协方差了。。