Define Jacobian Matrix to constraint the roll and pitch angle

[Phạm Ngọc Sơn]

Hi everyone.
I am implementing the gravity factor to constraint the roll and pitch angle to make the map align with ground.
I tried to define the H matrix according to 2 ways as image below:
I made some experiments to evaluate the error but it not stable.
Anyone in groups can help me to define Jacobian matrix or correct me if I am wrong.
I added the custom factor to lio-sam but it still not working.
Thanks

[Matías Mattamala]

Hi Phạm

From your snapshots I can say:

- Option 1 (IMG_3937) is computing the error obtained from roll and pitch constraints. It means the residual is a function f(T, roll, pitch) -> R^2. Your Jacobian is not correct, since it misses the conversion from an SE(3) pose to roll-pitch-yaw, which instead would make the function f(g(T), roll, pitch), with g(T) the function that extract the rotation matrix and extracts pitch and roll. Hence if you do the chain rule you will get a slightly more complicated Jacobian instead by compounding J_f (2x3, from roll-pitch-yaw to error in roll and pitch) and J_g (3x6, from the transformation T to roll, pitch and yaw).

However, since you have the conversion to Euler angles, the result will not be ideal as it's discussed in the IMU preintegration paper: https://rpg.ifi.uzh.ch/docs/TRO16_forster.pdf (Fig. 13)

- Option 2 (IMG_3940) suffers from the opposite problem: It defines the residual in local coordinates of rotation objetcs, i.e the error is defined as Log(R_meas^-1 * R). THis is good according to the previous paper, but the residual is not roll-pitch-yaw and an angle-axis representation instead. Here you "lose" the notion of the constraint you want to enforce, specially to define the Jacobian, as the error is axis-angle.

Instead, an alternative way to implement this that we have used in my research group is using the Pose3AttitudeFactor: https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.h

Here the attitude you enforce is given by an unit vector, and the evaluateError and Jacobians are in the cpp file: https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.cpp#L26

I hope it helps

Matias

[Phạm Ngọc Sơn]

Thank you for your help. I will try to implement again follow 2 options
- Option 1: I need to convert SE(3) pose to Roll-pitch-yaw and calculate derivative function.
- Option 2: I want to understand carefully the error function you defined. How can you define the error function?
Which formula or material you used to implement it?
What is the meaning of the denote in formula? 
Matrix23 D_nRef_R;
Matrix22 D_e_nRef;
I can not understand the meaning of these params.
Can you explain more detail ?
Thanks.

Vào lúc 03:43:08 UTC+7 ngày Thứ Năm, 17 tháng 11, 2022, Matías Mattamala đã viết:

[Matías Mattamala]

For Option 2, Log(R_meas^-1*R) is the standard error function defined for rotation matrices. Your code measured_. LocalCoordinates (new); is implicitly computing that, you may read more in a blog post I wrote a time ago: https://gtsam.org/2021/02/23/uncertainties-part2.html

For the Pose3AttitudeFactor question, I did not implement it. But from the comments in the code this is what I got:

- It considers a navigation frame n (world, fixed frame) and a body frame b

- The residual function is defined as error( nRef, nG), where "error" is an error metric for unit vectors, nRef is the "measured" attitude in the navigation frame, and nG is the attitude in the navigation frame.

- Also, nRef is defined as nRef=  nRb * bF, where nRb is the relative orientation between the navigation and body frame (effectively being the orientation of your robot's body) and bF is an unit vector indicating the direction of gravity as seen in the body frame.

- So the idea is that if you want bF == nG, forcing nRb (the robot's orientation) to align to gravity.

- The Jacobians you asked me about are due to the definition of the error function: e = error(nRef, nG)

- Then, the derivative with respect to the robot's orientation/attitude nRef will be De/DnRef * DnRef/DnRb

* De/DnRef corresponds to D_e_nRef in the code: it's the Jacobian of the error function w.r.t the rotated unit vector nRef we define before. The Jacobian is obtained directly from the error defined for unit vector that GTSAM implements.

* DnRef/DnRb is D_nRef_R : the Jacobian of nRef w.r.t the robot's attitude nRb used to define it nRef = nRb * bF 

[Phạm Ngọc Sơn]

I tried to implement the error function as Pose3AttitudeFactor. But result after optimizing still not correct.
My IMU follows the ROS REP-105 convention (x - forward, y - left, z - upward)

Vào lúc 16:34:36 UTC+7 ngày Thứ Năm, 17 tháng 11, 2022, Matías Mattamala đã viết:

[Phạm Ngọc Sơn]

Hi Matías Mattamala
In Lio-sam, my IMU follow this convention(x - forward, y - left, z - upward)
So I define the bRef (0,0, 1) and nZ(0, 0, -1). So the map not align with gravity
I defined the gravity factor in my code and the result:

https://drive.google.com/file/d/1S4xLtdYfxxsvRnXXLYIWKvd12mHxVlET/view?usp=sharing

Vào lúc 16:50:00 UTC+7 ngày Thứ Năm, 17 tháng 11, 2022, Phạm Ngọc Sơn đã viết: