视觉slam十四讲第ch7 PNP 3d2d手写位姿估计代码详解
最近在学习视觉slam十四讲,不管是书上内容还是高博视频,描述的都很细致。关于此书的资料博客也可以比较方便的在网上找到。但是因为此书除了第二版,一些代码什么的是第二版独有的,网上资料还没那么丰富。
今天在学到手写位姿估计的内容时就发现内容和代码都似懂非懂,网上的资料又不充分,因此花了很多时间去解决,现把pose_estimation_3d2d.cpp的代码手写高斯牛顿法片段和我的注释版附上,不一定对,但是应该会对初学者有一些帮助。
void bundleAdjustmentGaussNewton(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &