deprecate SimpleCamera tests
							parent
							
								
									e4c738dabf
								
							
						
					
					
						commit
						2703307a43
					
				|  | @ -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); } | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -43,7 +43,6 @@ typedef PriorFactor<Pose3>                PriorFactorPose3; | |||
| typedef PriorFactor<Cal3_S2>              PriorFactorCal3_S2; | ||||
| typedef PriorFactor<Cal3DS2>              PriorFactorCal3DS2; | ||||
| typedef PriorFactor<CalibratedCamera>     PriorFactorCalibratedCamera; | ||||
| typedef PriorFactor<SimpleCamera>         PriorFactorSimpleCamera; | ||||
| typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2; | ||||
| typedef PriorFactor<StereoCamera>         PriorFactorStereoCamera; | ||||
| 
 | ||||
|  | @ -68,7 +67,6 @@ typedef NonlinearEquality<Pose3>                  NonlinearEqualityPose3; | |||
| typedef NonlinearEquality<Cal3_S2>                NonlinearEqualityCal3_S2; | ||||
| typedef NonlinearEquality<Cal3DS2>                NonlinearEqualityCal3DS2; | ||||
| typedef NonlinearEquality<CalibratedCamera>       NonlinearEqualityCalibratedCamera; | ||||
| typedef NonlinearEquality<SimpleCamera>           NonlinearEqualitySimpleCamera; | ||||
| typedef NonlinearEquality<PinholeCameraCal3_S2>   NonlinearEqualityPinholeCameraCal3_S2; | ||||
| typedef NonlinearEquality<StereoCamera>           NonlinearEqualityStereoCamera; | ||||
| 
 | ||||
|  | @ -77,10 +75,8 @@ typedef RangeFactor<Pose3, Point3>                              RangeFactor3D; | |||
| typedef RangeFactor<Pose2, Pose2>                               RangeFactorPose2; | ||||
| typedef RangeFactor<Pose3, Pose3>                               RangeFactorPose3; | ||||
| typedef RangeFactor<CalibratedCamera, Point3>                   RangeFactorCalibratedCameraPoint; | ||||
| typedef RangeFactor<SimpleCamera, Point3>                       RangeFactorSimpleCameraPoint; | ||||
| typedef RangeFactor<PinholeCameraCal3_S2, Point3>               RangeFactorPinholeCameraCal3_S2Point; | ||||
| typedef RangeFactor<CalibratedCamera, CalibratedCamera>         RangeFactorCalibratedCamera; | ||||
| typedef RangeFactor<SimpleCamera, SimpleCamera>                 RangeFactorSimpleCamera; | ||||
| typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2; | ||||
| 
 | ||||
| typedef BearingRangeFactor<Pose2, Point2>  BearingRangeFactor2D; | ||||
|  | @ -90,6 +86,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC | |||
| typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2; | ||||
| 
 | ||||
| typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; | ||||
| //TODO fix issue 621
 | ||||
| //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 | ||||
| 
 | ||||
| typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> 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<SimpleCamera>               PriorFactorSimpleCamera; | ||||
| typedef NonlinearEquality<SimpleCamera>         NonlinearEqualitySimpleCamera; | ||||
| typedef RangeFactor<SimpleCamera, Point3>       RangeFactorSimpleCameraPoint; | ||||
| typedef RangeFactor<SimpleCamera, SimpleCamera> 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
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -89,10 +89,8 @@ typedef RangeFactor<Pose3, Point3>                              RangeFactor3D; | |||
| typedef RangeFactor<Pose2, Pose2>                               RangeFactorPose2; | ||||
| typedef RangeFactor<Pose3, Pose3>                               RangeFactorPose3; | ||||
| typedef RangeFactor<CalibratedCamera, Point3>                   RangeFactorCalibratedCameraPoint; | ||||
| typedef RangeFactor<SimpleCamera, Point3>                       RangeFactorSimpleCameraPoint; | ||||
| typedef RangeFactor<PinholeCameraCal3_S2, Point3>               RangeFactorPinholeCameraCal3_S2Point; | ||||
| typedef RangeFactor<CalibratedCamera, CalibratedCamera>         RangeFactorCalibratedCamera; | ||||
| typedef RangeFactor<SimpleCamera, SimpleCamera>                 RangeFactorSimpleCamera; | ||||
| typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2; | ||||
| 
 | ||||
| typedef BearingRangeFactor<Pose2, Point2>  BearingRangeFactor2D; | ||||
|  | @ -102,6 +100,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC | |||
| typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2; | ||||
| 
 | ||||
| typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; | ||||
| //TODO Fix issue 621 for this to work
 | ||||
| //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
 | ||||
| 
 | ||||
| typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> 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>(rangeFactorPose2)); | ||||
|   EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3)); | ||||
|   EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint)); | ||||
|   EXPECT(equalsObj<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint)); | ||||
|   EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point)); | ||||
|   EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera)); | ||||
|   EXPECT(equalsObj<RangeFactorSimpleCamera>(rangeFactorSimpleCamera)); | ||||
|   EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2)); | ||||
| 
 | ||||
|   EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D)); | ||||
| 
 | ||||
|  | @ -521,9 +520,9 @@ TEST (testSerializationSLAM, factors) { | |||
|   EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2)); | ||||
|   EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3)); | ||||
|   EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint)); | ||||
|   EXPECT(equalsXML<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint)); | ||||
|   EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point)); | ||||
|   EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera)); | ||||
|   EXPECT(equalsXML<RangeFactorSimpleCamera>(rangeFactorSimpleCamera)); | ||||
|   EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2)); | ||||
| 
 | ||||
|   EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D)); | ||||
| 
 | ||||
|  | @ -579,9 +578,9 @@ TEST (testSerializationSLAM, factors) { | |||
|   EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2)); | ||||
|   EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3)); | ||||
|   EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint)); | ||||
|   EXPECT(equalsBinary<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint)); | ||||
|   EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point)); | ||||
|   EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera)); | ||||
|   EXPECT(equalsBinary<RangeFactorSimpleCamera>(rangeFactorSimpleCamera)); | ||||
|   EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2)); | ||||
| 
 | ||||
|   EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D)); | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue