到此,3D和2D数据准备完成,后面就是使用不同的方法或者是不同的开源库做BA 使用ceres, 开始创建优化问题 在这部分的代码中,最重要的是定义重投影误差函数,重载operator(). 另外,还定义了一个静态的CostFunction创建函数,避免在客户端调用ceres创建。 struct ReprojectionError { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Repro...
(newceres::Grid2D<float>(&vec_pixel_gray_values[0],0,rows_,0,cols_));//双三次插值get_pixel_gray_val.reset(newceres::BiCubicInterpolator<ceres::Grid2D<float>>(*grid2d));}template<typenameT>booloperator()(constT*constso3t,//模型参数,位姿,6维constT*constxyz,//模型参数,3D点,3维T*...
首先讨论图像与图像匹配情况。不同相机匹配原理不一样,2D-2D:对极几何, 3D-3D:ICP, 3D-2D: PnP,本文重点考虑3D-3D的情况。 2.3D-3D: ICP 假设有一对匹配好的3D点: 寻找一个欧氏变换R,t使 ,这个问题使用迭代最近点ICP求解。ICP的求解有两种方式:线性代数的求解(SVD),非线性优化方式(BA). 2.1SVD方法的...
Vector2dErrorTerm(double obs_src_x, double obs_src_y, double obs_src_yaw, double obs_target_x, double obs_target_y,double obs_target_yaw, Eigen::Matrix3d sqrt_information, double weight=1.0) :obs_src_x_(obs_src_x), obs_src_y_(obs_src_y), obs_src_yaw_(obs_src_yaw), obs_ta...
之前用ceres求解了pnp问题,3d-2d构建cost fuction是最小重投影。那3d-3d呢? 也可以用最小重投影.思路是,将第一帧图像坐标系下的3d点经过旋转平移到第二帧图像下,然后通过相机内参求得其投影到图像坐标系下的坐标。第二帧观测到的与之匹配的3d点,也可以进行重投影得到图像坐标系下的坐标。两者就残差即可。
new ceres::BiCubicInterpolator<ceres::Grid2D<float> >(*grid2d)); } template <typename T> bool operator()(const T* const so3t, //模型参数,位姿,6维 const T* const xyz, //模型参数,3D点,3维 T* residual ) const //残差,4*4图像块,16维 ...
Pose3d含有一个p(平移向量),一个q(四元数旋转向量)。poses这个变量描述的是实际机器人的世界坐标位置,是确确实实发生的事实。 关键步骤 // Ceres will take ownership of the pointer.//将需要的参数传入,设置残差,构造costfunction,使用自动求导方式ceres::CostFunction*cost_function=PoseGraph2dErrorTerm::Create...
学习了pose_graph_2d部分因为先学习了3维的pose_graph_3d部分所以这个就比较容易。简单来说就是se2和se3的区别。整个的运行逻辑和3维部分的pose_graph_3d部分是一样的概括为1.设置好两个type分别是poses和constrains的格式并重写>>运算符2.是对于g2o文件的解析和分字符输入到不同的容器中这里部分的内容再read_...
solvePnP(match.pts_3d,match.pts_2d,K,cv::Mat(),rvec,t,false,cv::SOLVEPNP_EPNP);//Pnp 解算输出两帧之间的位姿或者是世界坐标系在相机坐标下的位姿cv::Rodrigues(rvec,R);//对旋转向量进行罗德里格式变换生成旋转矩阵//接下来就是ceres的优化函数PoseOptimization(match.pts_3d,match.pts_2d,rvec,t)...
() * residuals_map;return true; }static ceres::CostFunction *Create(double x_ab, double y_ab, double yaw_ab_radians,const Eigen::Matrix3d &sqrt_information){return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(new PoseGraph2dErrorTerm(x_ab, y_ab,...