Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Wrap BearingRange Factors #1240

Merged
merged 2 commits into from
Jul 8, 2022
Merged

Wrap BearingRange Factors #1240

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f5ec070
wrap BearingRange classes and factors
varunagrawal Jul 7, 2022
2ab09a5
add Python unit tests
varunagrawal Jul 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions gtsam/geometry/geometry.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1279,5 +1279,11 @@ class BearingRange {

typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double>
BearingRange2D;
typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double>
BearingRangePose2;
typedef gtsam::BearingRange<gtsam::Pose3, gtsam::Point3, gtsam::Unit3, double>
BearingRange3D;
typedef gtsam::BearingRange<gtsam::Pose3, gtsam::Pose3, gtsam::Unit3, double>
BearingRangePose3;

} // namespace gtsam
2 changes: 1 addition & 1 deletion gtsam/linear/linear.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -279,7 +279,6 @@ virtual class GaussianFactor {
virtual class JacobianFactor : gtsam::GaussianFactor {
//Constructors
JacobianFactor();
JacobianFactor(const gtsam::GaussianFactor& factor);
JacobianFactor(Vector b_in);
JacobianFactor(size_t i1, Matrix A1, Vector b,
const gtsam::noiseModel::Diagonal* model);
Expand All @@ -295,6 +294,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
JacobianFactor(const gtsam::GaussianFactorGraph& graph,
const gtsam::Ordering& ordering,
const gtsam::VariableSlots& p_variableSlots);
JacobianFactor(const gtsam::GaussianFactor& factor);

//Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
Expand Down
6 changes: 6 additions & 0 deletions gtsam/sam/sam.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,5 +108,11 @@ typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2,
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2,
double>
BearingRangeFactorPose2;
typedef gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3,
double>
BearingRangeFactor3D;
typedef gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Pose3, gtsam::Unit3,
double>
BearingRangeFactorPose3;

} // namespace gtsam
22 changes: 20 additions & 2 deletions python/gtsam/tests/test_Factors.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,16 +11,16 @@
"""
import unittest

import numpy as np

import gtsam
import numpy as np
from gtsam.utils.test_case import GtsamTestCase


class TestNonlinearEquality2Factor(GtsamTestCase):
"""
Test various instantiations of NonlinearEquality2.
"""

def test_point3(self):
"""Test for Point3 version."""
factor = gtsam.NonlinearEquality2Point3(0, 1)
Expand All @@ -30,5 +30,23 @@ def test_point3(self):
np.testing.assert_allclose(error, np.zeros(3))


class TestJacobianFactor(GtsamTestCase):
"""Test JacobianFactor"""

def test_gaussian_factor_graph(self):
"""Test construction from GaussianFactorGraph."""
gfg = gtsam.GaussianFactorGraph()
jf = gtsam.JacobianFactor(gfg)
self.assertIsInstance(jf, gtsam.JacobianFactor)

nfg = gtsam.NonlinearFactorGraph()
nfg.push_back(gtsam.PriorFactorDouble(1, 0.0, gtsam.noiseModel.Isotropic.Sigma(1, 1.0)))
values = gtsam.Values()
values.insert(1, 0.0)
gfg = nfg.linearize(values)
jf = gtsam.JacobianFactor(gfg)
self.assertIsInstance(jf, gtsam.JacobianFactor)


if __name__ == "__main__":
unittest.main()
28 changes: 28 additions & 0 deletions python/gtsam/tests/test_sam.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,34 @@ def test_BearingRangeFactor2D(self):
self.assertEqual(range_measurement, measurement.range())
self.gtsamAssertEquals(bearing_measurement, measurement.bearing())

def test_BearingRangeFactor3D(self):
"""
Test that `measured` works as expected for BearingRangeFactor3D.
"""
bearing_measurement = gtsam.Unit3()
range_measurement = 10.0
factor = gtsam.BearingRangeFactor3D(
1, 2, bearing_measurement, range_measurement,
gtsam.noiseModel.Isotropic.Sigma(3, 1))
measurement = factor.measured()

self.assertEqual(range_measurement, measurement.range())
self.gtsamAssertEquals(bearing_measurement, measurement.bearing())

def test_BearingRangeFactorPose3(self):
"""
Test that `measured` works as expected for BearingRangeFactorPose3.
"""
range_measurement = 10.0
bearing_measurement = gtsam.Unit3()
factor = gtsam.BearingRangeFactorPose3(
1, 2, bearing_measurement, range_measurement,
gtsam.noiseModel.Isotropic.Sigma(3, 1))
measurement = factor.measured()

self.assertEqual(range_measurement, measurement.range())
self.gtsamAssertEquals(bearing_measurement, measurement.bearing())


if __name__ == "__main__":
unittest.main()