diff --git a/gtsam/geometry/tests/testUtilities.cpp b/gtsam/geometry/tests/testUtilities.cpp new file mode 100644 index 000000000..25ac3acc8 --- /dev/null +++ b/gtsam/geometry/tests/testUtilities.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testUtilities.cpp + * @date Aug 19, 2021 + * @author Varun Agrawal + * @brief Tests for the utilities. + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using gtsam::symbol_shorthand::L; +using gtsam::symbol_shorthand::R; +using gtsam::symbol_shorthand::X; + +/* ************************************************************************* */ +TEST(Utilities, ExtractPoint2) { + Point2 p0(0, 0), p1(1, 0); + Values values; + values.insert(L(0), p0); + values.insert(L(1), p1); + values.insert(R(0), Rot3()); + values.insert(X(0), Pose3()); + + Matrix all_points = utilities::extractPoint2(values); + EXPECT_LONGS_EQUAL(2, all_points.rows()); +} + +/* ************************************************************************* */ +TEST(Utilities, ExtractPoint3) { + Point3 p0(0, 0, 0), p1(1, 0, 0); + Values values; + values.insert(L(0), p0); + values.insert(L(1), p1); + values.insert(R(0), Rot3()); + values.insert(X(0), Pose3()); + + Matrix all_points = utilities::extractPoint3(values); + EXPECT_LONGS_EQUAL(2, all_points.rows()); +} + +/* ************************************************************************* */ +int main() { + srand(time(nullptr)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index a55581e50..67c3278a3 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -165,22 +165,17 @@ gtsam::Values allPose2s(gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); -void perturbPoint2(gtsam::Values& values, double sigma, int seed); +void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u); void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR, - int seed); -void perturbPoint3(gtsam::Values& values, double sigma, int seed); + int seed = 42u); +void perturbPoint3(gtsam::Values& values, double sigma, int seed = 42u); void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCamera& c, Vector J, Matrix Z, double depth); -void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, - Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, - const gtsam::Cal3_S2* K); -void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, - Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, - const gtsam::Cal3_S2* K, - const gtsam::Pose3& body_P_sensor); +void insertProjectionFactors( + gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, + const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, + const gtsam::Pose3& body_P_sensor = gtsam::Pose3()); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); gtsam::Values localToWorld(const gtsam::Values& local, diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 4e79e20ff..fdc1da2c4 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file matlab.h + * @file utilities.h * @brief Contains *generic* global functions designed particularly for the matlab interface * @author Stephen Williams */ @@ -89,21 +89,41 @@ KeySet createKeySet(std::string s, const Vector& I) { /// Extract all Point2 values into a single matrix [x y] Matrix extractPoint2(const Values& values) { + Values::ConstFiltered points = values.filter(); + // Point2 is aliased as a gtsam::Vector in the wrapper + Values::ConstFiltered points2 = values.filter(); + + Matrix result(points.size() + points2.size(), 2); + size_t j = 0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(), 2); - for(const auto& key_value: points) + for (const auto& key_value : points) { result.row(j++) = key_value.value; + } + for (const auto& key_value : points2) { + if (key_value.value.rows() == 2) { + result.row(j++) = key_value.value; + } + } return result; } /// Extract all Point3 values into a single matrix [x y z] Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(), 3); + Values::ConstFiltered points = values.filter(); + // Point3 is aliased as a gtsam::Vector in the wrapper + Values::ConstFiltered points2 = values.filter(); + + Matrix result(points.size() + points2.size(), 3); + size_t j = 0; - for(const auto& key_value: points) + for (const auto& key_value : points) { result.row(j++) = key_value.value; + } + for (const auto& key_value : points2) { + if (key_value.value.rows() == 3) { + result.row(j++) = key_value.value; + } + } return result; } @@ -144,11 +164,18 @@ Matrix extractPose3(const Values& values) { /// Perturb all Point2 values using normally distributed noise void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, - sigma); + noiseModel::Isotropic::shared_ptr model = + noiseModel::Isotropic::Sigma(2, sigma); Sampler sampler(model, seed); - for(const auto& key_value: values.filter()) { - values.update(key_value.key, key_value.value + Point2(sampler.sample())); + for (const auto& key_value : values.filter()) { + values.update(key_value.key, + key_value.value + Point2(sampler.sample())); + } + for (const auto& key_value : values.filter()) { + if (key_value.value.rows() == 2) { + values.update(key_value.key, + key_value.value + Point2(sampler.sample())); + } } } @@ -165,19 +192,34 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = /// Perturb all Point3 values using normally distributed noise void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, - sigma); + noiseModel::Isotropic::shared_ptr model = + noiseModel::Isotropic::Sigma(3, sigma); Sampler sampler(model, seed); - for(const auto& key_value: values.filter()) { - values.update(key_value.key, key_value.value + Point3(sampler.sample())); + for (const auto& key_value : values.filter()) { + values.update(key_value.key, + key_value.value + Point3(sampler.sample())); + } + for (const auto& key_value : values.filter()) { + if (key_value.value.rows() == 3) { + values.update(key_value.key, + key_value.value + Point3(sampler.sample())); + } } } -/// Insert a number of initial point values by backprojecting +/** + * @brief Insert a number of initial point values by backprojecting + * + * @param values The values dict to insert the backprojections to. + * @param camera The camera model. + * @param J Vector of key indices. + * @param Z 2*J matrix of pixel values. + * @param depth Initial depth value. + */ void insertBackprojections(Values& values, const PinholeCamera& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) - throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + throw std::invalid_argument("insertBackProjections: Z must be 2*J"); if (Z.cols() != J.size()) throw std::invalid_argument( "insertBackProjections: J and Z must have same number of entries"); @@ -188,7 +230,17 @@ void insertBackprojections(Values& values, const PinholeCamera& camera, } } -/// Insert multiple projection factors for a single pose key +/** + * @brief Insert multiple projection factors for a single pose key + * + * @param graph The nonlinear factor graph to add the factors to. + * @param i Camera key. + * @param J Vector of key indices. + * @param Z 2*J matrix of pixel values. + * @param model Factor noise model. + * @param K Calibration matrix. + * @param body_P_sensor Pose of the camera sensor in the body frame. + */ void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { diff --git a/python/gtsam/tests/test_Utilities.py b/python/gtsam/tests/test_Utilities.py new file mode 100644 index 000000000..851684f12 --- /dev/null +++ b/python/gtsam/tests/test_Utilities.py @@ -0,0 +1,196 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Utilities unit tests. +Author: Varun Agrawal +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestUtilites(GtsamTestCase): + """Test various GTSAM utilities.""" + def test_createKeyList(self): + """Test createKeyList.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyList(I) + self.assertEqual(kl.size(), 3) + + kl = gtsam.utilities.createKeyList("s", I) + self.assertEqual(kl.size(), 3) + + def test_createKeyVector(self): + """Test createKeyVector.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyVector(I) + self.assertEqual(len(kl), 3) + + kl = gtsam.utilities.createKeyVector("s", I) + self.assertEqual(len(kl), 3) + + def test_createKeySet(self): + """Test createKeySet.""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeySet(I) + self.assertEqual(kl.size(), 3) + + kl = gtsam.utilities.createKeySet("s", I) + self.assertEqual(kl.size(), 3) + + def test_extractPoint2(self): + """Test extractPoint2.""" + initial = gtsam.Values() + point2 = gtsam.Point2(0.0, 0.1) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, point2) + np.testing.assert_equal(gtsam.utilities.extractPoint2(initial), + point2.reshape(-1, 2)) + + def test_extractPoint3(self): + """Test extractPoint3.""" + initial = gtsam.Values() + point3 = gtsam.Point3(0.0, 0.1, 0.0) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, point3) + np.testing.assert_equal(gtsam.utilities.extractPoint3(initial), + point3.reshape(-1, 3)) + + def test_allPose2s(self): + """Test allPose2s.""" + initial = gtsam.Values() + initial.insert(0, gtsam.Pose2()) + initial.insert(1, gtsam.Pose2(1, 1, 1)) + initial.insert(2, gtsam.Point2(1, 1)) + initial.insert(3, gtsam.Point3(1, 2, 3)) + result = gtsam.utilities.allPose2s(initial) + self.assertEqual(result.size(), 2) + + def test_extractPose2(self): + """Test extractPose2.""" + initial = gtsam.Values() + pose2 = np.asarray((0.0, 0.1, 0.1)) + + initial.insert(1, gtsam.Pose2(*pose2)) + initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0)) + np.testing.assert_allclose(gtsam.utilities.extractPose2(initial), + pose2.reshape(-1, 3)) + + def test_allPose3s(self): + """Test allPose3s.""" + initial = gtsam.Values() + initial.insert(0, gtsam.Pose3()) + initial.insert(2, gtsam.Point2(1, 1)) + initial.insert(1, gtsam.Pose3()) + initial.insert(3, gtsam.Point3(1, 2, 3)) + result = gtsam.utilities.allPose3s(initial) + self.assertEqual(result.size(), 2) + + def test_extractPose3(self): + """Test extractPose3.""" + initial = gtsam.Values() + pose3 = np.asarray([1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.]) + initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) + initial.insert(2, gtsam.Pose3()) + np.testing.assert_allclose(gtsam.utilities.extractPose3(initial), + pose3.reshape(-1, 12)) + + def test_perturbPoint2(self): + """Test perturbPoint2.""" + values = gtsam.Values() + values.insert(0, gtsam.Pose3()) + values.insert(1, gtsam.Point2(1, 1)) + gtsam.utilities.perturbPoint2(values, 1.0) + self.assertTrue( + not np.allclose(values.atPoint2(1), gtsam.Point2(1, 1))) + + def test_perturbPose2(self): + """Test perturbPose2.""" + values = gtsam.Values() + values.insert(0, gtsam.Pose2()) + values.insert(1, gtsam.Point2(1, 1)) + gtsam.utilities.perturbPose2(values, 1, 1) + self.assertTrue(values.atPose2(0) != gtsam.Pose2()) + + def test_perturbPoint3(self): + """Test perturbPoint3.""" + values = gtsam.Values() + point3 = gtsam.Point3(0, 0, 0) + values.insert(0, gtsam.Pose2()) + values.insert(1, point3) + gtsam.utilities.perturbPoint3(values, 1) + self.assertTrue(not np.allclose(values.atPoint3(1), point3)) + + def test_insertBackprojections(self): + """Test insertBackprojections.""" + values = gtsam.Values() + cam = gtsam.PinholeCameraCal3_S2() + gtsam.utilities.insertBackprojections( + values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]), + 10) + np.testing.assert_allclose(values.atPoint3(0), + gtsam.Point3(200, 200, 10)) + + def test_insertProjectionFactors(self): + """Test insertProjectionFactors.""" + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), + gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2()) + self.assertEqual(graph.size(), 2) + + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), + gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(), + gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))) + self.assertEqual(graph.size(), 2) + + def test_reprojectionErrors(self): + """Test reprojectionErrors.""" + pixels = np.asarray([[20, 30], [20, 30]]) + I = [1, 2] + K = gtsam.Cal3_S2() + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K) + values = gtsam.Values() + values.insert(0, gtsam.Pose3()) + cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K) + gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10) + errors = gtsam.utilities.reprojectionErrors(graph, values) + np.testing.assert_allclose(errors, np.zeros((2, 2))) + + def test_localToWorld(self): + """Test localToWorld.""" + local = gtsam.Values() + local.insert(0, gtsam.Point2(10, 10)) + local.insert(1, gtsam.Pose2(6, 11, 0.0)) + base = gtsam.Pose2(1, 0, 0) + world = gtsam.utilities.localToWorld(local, base) + + expected_point2 = gtsam.Point2(11, 10) + expected_pose2 = gtsam.Pose2(7, 11, 0) + np.testing.assert_allclose(world.atPoint2(0), expected_point2) + np.testing.assert_allclose( + world.atPose2(1).matrix(), expected_pose2.matrix()) + + user_keys = [1] + world = gtsam.utilities.localToWorld(local, base, user_keys) + np.testing.assert_allclose( + world.atPose2(1).matrix(), expected_pose2.matrix()) + + # Raise error since 0 is not in user_keys + self.assertRaises(RuntimeError, world.atPoint2, 0) + + +if __name__ == "__main__": + unittest.main()