求解結(jié)果如下：

mat 初始化，eigenvalue初始化

Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

Eigen::Matrix<float,3,1> vd_3d;v_3d << 3, 2, 1;

求解目標(biāo)函數(shù)結(jié)構(gòu)體構(gòu)造與實(shí)例

 struct CurveFittingCost {CurveFittingCost(Point2d point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}template<typename T>bool operator() (const T *const camera_pose,T *residual)const{T point[3];point[0] = T(_point3d.depth_x);point[1] = T(_point3d.depth_x);point[2] = T(_point3d.depth_z);T p[3];ceres::AngleAxisRotatePoint(camera_pose, point, p);p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] += camera_pose[5];//p[0] / p[2],p[1] / p[2] is norm coordinate，the real depth id p[2]T x_p = p[0] / p[2];T y_p = p[1] / p[2];T predicted_x = x_p * static_cast<T>(camera_matrix_temp(0, 0)) + static_cast<T>(camera_matrix_temp(0, 2));T predicted_y = y_p * static_cast<T>(camera_matrix_temp(1, 1)) + static_cast<T>(camera_matrix_temp(1, 2));residual[0] = T(_point2d.x) - predicted_x;residual[1] = T(_point2d.y) - predicted_y;return true;}static ceres::CostFunction* Create( Point2d point2d, Point3f point3d) {return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(new CurveFittingCost(point2d, point3d)));}const Point2d _point2d;const Point3f _point3d;};struct CurveFittingCost {CurveFittingCost(Point2f point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}template<typename T>bool operator() (const T *const camera_pose,T *residual)const{T point[3];point[0] = T(_point3d.depth_x);point[1] = T(_point3d.depth_y);point[2] = T(_point3d.depth_z);T p[3];ceres::AngleAxisRotatePoint(camera_pose, point, p);p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];p[0] / p[2],p[1] / p[2] is norm coordinate，the real depth id p[2]T predicted_x = p[0] / p[2] * camera_matrix(0,0) + camera_matrix(0,2);T predicted_y = p[1] / p[2] * camera_matrix(1,1) + camera_matrix(1,2);residual[0] = T(_point2d.x) - predicted_x;residual[1] = T(_point2d.y) - predicted_y;return true;}static ceres::CostFunction* Create(const Point2f point2d, const Point3f point3d) {return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(new CurveFittingCost(point2d, point3d)));}const Point2f _point2d;const Point3f _point3d;};struct PnPCeres{PnPCeres(Point2f uv, Point3f xyz) : _uv(uv), _xyz(xyz) {}// 殘差的計(jì)算template <typename T>bool operator() (const T *const camera, // 位姿參數(shù)，有6維T* residual) const     // 殘差{T p[3];T point[3];point[0] = T(_xyz.x);point[1] = T(_xyz.y);point[2] = T(_xyz.z);ceres::AngleAxisRotatePoint(camera, point, p);//計(jì)算RPp[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];T xp = p[0] / p[2];T yp = p[1] / p[2];//xp,yp是歸一化坐標(biāo)，深度為p[2]T u_ = xp*camera_matrix(0, 0) + camera_matrix(0, 2);T v_ = yp*camera_matrix(1, 1) + camera_matrix(1, 2);residual[0] = T(_uv.x) - u_;residual[1] = T(_uv.y) - v_;return true;}static ceres::CostFunction* Create(const Point2f uv, const Point3f xyz) {return (new ceres::AutoDiffCostFunction<PnPCeres, 2, 6>(new PnPCeres(uv, xyz)));}const Point2f _uv;const Point3f _xyz;};

目標(biāo)函數(shù)調(diào)用方式

struct CurveFittingCost {CurveFittingCost(Point2d point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}template<typename T>bool operator() (const T *const camera_pose,T *residual)const{T point[3];point[0] = T(_point3d.depth_x);point[1] = T(_point3d.depth_x);point[2] = T(_point3d.depth_z);T p[3];ceres::AngleAxisRotatePoint(camera_pose, point, p);p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] += camera_pose[5];//p[0] / p[2],p[1] / p[2] is norm coordinate，the real depth id p[2]T x_p = p[0] / p[2];T y_p = p[1] / p[2];T predicted_x = x_p * static_cast<T>(camera_matrix_temp(0, 0)) + static_cast<T>(camera_matrix_temp(0, 2));T predicted_y = y_p * static_cast<T>(camera_matrix_temp(1, 1)) + static_cast<T>(camera_matrix_temp(1, 2));residual[0] = T(_point2d.x) - predicted_x;residual[1] = T(_point2d.y) - predicted_y;return true;}static ceres::CostFunction* Create( Point2d point2d, Point3f point3d) {return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(new CurveFittingCost(point2d, point3d)));}const Point2d _point2d;const Point3f _point3d;
};

總結(jié)

以上是生活随笔為你收集整理的【SLAM后端】—— ceres优化相机位姿求解的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。