0
$\begingroup$

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

$\endgroup$
0
$\begingroup$

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;

}
$\endgroup$
  • $\begingroup$ and I think so3 is pure rotation , It should get the exactly same result just like rotation slerp $\endgroup$ – Mr.Guo May 27 '16 at 7:14

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.