SLAM中的非线性优-3D图优化之轴角在Opencv-PNP中的应用（三）

      本讲主要讲解实战篇，代码框架依赖与视觉SLAM十四讲，原版已经将特征提取跟匹配以及PnP求解做好，这里借助这个框架，开发也较快一些，主要展示下，利用第一节的结论，来解析的求解雅可比及残差，并最终优化出最优解，完整代码依然放在个人的码云仓库中

https://gitee.com/zl_vslam/slam_optimizer/tree/master/3d_optimize/ch2https://gitee.com/zl_vslam/slam_optimizer/tree/master/3d_optimize/ch21. 特征提取跟匹配结果展示

总共匹配了79个点对

2. PNP求解结果对比

利用opencv求解pnp，注意opencv中主要使用如下接口：

接口1：

solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false );

接口2： 

solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false, SOLVEPNP_EPNP)

注意这两个接口，在于最后一个参数有没有默认方法，如果不设置的话，默认自带了一个LM优化求解器，而若设置了参数，则不优化，只求解PNP问题，因此这里我们的优化器跟opencv作对比

先看接口1的求解结果

再看接口2的结果

接口2主要以旋转轴表示，可以看到，我们的结果与opencv结果基本上很接近了，也可证明求解的正确性

3. 代码展示

void bundleAdjustment (const vector< Point3f > points_3d,const vector< Point2f > points_2d,const Mat& K,Mat& R, Mat& t )
{cout << "=== PnP Solver with Ceres Optimization ===" << endl;// 生成测试数据vector<Eigen::Vector3d> landmarks;vector<Eigen::Vector3d> bearings;// 添加一些测试点for (int i = 0; i < points_3d.size(); ++i) {landmarks.push_back(Eigen::Vector3d(points_3d[i].x, points_3d[i].y, points_3d[i].z));Point2d norm_pt = pixel2cam ( Point2d(points_2d[i].x, points_2d[i].y), K );bearings.push_back(Eigen::Vector3d(norm_pt.x, norm_pt.y, 1.0));}Matrix3d eigenR = cvMatToEigen<double>(R);Vector3d eigent = cvMatToEigen<double>(t);Mat rotation_vector;cv::Rodrigues(R, rotation_vector);Vector3d eigen_rotation_vector = cvMatToEigen<double>(rotation_vector);// 初始位姿估计Vector3d initial_rotation(eigen_rotation_vector);Vector3d initial_translation(eigent);// 测试rodrigues函数MatrixXd jacobian;MyRodrigues(initial_rotation, eigenR, jacobian);cout << "Initial rotation vector: " << initial_rotation.transpose() << endl;cout << "Rotation matrix:\n" << R << endl;// 构建优化问题ceres::Problem problem;for (size_t i = 0; i < landmarks.size(); ++i) {// 使用解析导数（推荐，因为我们已经计算了精确的雅可比矩阵）ceres::CostFunction* cost_function = new AnalyticReprojectionError(landmarks[i], bearings[i]);// 或者使用自动微分// ceres::CostFunction* cost_function = //     AutoDiffReprojectionError::Create(landmarks[i], bearings[i]);problem.AddResidualBlock(cost_function, nullptr,  // 损失函数initial_rotation.data(), initial_translation.data());}// 配置优化选项ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;options.minimizer_progress_to_stdout = true;options.max_num_iterations = 100;options.function_tolerance = 1e-10;options.gradient_tolerance = 1e-10;options.parameter_tolerance = 1e-10;// 运行优化ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);// 输出结果cout << "\n=== Optimization Results ===" << endl;cout << summary.BriefReport() << endl;cout << "Initial rotation: " << eigen_rotation_vector.transpose() << endl;cout << "Initial translation: " << eigent.transpose() << endl;cout << "Final rotation: " << initial_rotation.transpose() << endl;cout << "Final translation: " << initial_translation.transpose() << endl;// 验证最终的重投影误差MatrixXd dpdrot, dpdt, final_reproj_err;ProjectPoints3D2D(landmarks, bearings, initial_rotation, initial_translation, dpdrot, dpdt, final_reproj_err, false);double total_error = final_reproj_err.norm();cout << "Final reprojection error: " << total_error << endl;}

这里展示了求解主要过程，详细情况请参考源码实现吧，这里算法并不完整，没有设置信息矩阵的鲁棒核函数，但是作为验证，足以说明情况

总结

        本节主要展示了算法验证过程，理论也与之前能够完全匹配上，通过对比ceres-solver优化，跟opencv结果，可以看出，本节效果基本与opencv算的结果，完全一致，足以说明算法的有效性，下一讲，主要是相对位姿之间的残差及雅可比推导