From fbcfbf0cdddef17ccf9dfe26b236fa7f4ba71fdf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Dec 2018 11:19:46 -0500 Subject: [PATCH] Made naming convention in wrapper uniform. 2D means Pose2 + Point2 3D means Pose3 + Point3 --- gtsam.h | 10 ++++--- gtsam_unstable/slam/serialization.cpp | 8 +++--- gtsampy.h | 4 +-- matlab/+gtsam/Contents.m | 4 +-- .../gtsam_examples/RangeISAMExample_plaza.m | 6 ++-- .../gtsam_examples/RangeSLAMExample_plaza.m | 2 +- matlab/gtsam_tests/testSerialization.m | 6 ++-- .../SmartRangeFactorExample.m | 20 ++++++------- tests/testSerializationSLAM.cpp | 28 +++++++++---------- wrap/Module.cpp | 2 +- 10 files changed, 46 insertions(+), 44 deletions(-) diff --git a/gtsam.h b/gtsam.h index d681ddb0d..629fd70cf 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2270,8 +2270,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { void serialize() const; }; -typedef gtsam::RangeFactor RangeFactorPosePoint2; -typedef gtsam::RangeFactor RangeFactorPosePoint3; +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; @@ -2289,8 +2289,8 @@ virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { void serialize() const; }; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint2; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint3; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; @@ -2304,6 +2304,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor { }; typedef gtsam::BearingFactor BearingFactor2D; +typedef gtsam::BearingFactor BearingFactorPose2; #include template @@ -2317,6 +2318,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor { }; typedef gtsam::BearingRangeFactor BearingRangeFactor2D; +typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; #include diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 7928a2aac..17c95e614 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -70,8 +70,8 @@ typedef NonlinearEquality NonlinearEqualityCalibratedCamera; typedef NonlinearEquality NonlinearEqualitySimpleCamera; typedef NonlinearEquality NonlinearEqualityStereoCamera; -typedef RangeFactor RangeFactorPosePoint2; -typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; typedef RangeFactor RangeFactorPose2; typedef RangeFactor RangeFactorPose3; typedef RangeFactor RangeFactorCalibratedCameraPoint; @@ -172,8 +172,8 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqua BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); +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"); diff --git a/gtsampy.h b/gtsampy.h index 9cadf6be3..27af74e74 100644 --- a/gtsampy.h +++ b/gtsampy.h @@ -2258,8 +2258,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); }; -typedef gtsam::RangeFactor RangeFactorPosePoint2; -typedef gtsam::RangeFactor RangeFactorPosePoint3; +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 023c61dbe..10fd5e142 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -142,8 +142,8 @@ % RangeFactorCalibratedCameraPoint - class RangeFactorCalibratedCameraPoint, see Doxygen page for details % RangeFactorPose2 - class RangeFactorPose2, see Doxygen page for details % RangeFactorPose3 - class RangeFactorPose3, see Doxygen page for details -% RangeFactorPosePoint2 - class RangeFactorPosePoint2, see Doxygen page for details -% RangeFactorPosePoint3 - class RangeFactorPosePoint3, see Doxygen page for details +% RangeFactor2D - class RangeFactor2D, see Doxygen page for details +% RangeFactor3D - class RangeFactor3D, see Doxygen page for details % RangeFactorSimpleCamera - class RangeFactorSimpleCamera, see Doxygen page for details % RangeFactorSimpleCameraPoint - class RangeFactorSimpleCameraPoint, see Doxygen page for details % VisualISAMGenerateData - VisualISAMGenerateData creates data for viusalSLAM::iSAM examples diff --git a/matlab/gtsam_examples/RangeISAMExample_plaza.m b/matlab/gtsam_examples/RangeISAMExample_plaza.m index cffe81c30..e31fb18a8 100644 --- a/matlab/gtsam_examples/RangeISAMExample_plaza.m +++ b/matlab/gtsam_examples/RangeISAMExample_plaza.m @@ -125,7 +125,7 @@ for i=1:M % M j = TD(k,3); range = TD(k,4); if addRange - factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range); + factor = RangeFactor2D(i, symbol('L',j), range, noiseModels.range); % Throw out obvious outliers based on current landmark estimates error=factor.unwhitenedError(landmarkEstimates); if k<=minK || abs(error)<5 @@ -146,14 +146,14 @@ for i=1:M % M end isam.update(newFactors, initial); result = isam.calculateEstimate(); - lastPose = result.at(i); + lastPose = result.atPose2(i); % update landmark estimates if addRange landmarkEstimates = Values; for jj=1:size(TL,1) j=TL(jj,1); key = symbol('L',j); - landmarkEstimates.insert(key,result.at(key)); + landmarkEstimates.insert(key,result.atPoint2(key)); end end newFactors = NonlinearFactorGraph; diff --git a/matlab/gtsam_examples/RangeSLAMExample_plaza.m b/matlab/gtsam_examples/RangeSLAMExample_plaza.m index bd643d854..d2d1e9d70 100644 --- a/matlab/gtsam_examples/RangeSLAMExample_plaza.m +++ b/matlab/gtsam_examples/RangeSLAMExample_plaza.m @@ -76,7 +76,7 @@ for i=1:M while k<=K && t>=TD(k,1) j = TD(k,3); range = TD(k,4); - factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range); + factor = RangeFactor2D(i, symbol('L',j), range, noiseModels.range); graph.add(factor); k=k+1; end diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m index baacb198b..f8b21b7ad 100644 --- a/matlab/gtsam_tests/testSerialization.m +++ b/matlab/gtsam_tests/testSerialization.m @@ -60,9 +60,9 @@ graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); % Range Factors rNoise = noiseModel.Diagonal.Sigmas([0.2]); -graph.add(RangeFactorPosePoint2(i1, j1, sqrt(4+4), rNoise)); -graph.add(RangeFactorPosePoint2(i2, j1, 2, rNoise)); -graph.add(RangeFactorPosePoint2(i3, j2, 2, rNoise)); +graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise)); +graph.add(RangeFactor2D(i2, j1, 2, rNoise)); +graph.add(RangeFactor2D(i3, j2, 2, rNoise)); % Bearing Factors degrees = pi/180; diff --git a/matlab/unstable_examples/SmartRangeFactorExample.m b/matlab/unstable_examples/SmartRangeFactorExample.m index 7535447df..a192a1f5e 100644 --- a/matlab/unstable_examples/SmartRangeFactorExample.m +++ b/matlab/unstable_examples/SmartRangeFactorExample.m @@ -103,9 +103,9 @@ for ind_pose = 2:7 r2 = curr_pose.range(lmk1); % range of lmk1 wrt x2 srf1.addRange(key_curr, r2); - rangef1 = RangeFactorPosePoint2(key_prev, lmkKey(1), r1, noiseRange); + rangef1 = RangeFactor2D(key_prev, lmkKey(1), r1, noiseRange); fullGraph.add(rangef1); - rangef2 = RangeFactorPosePoint2(key_curr, lmkKey(1), r2, noiseRange); + rangef2 = RangeFactor2D(key_curr, lmkKey(1), r2, noiseRange); fullGraph.add(rangef2); if goodInitFlag_lmk1==1 @@ -123,9 +123,9 @@ for ind_pose = 2:7 r4 = curr_pose.range(lmk2); % range of lmk2 wrt x3 srf2.addRange(key_curr, r4); - rangef3 = RangeFactorPosePoint2(key_curr, lmkKey(1), r3, noiseRange); + rangef3 = RangeFactor2D(key_curr, lmkKey(1), r3, noiseRange); fullGraph.add(rangef3); - rangef4 = RangeFactorPosePoint2(key_curr, lmkKey(2), r4, noiseRange); + rangef4 = RangeFactor2D(key_curr, lmkKey(2), r4, noiseRange); % IF WE ADD FACTOR HERE IT CRASHES: fullGraph.add(rangef4); %==================================================================== case 4 @@ -138,9 +138,9 @@ for ind_pose = 2:7 % DELAYED INITIALIZATION: fullGraph.add(rangef4); - rangef5 = RangeFactorPosePoint2(key_curr, lmkKey(2), r5, noiseRange); + rangef5 = RangeFactor2D(key_curr, lmkKey(2), r5, noiseRange); fullGraph.add(rangef5); - rangef6 = RangeFactorPosePoint2(key_curr, lmkKey(3), r6, noiseRange); + rangef6 = RangeFactor2D(key_curr, lmkKey(3), r6, noiseRange); % IF WE ADD FACTOR HERE IT CRASHES: fullGraph.add(rangef6); if goodInitFlag_lmk2==1 @@ -160,9 +160,9 @@ for ind_pose = 2:7 % DELAYED INITIALIZATION: fullGraph.add(rangef6); - rangef7 = RangeFactorPosePoint2(key_curr, lmkKey(2), r7, noiseRange); + rangef7 = RangeFactor2D(key_curr, lmkKey(2), r7, noiseRange); fullGraph.add(rangef7); - rangef8 = RangeFactorPosePoint2(key_curr, lmkKey(3), r8, noiseRange); + rangef8 = RangeFactor2D(key_curr, lmkKey(3), r8, noiseRange); fullGraph.add(rangef8); if goodInitFlag_lmk3==1 @@ -176,7 +176,7 @@ for ind_pose = 2:7 r9 = curr_pose.range(lmk3); % range of lmk3 wrt x6 srf3.addRange(key_curr, r9); - rangef9 = RangeFactorPosePoint2(key_curr, lmkKey(3), r9, noiseRange); + rangef9 = RangeFactor2D(key_curr, lmkKey(3), r9, noiseRange); fullGraph.add(rangef9); case 7 % x6-lmk3 @@ -184,7 +184,7 @@ for ind_pose = 2:7 srf3.addRange(key_curr, r10); smartGraph.add(srf3); - rangef10 = RangeFactorPosePoint2(key_curr, lmkKey(3), r10, noiseRange); + rangef10 = RangeFactor2D(key_curr, lmkKey(3), r10, noiseRange); fullGraph.add(rangef10); end diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 33453d7d3..6bc155214 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -96,8 +96,8 @@ typedef NonlinearEquality NonlinearEqualityCalibratedCamera; typedef NonlinearEquality NonlinearEqualitySimpleCamera; typedef NonlinearEquality NonlinearEqualityStereoCamera; -typedef RangeFactor RangeFactorPosePoint2; -typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; typedef RangeFactor RangeFactorPose2; typedef RangeFactor RangeFactorPose3; typedef RangeFactor RangeFactorCalibratedCameraPoint; @@ -204,8 +204,8 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqua BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); +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"); @@ -378,8 +378,8 @@ TEST (testSerializationSLAM, factors) { NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera); - RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1); - RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1); + RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1); + RangeFactor3D rangeFactor3D(a09, a05, 2.0, model1); RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1); RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1); RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1); @@ -439,8 +439,8 @@ TEST (testSerializationSLAM, factors) { graph += nonlinearEqualitySimpleCamera; graph += nonlinearEqualityStereoCamera; - graph += rangeFactorPosePoint2; - graph += rangeFactorPosePoint3; + graph += rangeFactor2D; + graph += rangeFactor3D; graph += rangeFactorPose2; graph += rangeFactorPose3; graph += rangeFactorCalibratedCameraPoint; @@ -505,8 +505,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); EXPECT(equalsObj(nonlinearEqualityStereoCamera)); - EXPECT(equalsObj(rangeFactorPosePoint2)); - EXPECT(equalsObj(rangeFactorPosePoint3)); + EXPECT(equalsObj(rangeFactor2D)); + EXPECT(equalsObj(rangeFactor3D)); EXPECT(equalsObj(rangeFactorPose2)); EXPECT(equalsObj(rangeFactorPose3)); EXPECT(equalsObj(rangeFactorCalibratedCameraPoint)); @@ -571,8 +571,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); EXPECT(equalsXML(nonlinearEqualityStereoCamera)); - EXPECT(equalsXML(rangeFactorPosePoint2)); - EXPECT(equalsXML(rangeFactorPosePoint3)); + EXPECT(equalsXML(rangeFactor2D)); + EXPECT(equalsXML(rangeFactor3D)); EXPECT(equalsXML(rangeFactorPose2)); EXPECT(equalsXML(rangeFactorPose3)); EXPECT(equalsXML(rangeFactorCalibratedCameraPoint)); @@ -637,8 +637,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(nonlinearEqualitySimpleCamera)); EXPECT(equalsBinary(nonlinearEqualityStereoCamera)); - EXPECT(equalsBinary(rangeFactorPosePoint2)); - EXPECT(equalsBinary(rangeFactorPosePoint3)); + EXPECT(equalsBinary(rangeFactor2D)); + EXPECT(equalsBinary(rangeFactor3D)); EXPECT(equalsBinary(rangeFactorPose2)); EXPECT(equalsBinary(rangeFactorPose3)); EXPECT(equalsBinary(rangeFactorCalibratedCameraPoint)); diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 5cf3b5d6c..a3b8df630 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -127,7 +127,7 @@ void Module::parseMarkup(const std::string& data) { TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList); - // typedef gtsam::RangeFactor RangeFactorPosePoint2; + // typedef gtsam::RangeFactor RangeFactor2D; TypeGrammar instantiationClass_g(singleInstantiation.class_); Rule templateSingleInstantiation_p = (str_p("typedef") >> instantiationClass_g >>