search

borglab / gtsam

GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices.
http://gtsam.org
Other
2.55k stars 754 forks source link

Are two entries of the Jacobian matrix in Pose2::ExpmapDerivative wrong? #833

Closed doublestrong closed 2 years ago

doublestrong commented 3 years ago

https://github.com/borglab/gtsam/blob/158d279d59201f79fac78d80465332ec1914aeec/gtsam/geometry/Pose2.cpp#L163

Hi, are the entries around the top right corner wrong for computing the jacobian d(x,y,theta)/d(v1,v2,alpha)? (v1, v2, alpha) is a tangent space vector. The entries are

v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha

and

-v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha

I checked and derived the J_l and J_r in Chirikjian11book2, pg.36 (stochastic models, information theory, and lie groups: volume 2 analytic methods and modern applications). I found that J_r[0, 2], J_r[1, 2], J_l[0, 2], and , J_l[1, 2] in my formula are different from those in the book. In my formula, I got alpha (v1cos(alpha)-v2 sin(alpha)) at the place of the alpha v1 term on that page, and got alpha (v1 sin(alpha) + v2) at the place of alpha v2 on that page. I simply start from Eq. (10.76) on page 35 in the book and derive the derivative of the translation part over alpha.

doublestrong commented 2 years ago

Sorry i was wrong. I get the same entries as those in the book and the GTSAM code. I will close this.