Position offset function

[VersLerclec]

Hi Dr. Taro,

Thanks for sharing the code! This is truly a fantastic project.

I just wanted to clarify one point in the implementation:
Did you ignore the lever arm effect when constructing the factors, and then compensate for it using the position offset function?
If so, it seems that the partial derivatives with respect to attitude may have been omitted.

In my case, I plan to implement the system using an IMU and a GNSS antenna with a precisely known lever arm.
Should I account for the lever arm effect directly in the GNSS factor (include pose3 key in the factor), or is it more appropriate to handle it using the Pose3Point3Factor_PX?

Thank you very much! Any guidance would be helpful to me.

[taroz]

In the Smartphone Decimeter Challenge, there was an unknown offset between the position of the smartphone and the ground truth. Therefore, to estimate this offset (a slight shift from the ground truth) from the score, I added an offset function that adjusts the position forward, backward, left, and right after estimating the trajectory.

The GNSS antenna and IMU lever arm that normally needs to be considered is small in smartphones, so I have not included it in the factors. If this lever arm offset is known to be large, it would be better to account for it in the factors. In that case, I think it would be fine to either consider it directly in the factors or to estimate P and X separately as IMU position and GNSS antenna position.

I hope my opinion is helpful.

[VersLerclec]

Hi Dr. Taro,

Thanks for your reply. It's very helpful.

I'm trying to modify the Pose3Point3Factor_PX to account for the lever arm effect. However, I just encountered some problems that the optimizer did not converge. Specifically, I changed the error vector as:

gtsam::Vector3 error = p.translation(Hpose_P) + p.rotation() * lever_ - x;
As for the Jacobian matrix, I’m not quite sure how to design the matrix Hp. In the original code, it is defined as: $H_{p} = [0_{3x3}, C^{n}_{b,3x3}]$.

I would like to understand why this partial derivative should be assigned to Hp. To my knowledge, if the lever arm is known, the partial derivative with respect to attitude should be gtsam::skewSymmetric(p.rotation() * lever_). I tried replacing the original $C^{n}_{b,3x3}$ term with this skew-symmetric matrix, but the optimizer still did not converge. How should I assign the Hp matrix to account for the lever arm effect?

Thank you very much.

[VersLerclec]

Hi,

Please ignore the previous comment. I just found the GTSAM built-in function transformFrom may solve the problem.
I modified the code to the following:

            // Rotation and translation from Pose3
            gtsam::Matrix36 Hpose_P;
            gtsam::Point3 pred_ant = p.transformFrom(lever_, Hpose_P);  // R*lever + t

            gtsam::Vector3 error = pred_ant - x;

            // Jacobian
            if (Hp) *Hp = Hpose_P;
            if (Hx) *Hx = -gtsam::I_3x3;

            return error;

It seems like it works. Thank you for your suggestion.