SLAM中的非线性优-3D图优化之轴角在Opencv-PNP中的应用（一）

       从本节开始讲解3D图优化，3D的优化算法位姿表示主要以轴角，四元数以及李群李代数表示为主，目前先从PNP中的轴角开始，先看问题描述；

总结：

上述即为pnp求解的基本过程，详细雅可比请继续往下看，先看源码部分

void projectPoints(const std::vector<Point3d> &inlier_landmarks,const std::vector<Point3d> &inlier_bearings,const Vector3d &rotation_vector,const Vector3d &translation_vector,MatrixXd &dpdrot, MatrixXd &dpdt,Eigen::MatrixXd &reproject_err,const bool &calc_derivative)
{double k[14] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, fx = fx_, fy = fy_, cx = cx_, cy = cy_;Matrix3d matR;Vector3d r, t = translation_vector;MatrixXd dRdr = Eigen::MatrixXd(3, 9);Matrix3d matTilt = Matrix3d::Identity();rodrigues(rotation_vector, matR, dRdr);uint32_t i, j, count;count = inlier_landmarks.size();reproject_err = Eigen::MatrixXd(2 * count, 1);for (i = 0; i < count; i++){// 世界系下3D点转相机系const Vector3d &pos_w = inlier_landmarks.at(i);Point3d pc = matR * pos_w + t;float64_t X = pos_w[0], Y = pos_w[1], Z = pos_w[2];float64_t x = pc[0];float64_t y = pc[1];float64_t z = pc[2];float64_t r2, r4, r6, a1, a2, a3, cdist, icdist2;float64_t xd, yd, xd0, yd0, invProj;Vector3d vecTilt;Vector3d dVecTilt;Eigen::Matrix2d dMatTilt;Vector2d dXdYd;// 归一化坐标z = z ? 1. / z : 1;x *= z;y *= z;// 径向畸变r2 = x * x + y * y;r4 = r2 * r2;r6 = r4 * r2;a1 = 2 * x * y;a2 = r2 + 2 * x * x;a3 = r2 + 2 * y * y;cdist = 1 + k[0] * r2 + k[1] * r4 + k[4] * r6;// 切向畸变icdist2 = 1. / (1 + k[5] * r2 + k[6] * r4 + k[7] * r6);xd0 = x * cdist * icdist2 + k[2] * a1 + k[3] * a2 + k[8] * r2 + k[9] * r4;yd0 = y * cdist * icdist2 + k[2] * a3 + k[3] * a1 + k[10] * r2 + k[11] * r4;// additional distortion by projecting onto a tilt planevecTilt = matTilt * Vector3d(xd0, yd0, 1);invProj = vecTilt(2) ? 1. / vecTilt(2) : 1;xd = invProj * vecTilt(0);yd = invProj * vecTilt(1);double ptx = xd * fx + cx;double pty = yd * fy + cy;const Vector3d &norm_point = inlier_bearings.at(i);// 重投影误差reproject_err(2 * i, 0) = ptx - (norm_point[0] / norm_point[2] * fx + cx);reproject_err(2 * i + 1, 0) = pty - (norm_point[1] / norm_point[2] * fy + cy);if (calc_derivative){for (int row = 0; row < 2; ++row)for (int col = 0; col < 2; ++col)dMatTilt(row, col) = matTilt(row, col) * vecTilt(2) - matTilt(2, col) * vecTilt(row);double invProjSquare = (invProj * invProj);dMatTilt *= invProjSquare;// dpdt 对平移的导数double dxdt[] = {z, 0, -x * z}, dydt[] = {0, z, -y * z};for (j = 0; j < 3; j++){double dr2dt = 2 * x * dxdt[j] + 2 * y * dydt[j];double dcdist_dt = k[0] * dr2dt + 2 * k[1] * r2 * dr2dt + 3 * k[4] * r4 * dr2dt;double dicdist2_dt = -icdist2 * icdist2 * (k[5] * dr2dt + 2 * k[6] * r2 * dr2dt + 3 * k[7] * r4 * dr2dt);double da1dt = 2 * (x * dydt[j] + y * dxdt[j]);double dmxdt = (dxdt[j] * cdist * icdist2 + x * dcdist_dt * icdist2 + x * cdist * dicdist2_dt +k[2] * da1dt + k[3] * (dr2dt + 4 * x * dxdt[j]) + k[8] * dr2dt + 2 * r2 * k[9] * dr2dt);double dmydt = (dydt[j] * cdist * icdist2 + y * dcdist_dt * icdist2 + y * cdist * dicdist2_dt +k[2] * (dr2dt + 4 * y * dydt[j]) + k[3] * da1dt + k[10] * dr2dt + 2 * r2 * k[11] * dr2dt);dXdYd = dMatTilt * Vector2d(dmxdt, dmydt);dpdt(2 * i, j) = fx * dXdYd(0);dpdt(2 * i + 1, j) = fy * dXdYd(1);}// dpdr 对旋转的导数double dx0dr[] ={X * dRdr(0, 0) + Y * dRdr(0, 1) + Z * dRdr(0, 2),X * dRdr(1, 0) + Y * dRdr(1, 1) + Z * dRdr(1, 2),X * dRdr(2, 0) + Y * dRdr(2, 1) + Z * dRdr(2, 2)};double dy0dr[] ={X * dRdr(0, 3) + Y * dRdr(0, 4) + Z * dRdr(0, 5),X * dRdr(1, 3) + Y * dRdr(1, 4) + Z * dRdr(1, 5),X * dRdr(2, 3) + Y * dRdr(2, 4) + Z * dRdr(2, 5)};double dz0dr[] ={X * dRdr(0, 6) + Y * dRdr(0, 7) + Z * dRdr(0, 8),X * dRdr(1, 6) + Y * dRdr(1, 7) + Z * dRdr(1, 8),X * dRdr(2, 6) + Y * dRdr(2, 7) + Z * dRdr(2, 8)};// 归一化坐标对旋转for (j = 0; j < 3; j++){double dxdr = z * (dx0dr[j] - x * dz0dr[j]);double dydr = z * (dy0dr[j] - y * dz0dr[j]);double dr2dr = 2 * x * dxdr + 2 * y * dydr;double dcdist_dr = (k[0] + 2 * k[1] * r2 + 3 * k[4] * r4) * dr2dr;double dicdist2_dr = -icdist2 * icdist2 * (k[5] + 2 * k[6] * r2 + 3 * k[7] * r4) * dr2dr;double da1dr = 2 * (x * dydr + y * dxdr);double dmxdr = (dxdr * cdist * icdist2 + x * dcdist_dr * icdist2 + x * cdist * dicdist2_dr +k[2] * da1dr + k[3] * (dr2dr + 4 * x * dxdr) + (k[8] + 2 * r2 * k[9]) * dr2dr);double dmydr = (dydr * cdist * icdist2 + y * dcdist_dr * icdist2 + y * cdist * dicdist2_dr +k[2] * (dr2dr + 4 * y * dydr) + k[3] * da1dr + (k[10] + 2 * r2 * k[11]) * dr2dr);dXdYd = dMatTilt * Vector2d(dmxdr, dmydr);dpdrot(2 * i, j) = fx * dXdYd(0);dpdrot(2 * i + 1, j) = fy * dXdYd(1);}}}
}

上述除了旋转的雅可比推导外，其他的都比较简单，接下来看旋转雅可比推导，

（1） 旋转矩阵对旋转向量的导数

根据罗德里格斯公式，以及导数定义

按照李群李代数推导，很容易获得

代码中实现

MatrixXd dRdr = Eigen::MatrixXd(3, 9);  // 3×9矩阵，存储9个导数分量
rodrigues(rotation_vector, matR, dRdr);

这里 dRdr 存储的是：

（2） 相机坐标对旋转的导数

相机坐标系下的点：

对旋转分量求导：

展开分量形式：

代码实现

double dx0dr[] = {X * dRdr(0, 0) + Y * dRdr(0, 1) + Z * dRdr(0, 2),  // ∂Xc/∂ωx, ∂Xc/∂ωy, ∂Xc/∂ωzX * dRdr(1, 0) + Y * dRdr(1, 1) + Z * dRdr(1, 2),X * dRdr(2, 0) + Y * dRdr(2, 1) + Z * dRdr(2, 2)};double dy0dr[] = {X * dRdr(0, 3) + Y * dRdr(0, 4) + Z * dRdr(0, 5),  // ∂Yc/∂ωx, ∂Yc/∂ωy, ∂Yc/∂ωz  X * dRdr(1, 3) + Y * dRdr(1, 4) + Z * dRdr(1, 5),X * dRdr(2, 3) + Y * dRdr(2, 4) + Z * dRdr(2, 5)};double dz0dr[] = {X * dRdr(0, 6) + Y * dRdr(0, 7) + Z * dRdr(0, 8),  // ∂Zc/∂ωx, ∂Zc/∂ωy, ∂Zc/∂ωzX * dRdr(1, 6) + Y * dRdr(1, 7) + Z * dRdr(1, 8),X * dRdr(2, 6) + Y * dRdr(2, 7) + Z * dRdr(2, 8)};

（3） 归一化坐标对旋转的导数

归一化坐标：

使用商法则求导：

代码实现

for (j = 0; j < 3; j++)  // j遍历ωx, ωy, ωz
{double dxdr = z * (dx0dr[j] - x * dz0dr[j]);  // z = 1/Zcdouble dydr = z * (dy0dr[j] - y * dz0dr[j]);// dxdr = (1/Zc)(∂Xc/∂ωj) - (Xc/Zc²)(∂Zc/∂ωj)// dydr = (1/Zc)(∂Yc/∂ωj) - (Yc/Zc²)(∂Zc/∂ωj)
}

（4）畸变坐标对归一化坐标的导数

畸变模型：

其中D(r), D2(r)跟delta(x,y) 分别表示（主径向畸变），（额外径向畸变倒数）， （切向畸变）

对x求导

代码实现

double dr2dr = 2 * x * dxdr + 2 * y * dydr;  // ∂r²/∂ωj
double dcdist_dr = (k[0] + 2 * k[1] * r2 + 3 * k[4] * r4) * dr2dr;  // ∂D/∂ωj
double dicdist2_dr = -icdist2 * icdist2 * (k[5] + 2 * k[6] * r2 + 3 * k[7] * r4) * dr2dr;  // ∂D₂/∂ωjdouble da1dr = 2 * (x * dydr + y * dxdr);  // ∂(2xy)/∂ωjdouble dmxdr = (dxdr * cdist * icdist2 +  // 第一项：∂x/∂ωj × D × D₂x * dcdist_dr * icdist2 +  // 第二项：x × ∂D/∂ωj × D₂  x * cdist * dicdist2_dr +  // 第三项：x × D × ∂D₂/∂ωjk[2] * da1dr +            // 切向畸变部分k[3] * (dr2dr + 4 * x * dxdr) + (k[8] + 2 * r2 * k[9]) * dr2dr);  // 额外畸变项

（5） 完整的旋转雅可比矩阵

代码实现

dXdYd = dMatTilt * Vector2d(dmxdr, dmydr);  // 倾斜投影平面的额外变换dpdrot(2 * i, j) = fx * dXdYd(0);     // ∂u/∂ωj
dpdrot(2 * i + 1, j) = fy * dXdYd(1); // ∂v/∂ωj

总结：

        本节总结了3D-2D重投影误差的残差跟雅可比推导，其中旋转部分还是主要还是以李群李代数方式推导最简单，下一讲及后续章节，将以轴角跟四元数方式推导