diff --git a/.cproject b/.cproject index ce5ac0778..a596e90bf 100644 --- a/.cproject +++ b/.cproject @@ -600,7 +600,6 @@ make - tests/testBayesTree.run true false @@ -608,7 +607,6 @@ make - testBinaryBayesNet.run true false @@ -656,7 +654,6 @@ make - testSymbolicBayesNet.run true false @@ -664,7 +661,6 @@ make - tests/testSymbolicFactor.run true false @@ -672,7 +668,6 @@ make - testSymbolicFactorGraph.run true false @@ -688,7 +683,6 @@ make - tests/testBayesTree true false @@ -1120,7 +1114,6 @@ make - testErrors.run true false @@ -1350,46 +1343,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1472,6 +1425,7 @@ make + testSimulated2DOriented.run true false @@ -1511,6 +1465,7 @@ make + testSimulated2D.run true false @@ -1518,6 +1473,7 @@ make + testSimulated3D.run true false @@ -1531,6 +1487,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1788,7 +1784,6 @@ cpack - -G DEB true false @@ -1796,7 +1791,6 @@ cpack - -G RPM true false @@ -1804,7 +1798,6 @@ cpack - -G TGZ true false @@ -1812,7 +1805,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2441,6 +2433,14 @@ true true + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + make -j5 @@ -2579,7 +2579,6 @@ make - testGraph.run true false @@ -2587,7 +2586,6 @@ make - testJunctionTree.run true false @@ -2595,7 +2593,6 @@ make - testSymbolicBayesNetB.run true false @@ -3115,6 +3112,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o deleted file mode 100644 index 0e2eddee2..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o deleted file mode 100644 index ea1e61b09..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o deleted file mode 100644 index 213b1cc0c..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o and /dev/null differ diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index c09cc7577..b075d73b3 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -65,9 +65,10 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : - rowStart_(0), rowEnd_(height), blockStart_(0) - { + VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, + bool appendOneDimension = false) : + variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); @@ -75,26 +76,28 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) - { + VerticalBlockMatrix(const CONTAINER& dimensions, + const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : + matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); - if(variableColOffsets_.back() != matrix_.cols()) - throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); + if (variableColOffsets_.back() != matrix_.cols()) + throw std::invalid_argument( + "Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); assertInvariants(); } - /** - * Construct from iterator over the sizes of each vertical block. */ + /** Construct from iterator over the sizes of each vertical block. */ template - VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : - rowStart_(0), rowEnd_(height), blockStart_(0) - { + VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, + DenseIndex height, bool appendOneDimension = false) : + variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); } - + /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data. * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and @@ -203,18 +206,12 @@ namespace gtsam { template void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); variableColOffsets_[0] = 0; DenseIndex j=0; - for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j) variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; - ++ j; - } if(appendOneDimension) - { variableColOffsets_[j+1] = variableColOffsets_[j] + 1; - ++ j; - } } friend class SymmetricBlockMatrix; diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp index fad23fa7d..c504752aa 100644 --- a/gtsam/base/tests/testVerticalBlockMatrix.cpp +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -24,9 +24,20 @@ using namespace std; using namespace gtsam; using boost::assign::list_of; +list L = list_of(3)(2)(1); +vector dimensions(L.begin(),L.end()); + //***************************************************************************** -TEST(VerticalBlockMatrix, constructor) { - VerticalBlockMatrix actual(list_of(3)(2)(1), +TEST(VerticalBlockMatrix, Constructor1) { + VerticalBlockMatrix actual(dimensions,6); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + +//***************************************************************************** +TEST(VerticalBlockMatrix, Constructor2) { + VerticalBlockMatrix actual(dimensions, (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, // 2, 8, 9, 10, 11, 12, // 3, 9, 15, 16, 17, 18, // @@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) { EXPECT_LONGS_EQUAL(3,actual.nBlocks()); } +//***************************************************************************** +TEST(VerticalBlockMatrix, Constructor3) { + VerticalBlockMatrix actual(dimensions.begin(),dimensions.end(),6); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + //***************************************************************************** int main() { TestResult tr; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b90012822..d1dc7625c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -82,20 +82,22 @@ namespace gtsam { class GTSAM_EXPORT JacobianFactor : public GaussianFactor { public: + typedef JacobianFactor This; ///< Typedef to this class typedef GaussianFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - protected: - VerticalBlockMatrix Ab_; // the block view of the full matrix - noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - - public: typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::constBlock constABlock; typedef ABlock::ColXpr BVector; typedef constABlock::ConstColXpr constBVector; + protected: + + VerticalBlockMatrix Ab_; // the block view of the full matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix + + public: /** Convert from other GaussianFactor */ explicit JacobianFactor(const GaussianFactor& gf); @@ -328,6 +330,21 @@ namespace gtsam { private: + /** Unsafe Constructor that creates an uninitialized Jacobian of right size + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, + const SharedDiagonal& model = SharedDiagonal()) : + Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { + } + + // be very selective on who can access these private methods: + template friend class ExpressionFactor; + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index e74d07b29..1d5d0cb12 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -107,23 +107,23 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { - // This method has been heavily optimized for maximum performance. - // We allocate a VerticalBlockMatrix on the stack first, and then create - // a JacobianMap view onto it, which is then passed - // to [expression_.value] to allow it to write directly into Ab_. + // Check whether noise model is constrained or not + noiseModel::Constrained::shared_ptr constrained = // + boost::dynamic_pointer_cast(this->noiseModel_); - // Another malloc saved by creating a Matrix on the stack - double memory[Dim * augmentedCols_]; - Eigen::Map > // - matrix(memory, Dim, augmentedCols_); - matrix.setZero(); // zero out - - // Construct block matrix, is of right size but un-initialized - VerticalBlockMatrix Ab(dimensions_, matrix, true); + // Create a writeable JacobianFactor in advance + boost::shared_ptr factor( + constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained->unit()) : + new JacobianFactor(keys_, dimensions_, Dim)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ + VerticalBlockMatrix& Ab = factor->matrixObject(); JacobianMap map(keys_, Ab); + // Zero out Jacobian so we can simply add to it + Ab.matrix().setZero(); + // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! Ab(size()).col(0) = -measurement_.localCoordinates(value); @@ -131,15 +131,7 @@ public: // Whiten the corresponding system now // TODO ! this->noiseModel_->WhitenSystem(Ab); - // TODO pass unwhitened + noise model to Gaussian factor - // For now, only linearized constrained factors have noise model at linear level!!! - noiseModel::Constrained::shared_ptr constrained = // - boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained) { - return boost::make_shared(this->keys(), Ab, - constrained->unit()); - } else - return boost::make_shared(this->keys(), Ab); + return factor; } }; // ExpressionFactor diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b0900a43b..23c2fa1ce 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -28,6 +28,9 @@ #include +#include +using boost::assign::list_of; + using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index 04908f129..92522c440 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -27,7 +27,7 @@ using namespace std; using namespace gtsam; -#define time timeMultiThreaded +#define time timeSingleThreaded boost::shared_ptr fixedK(new Cal3_S2());