12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Made naming convention in wrapper uniform. 

2D means Pose2 + Point2
3D means Pose3 + Point3
release/4.3a0
Frank Dellaert 2018-12-31 11:19:46 -05:00
parent f54b078447
commit fbcfbf0cdd
10 changed files with 46 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam.h
View File

@ -2270,8 +2270,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
void serialize() const;
};
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
@ -2289,8 +2289,8 @@ virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
void serialize() const;
};
typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransformPosePoint2;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransformPosePoint3;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransform2D;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransform3D;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Pose2> RangeFactorWithTransformPose2;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Pose3> RangeFactorWithTransformPose3;
@ -2304,6 +2304,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
#include <gtsam/sam/BearingRangeFactor.h>
template<POSE, POINT, BEARING, RANGE>
@ -2317,6 +2318,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double> BearingRangeFactorPose2;
#include <gtsam/slam/ProjectionFactor.h>

8
gtsam_unstable/slam/serialization.cpp
View File

@ -70,8 +70,8 @@ typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
typedef RangeFactor<Pose2, Point2> RangeFactorPosePoint2;
typedef RangeFactor<Pose3, Point3> RangeFactorPosePoint3;
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> 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");

4
gtsampy.h
View File

@ -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<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;

4
matlab/+gtsam/Contents.m
View File

@ -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

6
matlab/gtsam_examples/RangeISAMExample_plaza.m
View File

@ -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;

2
matlab/gtsam_examples/RangeSLAMExample_plaza.m
View File

@ -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

6
matlab/gtsam_tests/testSerialization.m
View File

@ -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;

20
matlab/unstable_examples/SmartRangeFactorExample.m
View File

@ -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

28
tests/testSerializationSLAM.cpp
View File

@ -96,8 +96,8 @@ typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
typedef RangeFactor<Pose2, Point2> RangeFactorPosePoint2;
typedef RangeFactor<Pose3, Point3> RangeFactorPosePoint3;
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
typedef RangeFactor<CalibratedCamera, Point3> 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>(nonlinearEqualitySimpleCamera));
EXPECT(equalsObj<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
EXPECT(equalsObj<RangeFactorPosePoint2>(rangeFactorPosePoint2));
EXPECT(equalsObj<RangeFactorPosePoint3>(rangeFactorPosePoint3));
EXPECT(equalsObj<RangeFactor2D>(rangeFactor2D));
EXPECT(equalsObj<RangeFactor3D>(rangeFactor3D));
EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2));
EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3));
EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
@ -571,8 +571,8 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsXML<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
EXPECT(equalsXML<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
EXPECT(equalsXML<RangeFactorPosePoint2>(rangeFactorPosePoint2));
EXPECT(equalsXML<RangeFactorPosePoint3>(rangeFactorPosePoint3));
EXPECT(equalsXML<RangeFactor2D>(rangeFactor2D));
EXPECT(equalsXML<RangeFactor3D>(rangeFactor3D));
EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2));
EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3));
EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
@ -637,8 +637,8 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsBinary<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
EXPECT(equalsBinary<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
EXPECT(equalsBinary<RangeFactorPosePoint2>(rangeFactorPosePoint2));
EXPECT(equalsBinary<RangeFactorPosePoint3>(rangeFactorPosePoint3));
EXPECT(equalsBinary<RangeFactor2D>(rangeFactor2D));
EXPECT(equalsBinary<RangeFactor3D>(rangeFactor3D));
EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2));
EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3));
EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));

2
wrap/Module.cpp
View File

@ -127,7 +127,7 @@ void Module::parseMarkup(const std::string& data) {
TemplateInstantiationTypedef singleInstantiation, singleInstantiation0;
TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList);
// typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
// typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
TypeGrammar instantiationClass_g(singleInstantiation.class_);
Rule templateSingleInstantiation_p =
(str_p("typedef") >> instantiationClass_g >>