diff --git a/.cproject b/.cproject
index 790f70daa..21ab6965c 100644
--- a/.cproject
+++ b/.cproject
@@ -365,6 +365,38 @@
true
true
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNonlinearConstraint.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testLieConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testConstraintOptimizer.run
+ true
+ true
+ true
+
make
-j5
@@ -445,38 +477,6 @@
true
true
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- testNonlinearConstraint.run
- true
- true
- true
-
-
- make
- -j2
- testLieConfig.run
- true
- true
- true
-
-
- make
- -j2
- testConstraintOptimizer.run
- true
- true
- true
-
make
-j2
@@ -551,7 +551,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -559,7 +558,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -607,7 +605,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -615,7 +612,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -623,7 +619,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -639,12 +634,19 @@
make
-
tests/testBayesTree
true
false
true
+
+ make
+ -j2
+ testVSLAMGraph
+ true
+ true
+ true
+
make
-j2
@@ -727,6 +729,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -766,6 +769,7 @@
make
+
testSimulated2D.run
true
false
@@ -773,6 +777,7 @@
make
+
testSimulated3D.run
true
false
@@ -786,14 +791,6 @@
true
true
-
- make
- -j2
- testVSLAMGraph
- true
- true
- true
-
make
-j2
@@ -833,21 +830,6 @@
false
true
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- tests/testGaussianISAM2
- true
- false
- true
-
make
-j5
@@ -986,7 +968,6 @@
make
-
testGraph.run
true
false
@@ -994,7 +975,6 @@
make
-
testJunctionTree.run
true
false
@@ -1002,7 +982,6 @@
make
-
testSymbolicBayesNetB.run
true
false
@@ -1064,6 +1043,22 @@
true
true
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+
+ tests/testGaussianISAM2
+ true
+ false
+ true
+
make
-j2
@@ -1424,6 +1419,22 @@
true
true
+
+ make
+ -j5
+ testEssentialMatrixFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -1504,10 +1515,66 @@
true
true
-
+
make
-j2
- testGaussianFactor.run
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testClusterTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testJunctionTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testEliminationTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testSymbolicFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testVariableSlots.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testConditional.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testSymbolicFactorGraph.run
true
true
true
@@ -1608,86 +1675,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- testClusterTree.run
- true
- true
- true
-
-
- make
- -j2
- testJunctionTree.run
- true
- true
- true
-
-
- make
- -j2
- tests/testEliminationTree.run
- true
- true
- true
-
-
- make
- -j2
- tests/testSymbolicFactor.run
- true
- true
- true
-
-
- make
- -j2
- tests/testVariableSlots.run
- true
- true
- true
-
-
- make
- -j2
- tests/testConditional.run
- true
- true
- true
-
-
- make
- -j2
- tests/testSymbolicFactorGraph.run
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
make
-j5
@@ -1752,6 +1739,22 @@
true
true
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
make
-j2
@@ -2185,7 +2188,6 @@
cpack
-
-G DEB
true
false
@@ -2193,7 +2195,6 @@
cpack
-
-G RPM
true
false
@@ -2201,7 +2202,6 @@
cpack
-
-G TGZ
true
false
@@ -2209,7 +2209,6 @@
cpack
-
--config CPackSourceConfig.cmake
true
false
@@ -2383,30 +2382,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- tests/testSPQRUtil.run
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
make
-j5
@@ -2447,18 +2422,26 @@
true
true
-
+
make
-j2
- tests/testPose2.run
+ check
true
true
true
-
+
make
-j2
- tests/testPose3.run
+ tests/testSPQRUtil.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
true
true
true
@@ -2553,12 +2536,27 @@
make
-
testErrors.run
true
false
true
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index d3f560b32..7ac992d79 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -22,6 +22,7 @@ class EssentialMatrixFactor: public NoiseModelFactor1 {
Vector vA_, vB_; ///< Homogeneous versions
typedef NoiseModelFactor1 Base;
+ typedef EssentialMatrixFactor This;
public:
@@ -33,6 +34,11 @@ public:
vB_(EssentialMatrix::Homogeneous(pB)) {
}
+ /// @return a deep copy of this factor
+ virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+ return boost::static_pointer_cast(
+ gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
index 34a26adbe..e7cbb0ada 100644
--- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp
@@ -6,6 +6,53 @@
*/
#include
+#include
+
+namespace gtsam {
+
+/**
+ * Binary factor that optimizes for E and inverse depth: assumes measurement
+ * in image 1 is perfect, and returns re-projection error in image 2
+ */
+class EssentialMatrixFactor2: public NoiseModelFactor2 {
+
+ Point2 pA_, pB_; ///< Measurements in image A and B
+
+ typedef NoiseModelFactor2 Base;
+
+public:
+
+ /// Constructor
+ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
+ const SharedNoiseModel& model) :
+ Base(model, key1, key2), pA_(pA), pB_(pB) {
+ }
+
+ /// print
+ virtual void print(const std::string& s = "",
+ const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+ Base::print(s);
+ std::cout << " EssentialMatrixFactor2 with measurements\n ("
+ << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
+ << ")'" << std::endl;
+ }
+
+ /// vector of errors returns 1D vector
+ Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
+ boost::optional H1 = boost::none, boost::optional H2 =
+ boost::none) const {
+ if (H1)
+ *H1 = zeros(2, 5);
+ if (H2)
+ *H2 = zeros(2, 1);
+ return (Vector(2) << 0,0);
+ }
+
+};
+
+} // gtsam
+
#include
#include
#include
@@ -22,7 +69,7 @@ const string filename = findExampleDataFile("5pointExample1.txt");
SfM_data data;
bool readOK = readBAL(filename, data);
Rot3 aRb = data.cameras[1].pose().rotation();
-Point3 aTb = data.cameras[1].pose().translation();
+Point3 aTb = data.cameras[1].pose().translation();
Point2 pA(size_t i) {
return data.tracks[i].measurements[0].second;
@@ -77,18 +124,18 @@ TEST (EssentialMatrixFactor, factor) {
// Check evaluation
Vector expected(1);
expected << 0;
- Matrix HActual;
- Vector actual = factor.evaluateError(trueE, HActual);
+ Matrix Hactual;
+ Vector actual = factor.evaluateError(trueE, Hactual);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
- Matrix HExpected;
- HExpected = numericalDerivative11(
+ Matrix Hexpected;
+ Hexpected = numericalDerivative11(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE);
// Verify the Jacobian is correct
- EXPECT(assert_equal(HExpected, HActual, 1e-8));
+ EXPECT(assert_equal(Hexpected, Hactual, 1e-8));
}
}
@@ -138,6 +185,36 @@ TEST (EssentialMatrixFactor, fromConstraints) {
}
+//*************************************************************************
+TEST (EssentialMatrixFactor2, factor) {
+ EssentialMatrix trueE(aRb, aTb);
+ LieScalar d(5);
+ noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
+
+ for (size_t i = 0; i < 5; i++) {
+ EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model);
+
+ // Check evaluation
+ Vector expected(1);
+ expected << 0;
+ Matrix Hactual1, Hactual2;
+ Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
+ EXPECT(assert_equal(expected, actual, 1e-7));
+
+ // Use numerical derivatives to calculate the expected Jacobian
+ Matrix Hexpected1, Hexpected2;
+ boost::function f =
+ boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
+ boost::none, boost::none);
+ Hexpected1 = numericalDerivative21(f, trueE, d);
+ Hexpected2 = numericalDerivative22(f, trueE, d);
+
+ // Verify the Jacobian is correct
+ EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
+ EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
+ }
+}
+
/* ************************************************************************* */
int main() {
TestResult tr;