BearingRange - GTSAM Docs

The bearing-range factor is useful when you have both a bearing and a range to an object in 2D or 3D. BearingRange bundles those two measurements, and the Python wrapper provides concrete specializations for pose-to-point and pose-to-pose geometry.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import gtsam
import numpy as np

Measuring in 2D¶

BearingRange2D measures a Pose2 to a Point2; BearingRangePose2 uses another Pose2 as the target. Measure() constructs the pair, while MeasureBearing() and MeasureRange() compute either component alone.

observer2 = gtsam.Pose2(1.0, 2.0, 0.3)
landmark2 = np.array([4.0, 3.0])
measurement2 = gtsam.BearingRange2D.Measure(observer2, landmark2)
print("bearing angle:", measurement2.bearing().theta())
print("range:", measurement2.range())

Measuring in 3D¶

BearingRange3D returns a Unit3 bearing from Pose3 to Point3; BearingRangePose3 targets another pose. The bearing is expressed in the observer’s local frame, while range is Euclidean distance.

observer3 = gtsam.Pose3(gtsam.Rot3.Yaw(0.2), np.array([1.0, 0.0, 0.0]))
landmark3 = np.array([4.0, 1.0, 2.0])
measurement3 = gtsam.BearingRange3D.Measure(observer3, landmark3)
print("unit bearing:", measurement3.bearing().unitVector())
print("range:", measurement3.range())

expected_range = np.linalg.norm(landmark3 - observer3.translation())
assert np.isclose(measurement3.range(), expected_range)

Accessors and use in factors¶

Instances expose only bearing() and range() because they are lightweight measurement values. Bearing-range factors use the same concrete specializations, so the type of bearing—Rot2 or Unit3—matches the geometry being estimated.

Source¶

BearingRange.h

AI assistance caveat¶

AI was used to help draft this documentation, and inaccuracies could be present.