lie group jacobian function deduce

Question

hi guys I have read the paper at http://www.ethaneade.org/lie.pdf , and regarding the equation (87) I have coded it for proven but found not correct , the code is like this

SE3f::Tangent v;
v << 100, 200, 300, 0.2, -0.3, 0.4;
SE3f::Tangent delta;
delta << 20, 20, 20, 0.3, -0.4, 0.5;
SE3f se3f = SE3f::exp(v);
SE3f se3f_delta = SE3f::exp(delta)*SE3f::exp(v);
SE3f se3f_delta_add = SE3f::exp(v + delta);
Vector3f wpt0(70, 60, 200);
Vector3f wpt1 = se3f*wpt0;
Vector3f wpt2 = se3f_delta*wpt0;
Vector3f wpt3 = se3f_delta_add*wpt0;

Matrix<float, 3, 6> jacobi;
jacobi.setZero();
jacobi.topLeftCorner(3, 3) = Matrix3f::Identity();
jacobi(0, 4) = wpt1[2];
jacobi(1, 3) = -wpt1[2];
jacobi(0, 5) = -wpt1[1];
jacobi(2, 3) = wpt1[1];
jacobi(1, 5) = wpt1[0];
jacobi(2, 4) = -wpt1[0];

cout << jacobi << endl;

Vector3f wpt_jacobi = wpt1 + jacobi*delta;

the wpt_jacobi is -297.683960, 64.0022125, 625.767151

while the wpt2 which is correct is -254.784164, -8.84317398, 541.750977 they miss too much , is there anyone could help me I have use the eigen and lie group lib at https://github.com/stevenlovegrove/Sophus

Answer 1

I'm afraid that the it's the nonlinear reason , so I have written a iterated approach code but it still can't get the exactly result, anyone could tell me why :)?

    SO3f::Tangent v;
v << 0.34, 0.34, 0.34;
SO3f::Tangent delta;
delta << 0.34, -0.34, -0.34;
SO3f se3f = SO3f::exp(v);
SO3f se3f_delta = SO3f::exp(delta)*SO3f::exp(v);
SO3f se3f_delta_add = SO3f::exp(v + delta);
Vector3f wpt0(200, 200, 200);
Vector3f wpt1 = se3f*wpt0;
Vector3f wpt2 = se3f_delta*wpt0;
Vector3f wpt3 = se3f_delta_add*wpt0;

Vector3f wpt_jacobi = wpt1;
SO3f::Tangent delta_2 = delta;
for (int i = 0; i < 100; i++)
{
    float nm = delta.norm();
    delta_2 = sin(asin(nm)/2) * (delta.normalized());

    Matrix<float, 3, 3> jacobi;
    jacobi.setZero();
    jacobi.topLeftCorner(3, 3) = Matrix3f::Zero();
    jacobi(0, 1) = wpt_jacobi[2];
    jacobi(1, 0) = -wpt_jacobi[2];
    jacobi(0, 2) = -wpt_jacobi[1];
    jacobi(2, 0) = wpt_jacobi[1];
    jacobi(1, 2) = wpt_jacobi[0];
    jacobi(2, 1) = -wpt_jacobi[0];
    wpt_jacobi = wpt_jacobi + jacobi*delta_2;

    delta = SO3f::log(SO3f::exp(delta_2).inverse()*SO3f::exp(delta));

    cout << jacobi << endl;
    cout << wpt_jacobi.transpose() << endl;
    cout << wpt2.transpose() << endl;

}