Use same pose model for symbolic and autodiff

src/aliceVision/geometry/lie.hpp

@@ -118,6 +118,172 @@ inline Eigen::Vector3d logm(const Eigen::Matrix3d& R)
 
     return ret;
 }
+
+/**
+Compute the jacobian of the logarithm wrt changes in the rotation matrix values
+@param R the input rotation matrix
+@return the jacobian matrix (3*9 matrix)
+*/
+inline Eigen::Matrix<double, 3, 9, Eigen::RowMajor> dlogmdr(const Eigen::Matrix3d& R)
+{
+    double p1 = R(2, 1) - R(1, 2);
+    double p2 = R(0, 2) - R(2, 0);
+    double p3 = R(1, 0) - R(0, 1);
+
+    double costheta = (R.trace() - 1.0) / 2.0;
+    if (costheta > 1.0)
+        costheta = 1.0;
+    else if (costheta < -1.0)
+        costheta = -1.0;
+
+    double theta = acos(costheta);
+
+    if (fabs(theta) < std::numeric_limits<float>::epsilon())
+    {
+        Eigen::Matrix<double, 3, 9> J;
+        J.fill(0);
+        J(0, 5) = 1;
+        J(0, 7) = -1;
+        J(1, 2) = -1;
+        J(1, 6) = 1;
+        J(2, 1) = 1;
+        J(2, 3) = -1;
+        return J;
+    }
+
+    double scale = theta / (2.0 * sin(theta));
+
+    Eigen::Vector3d resnoscale;
+    resnoscale(0) = p1;
+    resnoscale(1) = p2;
+    resnoscale(2) = p3;
+
+    Eigen::Matrix<double, 3, 3> dresdp = Eigen::Matrix3d::Identity() * scale;
+    Eigen::Matrix<double, 3, 9> dpdmat;
+    dpdmat.fill(0);
+    dpdmat(0, 5) = 1;
+    dpdmat(0, 7) = -1;
+    dpdmat(1, 2) = -1;
+    dpdmat(1, 6) = 1;
+    dpdmat(2, 1) = 1;
+    dpdmat(2, 3) = -1;
+
+    double dscaledtheta = -0.5 * theta * cos(theta) / (sin(theta) * sin(theta)) + 0.5 / sin(theta);
+    double dthetadcostheta = -1.0 / sqrt(-costheta * costheta + 1.0);
+
+    Eigen::Matrix<double, 1, 9> dcosthetadmat;
+    dcosthetadmat << 0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.5;
+    Eigen::Matrix<double, 1, 9> dscaledmat = dscaledtheta * dthetadcostheta * dcosthetadmat;
+
+    return dpdmat * scale + resnoscale * dscaledmat;
+}
+
+
+
+/**
+Compute the jacobian of the exponential wrt changes in the rotation vector values
+@param vecr the rotation vector
+@return the jacobian matrix (9*3 matrix)
+*/
+inline Eigen::Matrix<double, 9, 3, Eigen::RowMajor> dexpmdr(const Eigen::Vector3d & vecr)
+{
+    double angle = vecr.norm();  
+
+    if (angle < 1e-24)
+    {
+        Eigen::Matrix<double, 9, 3> d_K_d_vecr;
+        d_K_d_vecr.fill(0);
+        d_K_d_vecr(1, 2) = 1;
+        d_K_d_vecr(2, 1) = -1;
+        d_K_d_vecr(3, 2) = -1;
+        d_K_d_vecr(5, 0) = 1;
+        d_K_d_vecr(6, 1) = 1;
+        d_K_d_vecr(7, 0) = -1;
+        return d_K_d_vecr;
+    }
+
+    const double x = vecr(0);
+    const double y = vecr(1);
+    const double z = vecr(2);
+
+    double angle2 = angle * angle;
+    double angle3 = angle2 * angle;
+    double sina = sin(angle);
+    double cosa = cos(angle);
+    double mcosa = 1.0 - cosa;
+    double c = mcosa/ angle2;
+    double s = sina / angle;
+
+    double d_s_d_angle = cosa/angle - sina/angle2;
+    double d_c_d_angle = sina/angle2 - 2.0*mcosa/angle3;
+
+    double d_angle_d_x = x / angle;
+    double d_angle_d_y = y / angle;
+    double d_angle_d_z = z / angle;
+
+    //Jacobian of s*K + c*K*K
+    Eigen::Matrix<double, 9, 5> J;
+    J(0,0)=0;
+    J(1,0)=c*y;
+    J(2,0)=c*z;
+    J(3,0)=c*y;
+    J(4,0)=-2*c*x;
+    J(5,0)=s;
+    J(6,0)=c*z;
+    J(7,0)=-s;
+    J(8,0)=-2*c*x;
+    J(0,1)=-2*c*y;
+    J(1,1)=c*x;
+    J(2,1)=-s;
+    J(3,1)=c*x;
+    J(4,1)=0;
+    J(5,1)=c*z;
+    J(6,1)=s;
+    J(7,1)=c*z;
+    J(8,1)=-2*c*y;
+    J(0,2)=-2*c*z;
+    J(1,2)=s;
+    J(2,2)=c*x;
+    J(3,2)=-s;
+    J(4,2)=-2*c*z;
+    J(5,2)=c*y;
+    J(6,2)=c*x;
+    J(7,2)=c*y;
+    J(8,2)=0;
+    J(0,3)=0;
+    J(1,3)=z;
+    J(2,3)=-y;
+    J(3,3)=-z;
+    J(4,3)=0;
+    J(5,3)=x;
+    J(6,3)=y;
+    J(7,3)=-x;
+    J(8,3)=0;
+    J(0,4)=-y*y - z*z;
+    J(1,4)=x*y;
+    J(2,4)=x*z;
+    J(3,4)=x*y;
+    J(4,4)=-x*x - z*z;
+    J(5,4)=y*z;
+    J(6,4)=x*z;
+    J(7,4)=y*z;
+    J(8,4)=-x*x - y*y;
+
+    //Jacobian of [x y z s c] wrt [x y z]
+    Eigen::Matrix<double, 5, 3> M = Eigen::Matrix<double, 5, 3>::Zero();
+    M(0, 0) = 1.0;
+    M(1, 1) = 1.0;
+    M(2, 2) = 1.0;
+    M(3, 0) = d_s_d_angle * d_angle_d_x;
+    M(3, 1) = d_s_d_angle * d_angle_d_y;
+    M(3, 2) = d_s_d_angle * d_angle_d_z;
+    M(4, 0) = d_c_d_angle * d_angle_d_x;
+    M(4, 1) = d_c_d_angle * d_angle_d_y;
+    M(4, 2) = d_c_d_angle * d_angle_d_z;
+
+    return J * M;
+}
+
 }  // namespace SO3
 
 namespace SE3 {