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/Matrix.cpp b/gtsam/base/Matrix.cpp index f51197cf2..dd17c00ef 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...) void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { const DenseIndex m = A.rows(); if (inf_mask) { - // only scale the first v.size() rows of A to support augmented Matrix - for (DenseIndex i=0; i - 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/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index faf5d4bbb..7c4ac34e2 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -243,8 +243,8 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { if (Dpose) { - const Matrix R = R_.matrix(); - Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + const Matrix3 R = R_.matrix(); + Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Dpose->resize(3, 6); (*Dpose) << DR, R; } @@ -263,7 +263,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case - const Matrix3& Rt = R_.transpose(); + const Matrix3 Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); @@ -280,7 +280,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { - const Matrix3& Rt = R_.transpose(); + const Matrix3 Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); 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/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index fcaec3afa..3b9c5d79c 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -339,12 +339,12 @@ Vector Constrained::whiten(const Vector& v) const { // a hard constraint, we don't do anything. const Vector& a = v; const Vector& b = sigmas_; - // Now allows for whiten augmented vector with a new additional part coming - // from the Lagrange multiplier. So a.size() >= b.size() - Vector c = a; - for( DenseIndex i = 0; i < b.size(); i++ ) { + size_t n = a.size(); + assert (b.size()==a.size()); + Vector c(n); + for( size_t i = 0; i < n; i++ ) { const double& ai = a(i), &bi = b(i); - if (bi!=0) c(i) = ai/bi; + c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_() } return c; } @@ -362,11 +362,7 @@ double Constrained::distance(const Vector& v) const { /* ************************************************************************* */ Matrix Constrained::Whiten(const Matrix& H) const { // selective scaling - // Now allow augmented Matrix with a new additional part coming - // from the Lagrange multiplier. - Matrix M(H.block(0, 0, dim(), H.cols())); - Constrained::WhitenInPlace(M); - return M; + return vector_scale(invsigmas(), H, true); } /* ************************************************************************* */ @@ -375,8 +371,6 @@ void Constrained::WhitenInPlace(Matrix& H) const { // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas) // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0, // indicating a hard constraint, we leave H's row i in place. - // Now allow augmented Matrix with a new additional part coming - // from the Lagrange multiplier. // Inlined: vector_scale_inplace(invsigmas(), H, true); // vector_scale_inplace(v, A, true); for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) { @@ -399,14 +393,12 @@ void Constrained::WhitenInPlace(Eigen::Block H) const { } /* ************************************************************************* */ -Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const { - Vector sigmas = ones(dim()+augmentedDim); +Constrained::shared_ptr Constrained::unit() const { + Vector sigmas = ones(dim()); for (size_t i=0; isigmas_(i) == 0.0) sigmas(i) = 0.0; - Vector augmentedMu = zero(dim()+augmentedDim); - subInsert(augmentedMu, mu_, 0); - return MixedSigmas(augmentedMu, sigmas); + return MixedSigmas(mu_, sigmas); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 4b0e4848d..caad6b01a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -61,6 +61,11 @@ namespace gtsam { Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} + /** true if a constrained noise mode, saves slow/clumsy dynamic casting */ + virtual bool is_constrained() const { + return false; + } + /// Dimensionality inline size_t dim() const { return dim_;} @@ -385,6 +390,11 @@ namespace gtsam { virtual ~Constrained() {} + /** true if a constrained noise mode, saves slow/clumsy dynamic casting */ + virtual bool is_constrained() const { + return true; + } + /// Access mu as a vector const Vector& mu() const { return mu_; } @@ -481,9 +491,8 @@ namespace gtsam { /** * Returns a Unit version of a constrained noisemodel in which * constrained sigmas remain constrained and the rest are unit scaled - * Now support augmented part from the Lagrange multiplier. */ - shared_ptr unit(size_t augmentedDim = 0) const; + shared_ptr unit() const; private: /** Serialization function */ @@ -732,7 +741,7 @@ namespace gtsam { }; /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by: - /// Dipl.-Inform. Jan Oberl�nder (M.Sc.), FZI Research Center for + /// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for /// Information Technology, Karlsruhe, Germany. /// oberlaender@fzi.de /// Thanks Jan! diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 08b131152..f6f43b062 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -22,20 +22,21 @@ namespace gtsam { /* ************************************************************************* */ - void NonlinearFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s << " keys = { "; - BOOST_FOREACH(Key key, this->keys()) { + BOOST_FOREACH(Key key, keys()) { std::cout << keyFormatter(key) << " "; } std::cout << "}" << std::endl; } +/* ************************************************************************* */ bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const { return Base::equals(f); } +/* ************************************************************************* */ NonlinearFactor::shared_ptr NonlinearFactor::rekey( const std::map& rekey_mapping) const { shared_ptr new_factor = clone(); @@ -48,22 +49,23 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey( return new_factor; } +/* ************************************************************************* */ NonlinearFactor::shared_ptr NonlinearFactor::rekey( const std::vector& new_keys) const { - assert(new_keys.size() == this->keys().size()); + assert(new_keys.size() == keys().size()); shared_ptr new_factor = clone(); new_factor->keys() = new_keys; return new_factor; } /* ************************************************************************* */ - void NoiseModelFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { Base::print(s, keyFormatter); - this->noiseModel_->print(" noise model: "); + noiseModel_->print(" noise model: "); } +/* ************************************************************************* */ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { const NoiseModelFactor* e = dynamic_cast(&f); return e && Base::equals(f, tol) @@ -72,6 +74,7 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { && noiseModel_->equals(*e->noiseModel_, tol))); } +/* ************************************************************************* */ static void check(const SharedNoiseModel& noiseModel, size_t m) { if (!noiseModel) throw std::invalid_argument("NoiseModelFactor: no NoiseModel."); @@ -80,14 +83,16 @@ static void check(const SharedNoiseModel& noiseModel, size_t m) { "NoiseModelFactor was created with a NoiseModel of incorrect dimension."); } +/* ************************************************************************* */ Vector NoiseModelFactor::whitenedError(const Values& c) const { const Vector b = unwhitenedError(c); check(noiseModel_, b.size()); return noiseModel_->whiten(b); } +/* ************************************************************************* */ double NoiseModelFactor::error(const Values& c) const { - if (this->active(c)) { + if (active(c)) { const Vector b = unwhitenedError(c); check(noiseModel_, b.size()); return 0.5 * noiseModel_->distance(b); @@ -96,41 +101,36 @@ double NoiseModelFactor::error(const Values& c) const { } } +/* ************************************************************************* */ boost::shared_ptr NoiseModelFactor::linearize( const Values& x) const { // Only linearize if the factor is active - if (!this->active(x)) + if (!active(x)) return boost::shared_ptr(); // Call evaluate error to get Jacobians and RHS vector b - std::vector A(this->size()); + std::vector A(size()); Vector b = -unwhitenedError(x, A); check(noiseModel_, b.size()); // Whiten the corresponding system now - this->noiseModel_->WhitenSystem(A, b); + noiseModel_->WhitenSystem(A, b); // Fill in terms, needed to create JacobianFactor below - std::vector > terms(this->size()); - for (size_t j = 0; j < this->size(); ++j) { - terms[j].first = this->keys()[j]; + std::vector > terms(size()); + for (size_t j = 0; j < size(); ++j) { + terms[j].first = keys()[j]; terms[j].second.swap(A[j]); } // 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) { - // Create a factor of reduced row dimension d_ - size_t d_ = b.size() - constrained->dim(); - Vector zero_ = zero(d_); - Vector b_ = concatVectors(2, &b, &zero_); - noiseModel::Constrained::shared_ptr model = constrained->unit(d_); - return boost::make_shared(terms, b_, model); - } else - return boost::make_shared(terms, b); + if (noiseModel_->is_constrained()) + return GaussianFactor::shared_ptr( + new JacobianFactor(terms, b, + boost::static_pointer_cast(noiseModel_)->unit())); + else + return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 5919d1464..215525bb9 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -23,9 +23,11 @@ #include #include #include +#include #include #include +#include // template meta-programming headers #include @@ -48,7 +50,25 @@ namespace gtsam { template class Expression; -typedef std::map > JacobianMap; +/** + * Expressions are designed to write their derivatives into an already allocated + * Jacobian of the correct size, of type VerticalBlockMatrix. + * The JacobianMap provides a mapping from keys to the underlying blocks. + */ +class JacobianMap { + const FastVector& keys_; + VerticalBlockMatrix& Ab_; +public: + JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + /// Access via key + VerticalBlockMatrix::Block operator()(Key key) { + FastVector::const_iterator it = std::find(keys_.begin(),keys_.end(),key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); + } +}; //----------------------------------------------------------------------------- /** @@ -78,16 +98,14 @@ struct CallRecord { template void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { - JacobianMap::iterator it = jacobians.find(key); - it->second.block(0, 0) += dTdA; // block makes HUGE difference + jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference } /// Handle Leaf Case for Dynamic Matrix type (slower) template<> void handleLeafCase( const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { - JacobianMap::iterator it = jacobians.find(key); - it->second += dTdA; + jacobians(key) += dTdA; } //----------------------------------------------------------------------------- diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 6e97dd583..fc1efeb87 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -123,7 +123,7 @@ public: } /// Return value and derivatives, reverse AD version - T reverse(const Values& values, JacobianMap& jacobians) const { + T value(const Values& values, JacobianMap& jacobians) const { // The following piece of code is absolutely crucial for performance. // We allocate a block of memory on the stack, which can be done at runtime // with modern C++ compilers. The traceExecution then fills this memory @@ -142,11 +142,6 @@ public: return root_->value(values); } - /// Return value and derivatives - T value(const Values& values, JacobianMap& jacobians) const { - return reverse(values, jacobians); - } - const boost::shared_ptr >& root() const { return root_; } diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 84456c934..37a89af6b 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -75,7 +75,7 @@ public: } /** - * Error function *without* the NoiseModel, \f$ z-h(x) \f$. + * Error function *without* the NoiseModel, \f$ h(x)-z \f$. * We override this method to provide * both the function evaluation and its derivative(s) in H. */ @@ -85,17 +85,19 @@ public: // H should be pre-allocated assert(H->size()==size()); - // Create and zero out blocks to be passed to expression_ - JacobianMap blocks; - for (DenseIndex i = 0; i < size(); i++) { - Matrix& Hi = H->at(i); - Hi.resize(Dim, dimensions_[i]); - Hi.setZero(); // zero out - Eigen::Block block = Hi.block(0, 0, Dim, dimensions_[i]); - blocks.insert(std::make_pair(keys_[i], block)); - } + VerticalBlockMatrix Ab(dimensions_, Dim); + + // Wrap keys and VerticalBlockMatrix into structure passed to expression_ + JacobianMap map(keys_, Ab); + Ab.matrix().setZero(); + + // Evaluate error to get Jacobians and RHS vector b + T value = expression_.value(x, map); // <<< Reverse AD happens here ! + + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < size(); i++) + H->at(i) = Ab(i); - T value = expression_.value(x, blocks); return measurement_.localCoordinates(value); } else { const T& value = expression_.value(x); @@ -105,41 +107,34 @@ 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 - // Eigen::Block views on this piece of memory which is then passed - // to [expression_.value] below, which writes directly into Ab_. + // Only linearize if the factor is active + if (!active(x)) + return boost::shared_ptr(); - // Another malloc saved by creating a Matrix on the stack - double memory[Dim * augmentedCols_]; - Eigen::Map > // - matrix(memory, Dim, augmentedCols_); - matrix.setZero(); // zero out + // Create a writeable JacobianFactor in advance + // In case noise model is constrained, we need to provide a noise model + bool constrained = noiseModel_->is_constrained(); + boost::shared_ptr factor( + constrained ? new JacobianFactor(keys_, dimensions_, Dim, + boost::static_pointer_cast(noiseModel_)->unit()) : + new JacobianFactor(keys_, dimensions_, Dim)); - // Construct block matrix, is of right size but un-initialized - VerticalBlockMatrix Ab(dimensions_, matrix, true); + // Wrap keys and VerticalBlockMatrix into structure passed to expression_ + VerticalBlockMatrix& Ab = factor->matrixObject(); + JacobianMap map(keys_, Ab); - // Create blocks into Ab_ to be passed to expression_ - JacobianMap blocks; - for (DenseIndex i = 0; i < size(); i++) - blocks.insert(std::make_pair(keys_[i], Ab(i))); + // 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, blocks); // <<< Reverse AD happens here ! + T value = expression_.value(x, map); // <<< Reverse AD happens here ! Ab(size()).col(0) = -measurement_.localCoordinates(value); - // Whiten the corresponding system now - // TODO ! this->noiseModel_->WhitenSystem(Ab); + // Whiten the corresponding system, Ab already contains RHS + Vector dummy(Dim); + noiseModel_->WhitenSystem(Ab.matrix(),dummy); - // 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/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 1997bdb53..d660d28b5 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -46,11 +46,8 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); TEST(Expression, constant) { Expression R(someR); Values values; - JacobianMap actualMap; - Rot3 actual = R.value(values, actualMap); + Rot3 actual = R.value(values); EXPECT(assert_equal(someR, actual)); - JacobianMap expected; - EXPECT(actualMap == expected); EXPECT_LONGS_EQUAL(0, R.traceSize()) } @@ -61,15 +58,8 @@ TEST(Expression, Leaf) { Values values; values.insert(100, someR); - JacobianMap expected; - Matrix H = eye(3); - expected.insert(make_pair(100, H.block(0, 0, 3, 3))); - - JacobianMap actualMap2; - actualMap2.insert(make_pair(100, H.block(0, 0, 3, 3))); - Rot3 actual2 = R.reverse(values, actualMap2); + Rot3 actual2 = R.value(values); EXPECT(assert_equal(someR, actual2)); - EXPECT(actualMap2 == expected); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index a320ebc5a..ce8d6ac06 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; @@ -62,41 +65,41 @@ TEST(ExpressionFactor, Leaf) { EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); } -///* ************************************************************************* */ -//// non-zero noise model -//TEST(ExpressionFactor, Model) { -// using namespace leaf; -// -// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); -// -// // Create old-style factor to create expected value and derivatives -// PriorFactor old(2, Point2(0, 0), model); -// -// // Concise version -// ExpressionFactor f(model, Point2(0, 0), p); -// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); -// EXPECT_LONGS_EQUAL(2, f.dim()); -// boost::shared_ptr gf2 = f.linearize(values); -// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); -//} -// -///* ************************************************************************* */ -//// Constrained noise model -//TEST(ExpressionFactor, Constrained) { -// using namespace leaf; -// -// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); -// -// // Create old-style factor to create expected value and derivatives -// PriorFactor old(2, Point2(0, 0), model); -// -// // Concise version -// ExpressionFactor f(model, Point2(0, 0), p); -// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); -// EXPECT_LONGS_EQUAL(2, f.dim()); -// boost::shared_ptr gf2 = f.linearize(values); -// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); -//} +/* ************************************************************************* */ +// non-zero noise model +TEST(ExpressionFactor, Model) { + using namespace leaf; + + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); + + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); + + // Concise version + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +} + +/* ************************************************************************* */ +// Constrained noise model +TEST(ExpressionFactor, Constrained) { + using namespace leaf; + + SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); + + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); + + // Concise version + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +} /* ************************************************************************* */ // Unary(Leaf)) 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()); diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/gtsam_unstable/timing/timeOneCameraExpression.cpp index 67b83b78b..4cb655825 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/gtsam_unstable/timing/timeOneCameraExpression.cpp @@ -23,7 +23,7 @@ using namespace std; using namespace gtsam; -#define time timeMultiThreaded +#define time timeSingleThreaded int main() {