【SLAM后端】—— ceres优化相机位姿求解

求解结果如下：

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;

求解目标函数结构体构造与实例

 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) {}// 残差的计算template <typename T>bool operator() (const T *const camera, // 位姿参数，有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);//计算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是归一化坐标，深度为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;};

目标函数调用方式

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;
};

【SLAM后端】—— ceres优化相机位姿求解相关推荐

相机位姿求解——P3P问题
1.位姿求解是计算机视觉中经常遇到的,Perspective-n-Points, PnP(P3P)提供了一种解决方案,它是一种由3D-2D的位姿求解方式,即需要已知匹配的3D点和图像2D点.目前遇到的 ...
SLAM中相机位姿求解（李群李代数）
前言 slam中一个关键问题之一就是求解相机的位姿,人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投影误差,3D几何误差等等,希望使得误差最小,进而求得比较准确的相机位姿.举一个重投影例子: ...
基于图像的三维重建——P3P求解相机位姿(5)
文章目录 P3P简介直接线性变换法 Kneip算法算法原理及流程基于RANSAC的Kneip算法 P3P简介相机位姿求解是SFM中必要的一步,而P3P问题的目的就是通过三维点(3对)对应关系确 ...
SLAM学习--------相机位姿表示-李群李代数
slam 求解相机的位姿求解核心思想:将有约束的李群问题转换成无约束的李代数问题,然后使用高斯牛顿算法或者LM(列文伯格-马夸尔特法)求解. 人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投 ...
【SLAM文献】2017-2018 CVPR ICCV ECCV 相机位姿估计、视觉定位、SLAM相关论文综述
作者:变胖是梦想2014 来源链接:https://www.jianshu.com/p/22151f39b50c 目录 CVPR-2018 references CVPR-2017 reference ...
SLAM后端优化之-核函数
1.核函数作用:保证每条边的误差不会大的没边,掩盖掉其他的边在SLAM后端优化中,BA优化了所有的相机姿态和所有路标点,使用的最小化误差项作的二范数平方和作为目标函数:当我们的误差来源特别大的时候: ...
视觉SLAM笔记（56）位姿图优化
视觉SLAM笔记(56) 位姿图优化 1. g2o 原生位姿图 2. 李代数上的位姿图优化 3. 关于后端优化 1. g2o 原生位姿图下面来演示使用 g2o 进行位姿图优化首先,用 g2o_vi ...
激光SLAM后端优化总结之卡尔曼滤波
激光SLAM后端优化总结之卡尔曼滤波一.贝叶斯滤波 1.1 状态估计模型 1.2 公式推导二.卡尔曼滤波(KF) 三.扩展卡尔曼滤波(EKF) 一.贝叶斯滤波 1.1 状态估计模型 1.2 公式推 ...
三维空间刚体运动5：详解SLAM中显示机器人运动轨迹及相机位姿（原理流程）
三维空间刚体运动5:详解SLAM中显示机器人运动轨迹及相机位姿(原理流程) 一.显示运动轨迹原理讲解二.前期准备三.git管理子模块及克隆源代码 1.学习使用Git Submodule 2.克隆源 ...

【SLAM后端】—— ceres优化相机位姿求解

求解结果如下：

mat 初始化，eigenvalue初始化

求解目标函数结构体构造与实例

目标函数调用方式

【SLAM后端】—— ceres优化相机位姿求解相关推荐

最新文章

热门文章