BearingFactor<Pose3, Point3> noise model

[Robert Lukassen]

Hi. I'm new to this group. I have been following it a bit since I started to do some experiments with GTSAM, and it has been helping me already getting a better grip on this stuff. Even with the documentation and tutorials out there, it's not that easy to digest all that GTSAM has to offer!

Question

Why is the noiseModel of the BearingFactor3D two dimensional? The measurement that goes with this factor is a Unit3 (3d local coordinate vector), so I'd expect to supply some noise model that expresses the noise in those components.

Background

I'm experimenting with a NonlinearFactorGraph that has many Pose3 nodes 'connected' with BetweenFactorPose3 constraints. Some of them have Priors on their translation component, which works fine with PoseTranslationPrior3D.

This graph optimizes with the given constrains and reasonable results, which is great.

Now I want to add observations made from some of these nodes to landmarks with known positions. None of these landmarks are unique, but their position is known and they are somewhat sparse - within the observational accuracy there will be no alternatives.

I was hoping this would be easy, but it turns out not so simple (at least not for me, novice GTSAM user). 

I've managed to get something working. The idea is to add Point3 nodes to the graph for landmarks that have been observed. Since these come with known good positions, I'm inserting a PriorFactorPoint3 for each of these with a very strict noise model.

Then, for each node with an observation of that landmark, I'm adding a BearingFactor3D between the observation pose and the landmark point. This seems to be working as well, although the effect is kind of disappointing.

This may be due to the noiseModel used with that BearingFactor3D. To my surprise, this must be a 2 dimensional model, which I don't get. The measurement that goes with this factor is a 3d Unit3 vector, in local coordinates of the observation pose. So why is the noiseModel required to be a 2D model?

So I guess I'm not fully grasping the details here, and perhaps that's also why the effect of adding these observations does not seem to have much effect.

Regards,

Robert

[Robert Lukassen]

Bump.

Perhaps I should expand the question a bit. The measurement that goes with a BearingFactor3D factor between two poses is a general Unit3, a three dimensional vector. I was assuming this to be a vector expressed in the 'source' pose indicating the origin of the 'target' pose. Now I'm not so sure. It might also be in some polar coordinates with two angles and a range...

Has anyone worked with a BearingFactor3D factor before, and is willing to shed some light on this? I'm not able to dig this up from documentation and/or Google search :(

Cheers,

Robert

[Dellaert, Frank]

Unit3 is represented as a point3, but is a 2D manifold of directions​ in space. This is no different than, say, a Pose3, which can be represented as a 4x4 matrix but is a 6D manifold. a BetweenFactorPose3 takes a 6-D noise model.

FD

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Robert Lukassen <robert....@gmail.com>
Sent: Tuesday, May 10, 2022 05:59
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Re: BearingFactor<Pose3, Point3> noise model

 

--
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/dea8a869-0ad9-4e05-aa07-82e3909d49can%40googlegroups.com.

[Robert Lukassen]

Ah! That's actually quite obvious. 

I'll see whether I can use BearingRangeFactor. Is there any convenience method that can transform a local Point3 to a bearing & range (well, range seems obvious, just wondering how the bearing is defined).

Regards,

Robert

[voltron]

gtsam/geometry/Pose3.h

  /**
   * Calculate range to a landmark
   * @param point 3D location of landmark
   * @return range (double)
   */
  double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
      OptionalJacobian<1, 3> Hpoint = boost::none) const;

  /**
   * Calculate bearing to a landmark
   * @param point 3D location of landmark
   * @return bearing (Unit3)
   */
  Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
      OptionalJacobian<2, 3> Hpoint = boost::none) const;