我有一个简单的捆绑调整功能，使用 g2o 库来优化相机姿势和 3d 地图点。我传入两个KeyFrames，每个包含：
std::vector<cv::Point2d> vObs_pos; //the 2d coordinates that align to the 3d map points
int id; // the frame id
cv::Mat Rt; // the pose as a 4x4 matrix
// Calibration parameters
float fx, fy, cx, cy;
以及 a vectorof Mappoint，其中包含：
cv::Point3f pos; // the 3d position
long long id; // the map point id
std::map<KeyFrame*, size_t> mObs; //a list of Keyframe/2d point id pairs
我的功能是：
void BundleAdjustment::Solve(const std::vector<KeyFrame *> &vpKFs, const std::vector<MapPoint *> &allMapPoints)
{
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(true);
std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
linearSolver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>>();
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
optimizer.setAlgorithm(solver);
int num_cameras_ = vpKFs.size();
int num_points_ = allMapPoints.size();
const float thHuber2D = sqrt(5.99);
//camera poses
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKF = vpKFs[i];
g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKF->Rt));
vSE3->setId(pKF->id);
if (pKF->id == 0)
{
vSE3->setFixed(true);
}
optimizer.addVertex(vSE3);
}
// set point vertex
for (size_t i = 0; i < allMapPoints.size(); i++)
{
MapPoint* pMP = allMapPoints[i];
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
g2o::Vector3 pp(pMP->pos.x, pMP->pos.y, pMP->pos.z);
vPoint->setEstimate(pp);
vPoint->setId(i + num_cameras_);
optimizer.addVertex(vPoint);
//
//SET EDGES
const map<KeyFrame*, size_t> observations = pMP->mObs;
for (std::map<KeyFrame *, size_t>::const_iterator mit = observations.begin(); mit != observations.end(); mit++)
{
KeyFrame *pKF = mit->first;
const cv::Point2f &kpUn = pKF->vObs_pos[mit->second];
Eigen::Matrix<double, 2, 1> obs;
obs << kpUn.x, kpUn.y;
g2o::EdgeSE3ProjectXYZ *e = new g2o::EdgeSE3ProjectXYZ();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(thHuber2D);
e->setRobustKernel(rk);
const int camera_id = pKF->id;
int point_id = i + num_cameras_;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(point_id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(camera_id)));
e->setMeasurement(obs);
e->setInformation(Eigen::Matrix2d::Identity());
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
optimizer.addEdge(e);
}
}
// start optimization
std::cout << "Optimization starts..." << std::endl;
optimizer.initializeOptimization();
optimizer.setVerbose(true);
optimizer.optimize(20);
std::cout << "Optimization completed!" << std::endl;
}
我运行该功能，它似乎可以工作，打印：
Optimization starts...
iteration= 0 chi2= 751902.547901 time= 0.118062 cumTime= 0.118062 edges= 454 schur= 0 lambda= 40.936130 levenbergIter= 6
iteration= 1 chi2= 748570.165390 time= 0.0783672 cumTime= 0.196429 edges= 454 schur= 0
lambda= 873.304112 levenbergIter= 4
iteration= 2 chi2= 739010.123008 time= 0.0199937 cumTime= 0.216423 edges= 454 schur= 0
lambda= 291.101371 levenbergIter= 1
iteration= 3 chi2= 725682.849983 time= 0.0391275 cumTime= 0.25555 edges= 454 schur= 0
lambda= 194.067581 levenbergIter= 2
iteration= 4 chi2= 720377.943229 time= 0.0586551 cumTime= 0.314205 edges= 454 schur= 0
lambda= 517.513548 levenbergIter= 3
iteration= 5 chi2= 714174.854659 time= 0.039606 cumTime= 0.353811 edges= 454 schur= 0 lambda= 690.018064 levenbergIter= 2
iteration= 6 chi2= 708794.450845 time= 0.0410522 cumTime= 0.394864 edges= 454 schur= 0
lambda= 491.458828 levenbergIter= 2
iteration= 7 chi2= 703574.671138 time= 0.0409294 cumTime= 0.435793 edges= 454 schur= 0
lambda= 655.278438 levenbergIter= 2
iteration= 8 chi2= 698092.802055 time= 0.0395358 cumTime= 0.475329 edges= 454 schur= 0
lambda= 517.076870 levenbergIter= 2
iteration= 9 chi2= 696224.050949 time= 0.0589929 cumTime= 0.534322 edges= 454 schur= 0
lambda= 1378.871654 levenbergIter= 3
iteration= 10 chi2= 693660.045281 time= 0.0394453 cumTime= 0.573767 edges= 454 schur= 0
lambda= 919.247769 levenbergIter= 2
iteration= 11 chi2= 690353.709795 time= 0.0395756 cumTime= 0.613343 edges= 454 schur= 0
lambda= 1225.663693 levenbergIter= 2
iteration= 12 chi2= 685633.855487 time= 0.0201159 cumTime= 0.633458 edges= 454 schur= 0
lambda= 817.109128 levenbergIter= 1
iteration= 13 chi2= 681798.350668 time= 0.039852 cumTime= 0.67331 edges= 454 schur= 0 lambda= 685.975324 levenbergIter= 2
iteration= 14 chi2= 678163.586494 time= 0.0398645 cumTime= 0.713175 edges= 454 schur= 0
lambda= 914.633765 levenbergIter= 2
iteration= 15 chi2= 675402.095013 time= 0.0202596 cumTime= 0.733435 edges= 454 schur= 0
lambda= 609.755843 levenbergIter= 1
iteration= 16 chi2= 671423.770362 time= 0.0399762 cumTime= 0.773411 edges= 454 schur= 0
lambda= 813.007791 levenbergIter= 2
iteration= 17 chi2= 668154.458614 time= 0.0398402 cumTime= 0.813251 edges= 454 schur= 0
lambda= 1084.010388 levenbergIter= 2
iteration= 18 chi2= 665945.177033 time= 0.0201547 cumTime= 0.833406 edges= 454 schur= 0
lambda= 722.673592 levenbergIter= 1
iteration= 19 chi2= 662338.490227 time= 0.039615 cumTime= 0.873021 edges= 454 schur= 0 lambda= 963.564789 levenbergIter= 2
Optimization completed!
但是我怎样才能验证这些结果呢？g2o 中有没有办法在运行优化之前和之后打印重投影错误？或者我在上述结果中需要的信息是否在某处？