在手写BA之前,用ceres实现一个简单的版本。
ceres主要包含三种求导方式:1. 数值求导; 2. 自动求导; 3. 解析求导。
BA_ceres.cpp中实现了基于数值求导的BA,pose用李代数表示。
自动求导中的雅可比依赖模板类,需要将形参,Eigen<double,n,n>以及指数映射函数中的double都改成T。
解析求导除了实现一个Evaluate函数,手写residual和jacobian外,还需要将pose做局部参数化处理。核心是将李代数更新量加入到新的参数化形式中,如四元数+平移,旋转向量+平移。
为什么ceres的局部参数化偏爱四元数+平移? 猜测是两个原因:1. BCH公式; 2. 李代数中的旋转部分是旋转向量,它的旋转角要控制在[-pi,pi)之间。
基于Eigen的BA稍微复杂一些,主要多了两步:1. 非线性优化方法; 2. 矩阵求解。
第一步实现了一个简单的GN方法,第二步用Eigen自带的ldlt求解(一般求解方法如LU,LDLT,LLT,QR,SVD)。
由于观测量较少,需要将points固定,不然更新量被point分摊了(直接将point对应的雅可比赋值为0)。