Help: Calibration works sporadically (extrinsic)

[Nikhil W]

Hi all,

Description:

Sorry, long question, but I'm trying to optimize the extrinsic rotation calibration of one camera (the body to camera rotation. Assume the translation is zero, I just want to optimize rotation). I have 81 frames, and each frame has accurate ground truth pose, and associated imaged keypoints in each frame.

I set up an optimization graph, by adding priors for the 3D cartesian known keypoints and intrinsic calibration matrices (K and D).

The custom between factor links all the poses together such that the relative rotation is consistent across all frames.

For each frame, I'm adding: (in python)

- a position prior (setting covariance to zero) and rotation prior (covariance set to allow the optimizer to rotate e.g. sigma=0.1) factor_graph.addPriorPose3 ]

- the imaged keypoints [ GeneralSFMFactor2Cal3Fisheye ] with Huber robustification

- a custom between factor

The custom between factor links all the poses together such that the relative rotation is consistent across all frames. I define the error function as such for [ X(0),  X(1) ] (measurement is the initial camera pose)

# initial pose

pose0_initial = measurement[0]

pose1_initial = measurement[1]

# current pose

pose0_opt = values.atPose3(this.keys()[0])

pose1_opt = values.atPose3(this.keys()[1])

# pose difference

pose0_diff = cv2.Rodrigues((pose0_initial.transformPoseTo(pose0_opt)).rotation().matrix())[0].reshape(3)

pose1_diff = cv2.Rodrigues((pose1_initial.transformPoseTo(pose1_opt)).rotation().matrix())[0].reshape(3)

error = pose1_diff-pose0_diff

Which basically compares the two relative Rodrigues rotational vectors (size 3). When putting the covariance of this factor to zero, it should ensure that all of the poses will rotate in the same manner w.r.t. the initial estimates.

Problem

This works for a certain combination of poses I use. For example, when I add poses 0 - 75 to the graph it works, and all relative rotations are equal and look good. However when I try to optimize for poses 1-76, it does not work, and all results are zero (equal to initialization). But when I try 1-77 it does work again. And the combination of which ones work is sensitive to the prior covariances/setup.

I've also tried instead of sequentially linking all poses (1 to 2, 2 to 3, 3 to 4 etc), to link all poses to a central pose (1 to 0, 2 to 0, 3 to 0 etc), and as well adding both of these custom factors, but somehow it always fails for a certain combination. If I omit these custom between factors, it rotates each pose individually to align with the imaged keypoints.

Does anyone have any insight into what I could try?

Thank you so much,

Nikhil

[Dellaert, Frank]

Don’t have time to read your email in depth, but did you try Shonan averaging?

Best!

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Nikhil W <n.wess...@gmail.com>
Sent: Monday, March 4, 2024 4:42:56 PM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Help: Calibration works sporadically (extrinsic)

 

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/1b0ad9bf-2d10-4a39-a6ac-fdd6946d81b4n%40googlegroups.com.

[Nikhil W]

Thank you Frank.

Just in case anyone is interested, I tried to find a way to do it with shonan averaging but in the end, I defined a custom factor with error:

x0 = this.keys()[0]# X(n-1)

x1 = this.keys()[1]# X(n)

xc = this.keys()[2]# X(c)

zero_tf = values.atPose3(x0).transformPoseTo(values.atPose3(x1)).transformPoseTo(values.atPose3(xc))

rvec_diff = zero_tf.rotation().xyz()

tvec_diff = zero_tf.translation()

error = np.concatenate((rvec_diff, tvec_diff))

Thanks,

Nikhil

[Yotam Stern]

you should at least replace the rvec_diff and tvec_diff with error = Pose3.Logmap(zero_tf)