Jacobian for a translation component of Pose3 (e.g PartialPriorFactor)

[Milo Knowles]

Hi,

I'm trying to add a prior on one component of translation in the world frame, similar to this thread. For example, if I have a measurement of y-position, I would like to add a prior on that component of world_P_body.

The PartialPriorFactor seemed like a natural choice for this. For example, to constrain y (index 4 in the pose) I would add the following factor:

auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0)

PartialPriorFactor<Pose3>(pose_symbol, 4, measured_y, noise_model)

The Jacobian in PartialPriorFactor is set to [0, 0, 0, 0, 1, 0] in this case. This seemed intuitive at first, since the observation function just selects the y-position. However, it seems to cause incorrect results when the world_P_body orientation is *not* the identity.

In other factors that create partial pose constraints, such as GPSFactor, the derivative of a Pose3 w.r.t its translation involves orientation:

// GPSFactor.cpp

Vector GPSFactor::evaluateError(const Pose3& p, boost::optional<Matrix&> H) const {

   return p.translation(H) -nT_;

}

// Pose3.cpp

const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {

   if (Hself) *Hself << Z_3x3, rotation().matrix();

   return t_;

}

In the GPSFactor, my understanding is that the derivative of position depends on rotation because we are linearizing around the current pose estimate, and applying incremental updates in the *local* frame, not the world frame.

My question is: why does PartialPriorFactor have the simple "one-hot" Jacobian, while GPSFactor has an orientation-dependent Jacobian? Am I missing an obvious difference between these two cases?

Based on GPSFactor, shouldn't the Jacobian of a Pose3 w.r.t to one of its position components be the following?

Matrix36 H_world_t_body;

world_P_body.translation(H_world_t_body);

Matrix16 H_world_axis_body = H_world_t_body.row(axis_index);

Happy to provide the toy example where PartialPriorFactor<Pose3> fails under non-identity rotation if that would be helpful.

Thanks,

Milo

[Dellaert, Frank]

This is definitely an "unstable" factor, so an example but even better, a failing unit test would be a welcome PR :-)

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Milo Knowles <mil...@gmail.com>
Sent: Friday, March 19, 2021 13:27
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Jacobian for a translation component of Pose3 (e.g PartialPriorFactor)

 

--
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/7a0d11a7-0450-4dd0-ab0e-576afaf43ff2n%40googlegroups.com.

[Milo Knowles]

Just opened a PR with failing unit tests! Will start working on a fix.