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

Removed the use of the ADD_CLONE_NONLINEAR_FACTOR macro, documented instances of clone() in factors

release/4.3a0
Alex Cunningham 2012-06-09 21:06:06 +00:00
parent 3d2c3aff05
commit b602e75a99
24 changed files with 117 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/LocalizationExample2.cpp
View File

@ -47,8 +47,6 @@ public:
if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0);
return Vector_(2, q.x() - mx_, q.y() - my_);
}
ADD_CLONE_NONLINEAR_FACTOR(UnaryFactor)
};
/**

15
gtsam/nonlinear/NonlinearEquality.h
View File

@ -161,7 +161,10 @@ namespace gtsam {
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model));
}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// @}
@ -212,7 +215,10 @@ namespace gtsam {
virtual ~NonlinearEquality1() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** g(x) with optional derivative */
Vector evaluateError(const X& x1, boost::optional<Matrix&> H = boost::none) const {
@ -269,7 +275,10 @@ namespace gtsam {
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2,

3
gtsam/nonlinear/NonlinearFactor.h
View File

@ -36,9 +36,10 @@
/**
* Macro to add a standard clone function to a derived factor
* @DEPRECIATED: will go away shortly - just add the clone function directly
*/
#define ADD_CLONE_NONLINEAR_FACTOR(Derived) \
virtual gtsam::NonlinearFactor::shared_ptr clone() const { \
virtual gtsam::NonlinearFactor::shared_ptr clone() const { \
return boost::static_pointer_cast<gtsam::NonlinearFactor>( \
gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }

5
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -163,7 +163,10 @@ namespace gtsam {
return linearize(z_, u, p, j1, j2);
}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// @}

5
gtsam/slam/AntiFactor.h
View File

@ -50,7 +50,10 @@ namespace gtsam {
virtual ~AntiFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */

5
gtsam/slam/BearingFactor.h
View File

@ -55,7 +55,10 @@ namespace gtsam {
virtual ~BearingFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose& pose, const Point& point,

5
gtsam/slam/BearingRangeFactor.h
View File

@ -57,7 +57,10 @@ namespace gtsam {
virtual ~BearingRangeFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Print */
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {

5
gtsam/slam/BetweenFactor.h
View File

@ -62,7 +62,10 @@ namespace gtsam {
virtual ~BetweenFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */

5
gtsam/slam/GeneralSFMFactor.h
View File

@ -62,7 +62,10 @@ namespace gtsam {
virtual ~GeneralSFMFactor() {} ///< destructor
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print

5
gtsam/slam/PartialPriorFactor.h
View File

@ -87,7 +87,10 @@ namespace gtsam {
this->fillH();
}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */

5
gtsam/slam/PriorFactor.h
View File

@ -56,7 +56,10 @@ namespace gtsam {
Base(model, key), prior_(prior) {
}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */

5
gtsam/slam/ProjectionFactor.h
View File

@ -70,7 +70,10 @@ namespace gtsam {
/** Virtual destructor */
virtual ~GenericProjectionFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print

5
gtsam/slam/RangeFactor.h
View File

@ -51,7 +51,10 @@ namespace gtsam {
virtual ~RangeFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z */
Vector evaluateError(const Pose& pose, const Point& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

5
gtsam/slam/StereoFactor.h
View File

@ -57,7 +57,10 @@ public:
virtual ~GenericStereoFactor() {} ///< Virtual destructor
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print

7
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -51,11 +51,14 @@ public:
virtual ~FullIMUFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
const This* const f = dynamic_cast<const This*>(&e);
const This* const f = dynamic_cast<const This*>(&e);
return f && Base::equals(e) &&
equal_with_abs_tol(accel_, f->accel_, tol) &&
equal_with_abs_tol(gyro_, f->gyro_, tol) &&

7
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -44,11 +44,14 @@ public:
virtual ~IMUFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
const This* const f = dynamic_cast<const This*>(&e);
const This* const f = dynamic_cast<const This*>(&e);
return f && Base::equals(e) &&
equal_with_abs_tol(accel_, f->accel_, tol) &&
equal_with_abs_tol(gyro_, f->gyro_, tol) &&

5
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -72,7 +72,10 @@ public:
virtual ~VelocityConstraint() {}
ADD_CLONE_NONLINEAR_FACTOR(VelocityConstraint)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); }
/**
* Calculates the error for trapezoidal model given

5
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -59,7 +59,10 @@ public:
virtual ~LinearizedFactor() {}
ADD_CLONE_NONLINEAR_FACTOR(This);
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* Use this constructor with pre-constructed decoders

15
tests/simulated2D.h
View File

@ -135,7 +135,10 @@ namespace simulated2D {
virtual ~GenericPrior() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
private:
@ -178,7 +181,10 @@ namespace simulated2D {
virtual ~GenericOdometry() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
private:
@ -222,7 +228,10 @@ namespace simulated2D {
virtual ~GenericMeasurement() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
private:

35
tests/simulated2DConstraints.h
View File

@ -59,7 +59,10 @@ namespace simulated2D {
virtual ~ScalarCoordConstraint1() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* Constructor for constraint
@ -69,8 +72,8 @@ namespace simulated2D {
* @param mu is the penalty function gain
*/
ScalarCoordConstraint1(Key key, double c,
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
}
/**
@ -124,7 +127,10 @@ namespace simulated2D {
virtual ~MaxDistanceConstraint() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* Primary constructor for factor
@ -163,18 +169,21 @@ namespace simulated2D {
*/
template<class POSE, class POINT>
struct MinDistanceConstraint : public BoundingConstraint2<POSE, POINT> {
typedef BoundingConstraint2<POSE, POINT> Base; ///< Base class for factor
typedef MinDistanceConstraint<POSE, POINT> This; ///< This class for factor
typedef POSE Pose; ///< Type of pose variable constrained
typedef POINT Point; ///< Type of point variable constrained
typedef BoundingConstraint2<POSE, POINT> Base; ///< Base class for factor
typedef MinDistanceConstraint<POSE, POINT> This; ///< This class for factor
typedef POSE Pose; ///< Type of pose variable constrained
typedef POINT Point; ///< Type of point variable constrained
virtual ~MinDistanceConstraint() {}
virtual ~MinDistanceConstraint() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* Primary constructor for factor
* @param key1 is the first variable key
/**
* Primary constructor for factor
* @param key1 is the first variable key
* @param key2 is the second variable key
* @param range_bound is the minimum range allowed between the variables
* @param mu is the gain for the penalty function

5
tests/simulated2DOriented.h
View File

@ -118,7 +118,10 @@ namespace simulated2DOriented {
return measured_.localCoordinates(odo(x1, x2, H1, H2));
}
ADD_CLONE_NONLINEAR_FACTOR(This)
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
};

2
tests/smallExample.cpp
View File

@ -210,8 +210,6 @@ namespace example {
return (h(x) - z_).vector();
}
ADD_CLONE_NONLINEAR_FACTOR(UnaryFactor)
};
}

4
tests/testExtendedKalmanFilter.cpp
View File

@ -125,8 +125,6 @@ public:
virtual ~NonlinearMotionModel() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
// Calculate the next state prediction using the nonlinear function f()
T f(const T& x_t0) const {
@ -269,8 +267,6 @@ public:
virtual ~NonlinearMeasurementModel() {}
ADD_CLONE_NONLINEAR_FACTOR(This)
// Calculate the predicted measurement using the nonlinear function h()
// Byproduct: updates Jacobian H, and noiseModel (R)
Vector h(const T& x_t1) const {

7
tests/testNonlinearFactor.cpp
View File

@ -257,7 +257,9 @@ public:
return (Vector(1) << x1 + x2 + x3 + x4).finished();
}
ADD_CLONE_NONLINEAR_FACTOR(TestFactor4)
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); }
};
/* ************************************ */
@ -305,8 +307,6 @@ public:
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
}
ADD_CLONE_NONLINEAR_FACTOR(TestFactor5)
};
/* ************************************ */
@ -360,7 +360,6 @@ public:
return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
}
ADD_CLONE_NONLINEAR_FACTOR(TestFactor6)
};
/* ************************************ */