From e4c738dabf9b603cdbd26e2214611bd211bbcde9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 06:40:54 -0500 Subject: [PATCH 1/3] Deprecate SimpleCamera properly --- gtsam/geometry/SimpleCamera.cpp | 2 ++ gtsam/geometry/SimpleCamera.h | 17 ++++++++-- gtsam/gtsam.i | 59 +++++---------------------------- 3 files changed, 25 insertions(+), 53 deletions(-) diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index 6134ae3d4..d1a5ed330 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,6 +21,7 @@ namespace gtsam { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 SimpleCamera simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale @@ -45,5 +46,6 @@ namespace gtsam { return SimpleCamera(Pose3(wRc, T), Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); } +#endif } diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 82f26aee2..04746ba6f 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -19,14 +19,23 @@ #pragma once #include -#include +#include +#include +#include #include +#include namespace gtsam { - /// A simple camera class with a Cal3_S2 calibration -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; + /// Convenient aliases for Pinhole camera classes with different calibrations. + /// Also needed as forward declarations in the wrapper. + using PinholeCameraCal3_S2 = gtsam::PinholeCamera; + using PinholeCameraCal3Bundler = gtsam::PinholeCamera; + //TODO Need to fix issue 621 for this to work with wrapper + // using PinholeCameraCal3DS2 = gtsam::PinholeCamera; + // using PinholeCameraCal3Unified = gtsam::PinholeCamera; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x * Use PinholeCameraCal3_S2 instead @@ -140,4 +149,6 @@ struct traits : public internal::Manifold {}; template struct Range : HasRange {}; +#endif + } // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 1b4d976da..eb36e73a3 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -329,7 +329,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; @@ -1059,52 +1059,12 @@ class PinholeCamera { void serialize() const; }; +// Forward declaration of PinholeCameraCalX is defined here. #include -virtual class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration() const; - - // Manifold - gtsam::SimpleCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; - -}; - -gtsam::SimpleCamera simpleCamera(const Matrix& P); - // Some typedefs for common camera types // PinholeCameraCal3_S2 is the same as SimpleCamera above typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified +//TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; @@ -1150,7 +1110,7 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); - + //************************************************************************* // Symbolic //************************************************************************* @@ -2069,7 +2029,7 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template, gtsam::imuBias::ConstantBias}> + template, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph @@ -2493,7 +2453,7 @@ class ISAM2 { template , + gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; @@ -2527,12 +2487,11 @@ class NonlinearISAM { //************************************************************************* // Nonlinear factor types //************************************************************************* -#include #include #include #include -template}> +template}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2556,7 +2515,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation @@ -2675,7 +2634,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { gtsam::Point2 measured() const; }; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 +//TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; From 2703307a430cf7dd6ea58dd63e95d8b6795ba30c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 06:44:45 -0500 Subject: [PATCH 2/3] deprecate SimpleCamera tests --- gtsam/geometry/tests/testSimpleCamera.cpp | 4 ++++ gtsam_unstable/slam/serialization.cpp | 27 ++++++++++++++------- tests/testSerializationSLAM.cpp | 29 +++++++++++------------ 3 files changed, 36 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index edf122d3c..18a25c553 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -26,6 +26,8 @@ using namespace std; using namespace gtsam; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 + static const Cal3_S2 K(625, 625, 0, 0, 0); static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), @@ -149,6 +151,8 @@ TEST( SimpleCamera, simpleCamera) CHECK(assert_equal(expected, actual,1e-1)); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 8a661f2ef..803e4353a 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -43,7 +43,6 @@ typedef PriorFactor PriorFactorPose3; typedef PriorFactor PriorFactorCal3_S2; typedef PriorFactor PriorFactorCal3DS2; typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; typedef PriorFactor PriorFactorPinholeCameraCal3_S2; typedef PriorFactor PriorFactorStereoCamera; @@ -68,7 +67,6 @@ typedef NonlinearEquality NonlinearEqualityPose3; typedef NonlinearEquality NonlinearEqualityCal3_S2; typedef NonlinearEquality NonlinearEqualityCal3DS2; typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; typedef NonlinearEquality NonlinearEqualityStereoCamera; @@ -77,10 +75,8 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactor RangeFactorPose2; typedef RangeFactor RangeFactorPose3; typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; @@ -90,6 +86,7 @@ typedef GenericProjectionFactor GenericProjectionFactorC typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//TODO fix issue 621 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -129,7 +126,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); -GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::StereoCamera); @@ -150,7 +146,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); @@ -174,7 +169,6 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3") BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); @@ -182,9 +176,7 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); @@ -192,12 +184,29 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//TODO Fix issue 621 //BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 + +typedef PriorFactor PriorFactorSimpleCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorSimpleCamera; + +GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); + +#endif + + /* ************************************************************************* */ // Actual implementations of functions /* ************************************************************************* */ diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 84e521156..53086e921 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -89,10 +89,8 @@ typedef RangeFactor RangeFactor3D; typedef RangeFactor RangeFactorPose2; typedef RangeFactor RangeFactorPose3; typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; @@ -102,6 +100,7 @@ typedef GenericProjectionFactor GenericProjectionFactorC typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//TODO Fix issue 621 for this to work //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -145,7 +144,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); -GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::StereoCamera); @@ -190,9 +188,9 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point"); BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2"); BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); @@ -200,6 +198,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//TODO fix issue 621 //BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); @@ -352,9 +351,9 @@ TEST (testSerializationSLAM, factors) { RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1); RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1); RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1); - RangeFactorSimpleCameraPoint rangeFactorSimpleCameraPoint(a13, a05, 2.0, model1); + RangeFactorPinholeCameraCal3_S2Point rangeFactorPinholeCameraCal3_S2Point(a13, a05, 2.0, model1); RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1); - RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1); + RangeFactorPinholeCameraCal3_S2 rangeFactorPinholeCameraCal3_S2(a13, b13, 2.0, model1); BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); @@ -405,9 +404,9 @@ TEST (testSerializationSLAM, factors) { graph += rangeFactorPose2; graph += rangeFactorPose3; graph += rangeFactorCalibratedCameraPoint; - graph += rangeFactorSimpleCameraPoint; + graph += rangeFactorPinholeCameraCal3_S2Point; graph += rangeFactorCalibratedCamera; - graph += rangeFactorSimpleCamera; + graph += rangeFactorPinholeCameraCal3_S2; graph += bearingRangeFactor2D; @@ -463,9 +462,9 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(rangeFactorPose2)); EXPECT(equalsObj(rangeFactorPose3)); EXPECT(equalsObj(rangeFactorCalibratedCameraPoint)); - EXPECT(equalsObj(rangeFactorSimpleCameraPoint)); + EXPECT(equalsObj(rangeFactorPinholeCameraCal3_S2Point)); EXPECT(equalsObj(rangeFactorCalibratedCamera)); - EXPECT(equalsObj(rangeFactorSimpleCamera)); + EXPECT(equalsObj(rangeFactorPinholeCameraCal3_S2)); EXPECT(equalsObj(bearingRangeFactor2D)); @@ -521,9 +520,9 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(rangeFactorPose2)); EXPECT(equalsXML(rangeFactorPose3)); EXPECT(equalsXML(rangeFactorCalibratedCameraPoint)); - EXPECT(equalsXML(rangeFactorSimpleCameraPoint)); + EXPECT(equalsXML(rangeFactorPinholeCameraCal3_S2Point)); EXPECT(equalsXML(rangeFactorCalibratedCamera)); - EXPECT(equalsXML(rangeFactorSimpleCamera)); + EXPECT(equalsXML(rangeFactorPinholeCameraCal3_S2)); EXPECT(equalsXML(bearingRangeFactor2D)); @@ -579,9 +578,9 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(rangeFactorPose2)); EXPECT(equalsBinary(rangeFactorPose3)); EXPECT(equalsBinary(rangeFactorCalibratedCameraPoint)); - EXPECT(equalsBinary(rangeFactorSimpleCameraPoint)); + EXPECT(equalsBinary(rangeFactorPinholeCameraCal3_S2Point)); EXPECT(equalsBinary(rangeFactorCalibratedCamera)); - EXPECT(equalsBinary(rangeFactorSimpleCamera)); + EXPECT(equalsBinary(rangeFactorPinholeCameraCal3_S2)); EXPECT(equalsBinary(bearingRangeFactor2D)); From d9018a9593a7dc1009986ceb3d01ebf0362d5a93 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 10:40:39 -0500 Subject: [PATCH 3/3] update Python test --- python/gtsam/tests/test_SimpleCamera.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_SimpleCamera.py b/python/gtsam/tests/test_SimpleCamera.py index efdfec561..358eb1f48 100644 --- a/python/gtsam/tests/test_SimpleCamera.py +++ b/python/gtsam/tests/test_SimpleCamera.py @@ -14,11 +14,12 @@ import unittest import numpy as np import gtsam -from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 as SimpleCamera from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) + class TestSimpleCamera(GtsamTestCase): def test_constructor(self): @@ -29,15 +30,15 @@ class TestSimpleCamera(GtsamTestCase): def test_level2(self): # Create a level camera, looking in Y-direction - pose2 = Pose2(0.4,0.3,math.pi/2.0) + pose2 = Pose2(0.4, 0.3, math.pi/2.0) camera = SimpleCamera.Level(K, pose2, 0.1) # expected - x = Point3(1,0,0) - y = Point3(0,0,-1) - z = Point3(0,1,0) - wRc = Rot3(x,y,z) - expected = Pose3(wRc,Point3(0.4,0.3,0.1)) + x = Point3(1, 0, 0) + y = Point3(0, 0, -1) + z = Point3(0, 1, 0) + wRc = Rot3(x, y, z) + expected = Pose3(wRc, Point3(0.4, 0.3, 0.1)) self.gtsamAssertEquals(camera.pose(), expected, 1e-9)