diff --git a/.cproject b/.cproject
index 8e20710ec..fa8c24875 100644
--- a/.cproject
+++ b/.cproject
@@ -540,6 +540,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -566,7 +574,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -574,7 +581,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -622,7 +628,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -630,7 +635,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -638,7 +642,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -654,20 +657,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j5
@@ -892,22 +886,6 @@
false
true
-
- make
- -j2
- tests/testPose2.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose3.run
- true
- true
- true
-
make
-j2
@@ -924,6 +902,22 @@
true
true
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
make
-j2
@@ -1260,6 +1254,14 @@
true
true
+
+ make
+ -j5
+ testAttitudeFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -1534,7 +1536,6 @@
make
-
testGraph.run
true
false
@@ -1542,7 +1543,6 @@
make
-
testJunctionTree.run
true
false
@@ -1550,7 +1550,6 @@
make
-
testSymbolicBayesNetB.run
true
false
@@ -1718,7 +1717,6 @@
make
-
testErrors.run
true
false
@@ -1764,6 +1762,22 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testParticleFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -1844,22 +1858,6 @@
true
true
-
- make
- -j5
- testParticleFactor.run
- true
- true
- true
-
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -2012,6 +2010,22 @@
true
true
+
+ make
+ -j5
+ testImuFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testCombinedImuFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -2094,6 +2108,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -2133,6 +2148,7 @@
make
+
testSimulated2D.run
true
false
@@ -2140,6 +2156,7 @@
make
+
testSimulated3D.run
true
false
@@ -2153,22 +2170,6 @@
true
true
-
- make
- -j5
- testImuFactor.run
- true
- true
- true
-
-
- make
- -j5
- testCombinedImuFactor.run
- true
- true
- true
-
make
-j5
@@ -2395,6 +2396,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -2416,6 +2418,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
-j3
@@ -2617,7 +2715,6 @@
cpack
-
-G DEB
true
false
@@ -2625,7 +2722,6 @@
cpack
-
-G RPM
true
false
@@ -2633,7 +2729,6 @@
cpack
-
-G TGZ
true
false
@@ -2641,7 +2736,6 @@
cpack
-
--config CPackSourceConfig.cmake
true
false
@@ -2815,98 +2909,34 @@
true
true
-
+
make
- -j2
- testRot3.run
+ -j5
+ testSpirit.run
true
true
true
-
+
make
- -j2
- testRot2.run
+ -j5
+ testWrap.run
true
true
true
-
+
make
- -j2
- testPose3.run
+ -j5
+ check.wrap
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
+ -j5
+ wrap
true
true
true
@@ -2950,38 +2980,6 @@
false
true
-
- make
- -j5
- testSpirit.run
- true
- true
- true
-
-
- make
- -j5
- testWrap.run
- true
- true
- true
-
-
- make
- -j5
- check.wrap
- true
- true
- true
-
-
- make
- -j5
- wrap
- true
- true
- true
-
diff --git a/gtsam_unstable/slam/JacobianFactorQ.h b/gtsam_unstable/slam/JacobianFactorQ.h
index fdbe0e231..f4dfb9422 100644
--- a/gtsam_unstable/slam/JacobianFactorQ.h
+++ b/gtsam_unstable/slam/JacobianFactorQ.h
@@ -23,6 +23,19 @@ public:
JacobianFactorQ() {
}
+ /// Empty constructor with keys
+ JacobianFactorQ(const FastVector& keys,
+ const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() {
+ Matrix zeroMatrix = Matrix::Zero(0,D);
+ Vector zeroVector = Vector::Zero(0);
+ typedef std::pair KeyMatrix;
+ std::vector QF;
+ QF.reserve(keys.size());
+ BOOST_FOREACH(const Key& key, keys)
+ QF.push_back(KeyMatrix(key, zeroMatrix));
+ JacobianFactor::fillTerms(QF, zeroVector, model);
+ }
+
/// Constructor
JacobianFactorQ(const std::vector& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam_unstable/slam/JacobianFactorSVD.h
index 752a9f48e..e8ade3b1b 100644
--- a/gtsam_unstable/slam/JacobianFactorSVD.h
+++ b/gtsam_unstable/slam/JacobianFactorSVD.h
@@ -18,10 +18,23 @@ public:
typedef Eigen::Matrix Matrix2D;
typedef std::pair KeyMatrix2D;
+ typedef std::pair KeyMatrix;
/// Default constructor
JacobianFactorSVD() {}
+ /// Empty constructor with keys
+ JacobianFactorSVD(const FastVector& keys,
+ const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() {
+ Matrix zeroMatrix = Matrix::Zero(0,D);
+ Vector zeroVector = Vector::Zero(0);
+ std::vector QF;
+ QF.reserve(keys.size());
+ BOOST_FOREACH(const Key& key, keys)
+ QF.push_back(KeyMatrix(key, zeroMatrix));
+ JacobianFactor::fillTerms(QF, zeroVector, model);
+ }
+
/// Constructor
JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() {
@@ -32,7 +45,6 @@ public:
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// JacobianFactor factor(QF, Q * b);
- typedef std::pair KeyMatrix;
std::vector QF;
QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h
index 33730db8a..2124dd6f6 100644
--- a/gtsam_unstable/slam/SmartProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartProjectionFactor.h
@@ -424,7 +424,7 @@ public:
if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda);
else
- return boost::shared_ptr >();
+ return boost::make_shared< JacobianFactorQ >(this->keys_);
}
/// Create a factor, takes values
@@ -436,16 +436,16 @@ public:
if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda);
else
- return boost::shared_ptr >();
+ return boost::make_shared< JacobianFactorQ >(this->keys_);
}
/// different (faster) way to compute Jacobian factor
- boost::shared_ptr createJacobianSVDFactor(const Cameras& cameras,
+ boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
double lambda) const {
if (triangulateForLinearize(cameras))
- return Base::createJacobianQFactor(cameras, point_, lambda);
+ return Base::createJacobianSVDFactor(cameras, point_, lambda);
else
- return boost::shared_ptr >();
+ return boost::make_shared< JacobianFactorSVD >(this->keys_);
}
/// Returns true if nonDegenerate