diff --git a/.cproject b/.cproject
index efdaf0c8a..be5e7bc64 100644
--- a/.cproject
+++ b/.cproject
@@ -311,6 +311,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -337,7 +345,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -345,7 +352,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -393,7 +399,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -401,7 +406,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -409,7 +413,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -425,20 +428,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -465,6 +459,7 @@
make
+
testGraph.run
true
false
@@ -536,6 +531,7 @@
make
+
testInference.run
true
false
@@ -543,6 +539,7 @@
make
+
testGaussianFactor.run
true
false
@@ -550,6 +547,7 @@
make
+
testJunctionTree.run
true
false
@@ -557,6 +555,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -564,6 +563,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -665,22 +665,6 @@
false
true
-
- make
- -j2
- tests/testPose2.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose3.run
- true
- true
- true
-
make
-j2
@@ -697,6 +681,22 @@
true
true
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
make
-j2
@@ -721,26 +721,18 @@
true
true
-
+
make
- -j2
- all
+ -j5
+ testValues.run
true
true
true
-
+
make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
+ -j5
+ testOrdering.run
true
true
true
@@ -777,18 +769,26 @@
true
true
-
+
make
- -j5
- testValues.run
+ -j2
+ all
true
true
true
-
+
make
- -j5
- testOrdering.run
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
true
true
true
@@ -1049,6 +1049,14 @@
true
true
+
+ make
+ -j5
+ testNonlinearFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -1139,7 +1147,6 @@
make
-
testErrors.run
true
false
@@ -1595,6 +1602,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -1634,6 +1642,7 @@
make
+
testSimulated2D.run
true
false
@@ -1641,6 +1650,7 @@
make
+
testSimulated3D.run
true
false
@@ -1832,6 +1842,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -1853,6 +1864,102 @@
true
true
+
+ make
+ -j2
+ testRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testRot2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testHomography2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPoint2.run
+ true
+ true
+ true
+
make
-j2
@@ -2054,7 +2161,6 @@
cpack
-
-G DEB
true
false
@@ -2062,7 +2168,6 @@
cpack
-
-G RPM
true
false
@@ -2070,7 +2175,6 @@
cpack
-
-G TGZ
true
false
@@ -2078,7 +2182,6 @@
cpack
-
--config CPackSourceConfig.cmake
true
false
@@ -2092,98 +2195,42 @@
true
true
-
+
make
- -j2
- testRot3.run
+ -j5
+ wrap.testSpirit.run
true
true
true
-
+
make
- -j2
- testRot2.run
+ -j5
+ wrap.testWrap.run
true
true
true
-
+
make
- -j2
- testPose3.run
+ -j5
+ check.wrap
true
true
true
-
+
make
- -j2
- timeRot3.run
+ -j5
+ wrap_gtsam
true
true
true
-
+
make
- -j2
- testPose2.run
- true
- true
- true
-
-
- make
- -j2
- testCal3_S2.run
- true
- true
- true
-
-
- make
- -j2
- testSimpleCamera.run
- true
- true
- true
-
-
- make
- -j2
- testHomography2.run
- true
- true
- true
-
-
- make
- -j2
- testCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testPoint2.run
+ -j5
+ wrap
true
true
true
@@ -2227,46 +2274,6 @@
false
true
-
- make
- -j5
- wrap.testSpirit.run
- true
- true
- true
-
-
- make
- -j5
- wrap.testWrap.run
- true
- true
- true
-
-
- make
- -j5
- check.wrap
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam
- true
- true
- true
-
-
- make
- -j5
- wrap
- true
- true
- true
-
diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index 7d0e37e7f..0280a4f8d 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -44,6 +44,8 @@ public:
virtual ~ResectioningFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(ResectioningFactor)
+
virtual Vector evaluateError(const Pose3& X, boost::optional H = boost::none) const {
SimpleCamera camera(*K_, X);
Point2 reprojectionError(camera.project(P_, H) - p_);
diff --git a/examples/LocalizationExample2.cpp b/examples/LocalizationExample2.cpp
index 419975537..a19526a67 100644
--- a/examples/LocalizationExample2.cpp
+++ b/examples/LocalizationExample2.cpp
@@ -39,12 +39,16 @@ public:
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1(model, j), mx_(x), my_(y) {}
+ virtual ~UnaryFactor() {}
+
Vector evaluateError(const Pose2& q,
boost::optional H = boost::none) const
{
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)
};
/**
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index a1c685668..e9710c780 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -161,6 +161,8 @@ namespace gtsam {
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model));
}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/// @}
private:
@@ -190,6 +192,7 @@ namespace gtsam {
protected:
typedef NoiseModelFactor1 Base;
+ typedef NonlinearEquality1 This;
/** default constructor to allow for serialization */
NonlinearEquality1() {}
@@ -209,6 +212,8 @@ namespace gtsam {
virtual ~NonlinearEquality1() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** g(x) with optional derivative */
Vector evaluateError(const X& x1, boost::optional H = boost::none) const {
if (H) (*H) = eye(x1.dim());
@@ -248,6 +253,7 @@ namespace gtsam {
protected:
typedef NoiseModelFactor2 Base;
+ typedef NonlinearEquality2 This;
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
@@ -263,6 +269,8 @@ namespace gtsam {
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2,
boost::optional H1 = boost::none,
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index bf19555bf..809df99fa 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -35,6 +35,14 @@
#include
#include
+/**
+ * Macro to add a standard clone function to a derived factor
+ */
+#define ADD_CLONE_NONLINEAR_FACTOR(Derived) \
+ virtual gtsam::NonlinearFactor::shared_ptr clone() const { \
+ return boost::static_pointer_cast( \
+ gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); }
+
namespace gtsam {
/* ************************************************************************* */
@@ -146,6 +154,14 @@ public:
return IndexFactor::shared_ptr(new IndexFactor(indices));
}
+ /**
+ * Creates a shared_ptr clone of the factor - needs to be specialized to allow
+ * for subclasses
+ *
+ * Default implementation will slice the factor
+ */
+ virtual shared_ptr clone() const =0;
+
}; // \class NonlinearFactor
/* ************************************************************************* */
diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h
index f5fb7fae1..354a1bcbc 100644
--- a/gtsam/nonlinear/WhiteNoiseFactor.h
+++ b/gtsam/nonlinear/WhiteNoiseFactor.h
@@ -163,6 +163,8 @@ namespace gtsam {
return linearize(z_, u, p, j1, j2);
}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/// @}
};
diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h
index 633360fed..47e0fd40c 100644
--- a/gtsam/slam/AntiFactor.h
+++ b/gtsam/slam/AntiFactor.h
@@ -50,6 +50,8 @@ namespace gtsam {
virtual ~AntiFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** implement functions needed for Testable */
/** print */
diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h
index 7c208436a..edd484e36 100644
--- a/gtsam/slam/BearingFactor.h
+++ b/gtsam/slam/BearingFactor.h
@@ -55,6 +55,8 @@ namespace gtsam {
virtual ~BearingFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose& pose, const Point& point,
boost::optional H1, boost::optional H2) const {
diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h
index 4c170c811..9fe70123e 100644
--- a/gtsam/slam/BearingRangeFactor.h
+++ b/gtsam/slam/BearingRangeFactor.h
@@ -57,6 +57,8 @@ namespace gtsam {
virtual ~BearingRangeFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** Print */
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << ": BearingRangeFactor("
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 2df0b98de..ec55c253b 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -62,6 +62,8 @@ namespace gtsam {
virtual ~BetweenFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** implement functions needed for Testable */
/** print */
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 4f1f48958..d046d0e1b 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -61,6 +61,8 @@ namespace gtsam {
virtual ~GeneralSFMFactor() {} ///< destructor
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/**
* print
* @param s optional string naming the factor
diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h
index 4c7069174..479fd3fa6 100644
--- a/gtsam/slam/PartialPriorFactor.h
+++ b/gtsam/slam/PartialPriorFactor.h
@@ -86,6 +86,8 @@ namespace gtsam {
this->fillH();
}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** implement functions needed for Testable */
/** print */
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index 2ab011bf5..6d958c652 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -56,6 +56,8 @@ namespace gtsam {
Base(model, key), prior_(prior) {
}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** implement functions needed for Testable */
/** print */
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index d5e9048e5..179362a42 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -70,6 +70,8 @@ namespace gtsam {
/** Virtual destructor */
virtual ~GenericProjectionFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/**
* print
* @param s optional string naming the factor
diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h
index 489f751db..ee3dc0b8e 100644
--- a/gtsam/slam/RangeFactor.h
+++ b/gtsam/slam/RangeFactor.h
@@ -51,6 +51,8 @@ namespace gtsam {
virtual ~RangeFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** h(x)-z */
Vector evaluateError(const Pose& pose, const Point& point, boost::optional H1, boost::optional H2) const {
double hx = pose.range(point, H1, H2);
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index ae2deeae3..500009ab4 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -34,6 +34,7 @@ public:
// shorthand for base class type
typedef NoiseModelFactor2 Base; ///< typedef for base class
+ typedef GenericStereoFactor This; ///< typedef for this class (with templates)
typedef boost::shared_ptr shared_ptr; ///< typedef for shared pointer to this object
typedef POSE CamPose; ///< typedef for Pose Lie Value type
@@ -56,6 +57,8 @@ public:
virtual ~GenericStereoFactor() {} ///< Virtual destructor
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/**
* print
* @param s optional string naming the factor
diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h
index ca9c0cf15..e9b4f2bc9 100644
--- a/gtsam/slam/simulated2D.h
+++ b/gtsam/slam/simulated2D.h
@@ -123,6 +123,7 @@ namespace simulated2D {
class GenericPrior: public NoiseModelFactor1 {
public:
typedef NoiseModelFactor1 Base; ///< base class
+ typedef GenericPrior This;
typedef boost::shared_ptr > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type
@@ -138,11 +139,14 @@ namespace simulated2D {
return (prior(x, H) - measured_).vector();
}
+ virtual ~GenericPrior() {}
+
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
private:
/// Default constructor
- GenericPrior() {
- }
+ GenericPrior() { }
/// Serialization function
friend class boost::serialization::access;
@@ -160,6 +164,7 @@ namespace simulated2D {
class GenericOdometry: public NoiseModelFactor2 {
public:
typedef NoiseModelFactor2 Base; ///< base class
+ typedef GenericOdometry This;
typedef boost::shared_ptr > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type
@@ -177,11 +182,14 @@ namespace simulated2D {
return (odo(x1, x2, H1, H2) - measured_).vector();
}
+ virtual ~GenericOdometry() {}
+
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
private:
/// Default constructor
- GenericOdometry() {
- }
+ GenericOdometry() { }
/// Serialization function
friend class boost::serialization::access;
@@ -199,6 +207,7 @@ namespace simulated2D {
class GenericMeasurement: public NoiseModelFactor2 {
public:
typedef NoiseModelFactor2 Base; ///< base class
+ typedef GenericMeasurement This;
typedef boost::shared_ptr > shared_ptr;
typedef POSE Pose; ///< shortcut to Pose type
typedef LANDMARK Landmark; ///< shortcut to Landmark type
@@ -217,11 +226,14 @@ namespace simulated2D {
return (mea(x1, x2, H1, H2) - measured_).vector();
}
+ virtual ~GenericMeasurement() {}
+
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
private:
/// Default constructor
- GenericMeasurement() {
- }
+ GenericMeasurement() { }
/// Serialization function
friend class boost::serialization::access;
diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h
index b44062933..4594a8e1f 100644
--- a/gtsam/slam/simulated2DConstraints.h
+++ b/gtsam/slam/simulated2DConstraints.h
@@ -53,11 +53,14 @@ namespace simulated2D {
template
struct ScalarCoordConstraint1: public BoundingConstraint1 {
typedef BoundingConstraint1 Base; ///< Base class convenience typedef
+ typedef ScalarCoordConstraint1 This; ///< This class convenience typedef
typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef VALUE Point; ///< Constrained variable type
virtual ~ScalarCoordConstraint1() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/**
* Constructor for constraint
* @param key is the label for the constrained variable
@@ -116,10 +119,13 @@ namespace simulated2D {
template
struct MaxDistanceConstraint : public BoundingConstraint2 {
typedef BoundingConstraint2 Base; ///< Base class for factor
+ typedef MaxDistanceConstraint This; ///< This class for factor
typedef VALUE Point; ///< Type of variable constrained
virtual ~MaxDistanceConstraint() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/**
* Primary constructor for factor
* @param key1 is the first variable key
@@ -158,11 +164,14 @@ namespace simulated2D {
template
struct MinDistanceConstraint : public BoundingConstraint2 {
typedef BoundingConstraint2 Base; ///< Base class for factor
+ typedef MinDistanceConstraint This; ///< This class for factor
typedef POSE Pose; ///< Type of pose variable constrained
typedef POINT Point; ///< Type of point variable constrained
virtual ~MinDistanceConstraint() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/**
* Primary constructor for factor
* @param key1 is the first variable key
diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h
index 242aab94b..c7ebd466c 100644
--- a/gtsam/slam/simulated2DOriented.h
+++ b/gtsam/slam/simulated2DOriented.h
@@ -102,6 +102,8 @@ namespace simulated2DOriented {
struct GenericOdometry: public NoiseModelFactor2 {
Pose2 measured_; ///< Between measurement for odometry factor
+ typedef GenericOdometry This;
+
/**
* Creates an odometry factor between two poses
*/
@@ -110,6 +112,8 @@ namespace simulated2DOriented {
NoiseModelFactor2(model, i1, i2), measured_(measured) {
}
+ virtual ~GenericOdometry() {}
+
/// Evaluate error and optionally derivative
Vector evaluateError(const VALUE& x1, const VALUE& x2,
boost::optional H1 = boost::none,
@@ -117,6 +121,8 @@ namespace simulated2DOriented {
return measured_.localCoordinates(odo(x1, x2, H1, H2));
}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
};
typedef GenericOdometry Odometry;
diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp
index 170f3ef03..fae2d2e22 100644
--- a/gtsam/slam/smallExample.cpp
+++ b/gtsam/slam/smallExample.cpp
@@ -211,6 +211,8 @@ namespace example {
return (h(x) - z_).vector();
}
+ ADD_CLONE_NONLINEAR_FACTOR(UnaryFactor)
+
};
}
diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h
index 29b77c661..306ec5191 100644
--- a/gtsam_unstable/dynamics/FullIMUFactor.h
+++ b/gtsam_unstable/dynamics/FullIMUFactor.h
@@ -51,6 +51,8 @@ public:
virtual ~FullIMUFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
const This* const f = dynamic_cast(&e);
diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h
index 7ded5ef82..b1ccdb3bc 100644
--- a/gtsam_unstable/dynamics/IMUFactor.h
+++ b/gtsam_unstable/dynamics/IMUFactor.h
@@ -44,6 +44,8 @@ public:
virtual ~IMUFactor() {}
+ ADD_CLONE_NONLINEAR_FACTOR(This)
+
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
const This* const f = dynamic_cast(&e);
diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h
index a35f09bb7..7646b7486 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint.h
@@ -72,6 +72,8 @@ public:
virtual ~VelocityConstraint() {}
+ ADD_CLONE_NONLINEAR_FACTOR(VelocityConstraint)
+
/**
* Calculates the error for trapezoidal model given
*/
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 1101a0a4b..43420e1c9 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -13,6 +13,7 @@ set(tests_local_libs
# note the source dir on each
set (tests_exclude
"${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp"
+ #"${CMAKE_CURRENT_SOURCE_DIR}/testOccupancyGrid.cpp"
)
# Build tests
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index d9e5643a4..c0e5af4f7 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -120,6 +120,8 @@ 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 {
@@ -262,6 +264,8 @@ 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 {
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index d32a50fe5..c74c3adab 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -254,6 +254,8 @@ public:
}
return (Vector(1) << x1 + x2 + x3 + x4).finished();
}
+
+ ADD_CLONE_NONLINEAR_FACTOR(TestFactor4)
};
/* ************************************ */
@@ -301,6 +303,8 @@ public:
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
}
+
+ ADD_CLONE_NONLINEAR_FACTOR(TestFactor5)
};
/* ************************************ */
@@ -353,6 +357,8 @@ public:
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
}
+
+ ADD_CLONE_NONLINEAR_FACTOR(TestFactor6)
};
/* ************************************ */
@@ -385,6 +391,20 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
}
+/* ************************************************************************* */
+TEST( NonlinearFactor, clone )
+{
+ shared_nlf init(new TestFactor4());
+ EXPECT_LONGS_EQUAL(PoseKey(1), init->keys()[0]);
+ EXPECT_LONGS_EQUAL(PoseKey(2), init->keys()[1]);
+ EXPECT_LONGS_EQUAL(PoseKey(3), init->keys()[2]);
+ EXPECT_LONGS_EQUAL(PoseKey(4), init->keys()[3]);
+
+ shared_nlf actClone = init->clone();
+ EXPECT(actClone.get() != init.get()); // Ensure different pointers
+ EXPECT(assert_equal(*init, *actClone));
+}
+
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */