diff --git a/.cproject b/.cproject index 84b258330..4f449ae92 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -54,13 +52,13 @@ - - + + @@ -99,13 +97,13 @@ - - + + @@ -293,30 +291,6 @@ - - make - -j5 - SmartProjectionFactorExample_kitti_nonbatch.run - true - true - true - - - make - -j5 - SmartProjectionFactorExample_kitti.run - true - true - true - - - make - -j5 - SmartProjectionFactorTesting.run - true - true - true - make -j2 @@ -863,6 +837,14 @@ true true + + make + -j5 + testSmartProjectionHessianFactor.run + true + true + true + make -j5 @@ -1643,46 +1625,6 @@ true true - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - make -j2 @@ -2027,10 +1969,10 @@ true true - + make -j5 - SFMExample.run + VisualSLAMExample.run true true true @@ -2067,14 +2009,6 @@ true true - - make - -j5 - SFMExample_bal.run - true - true - true - make -j4 diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 23634fb12..af991231e 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -86,8 +86,8 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, boost::optional(1)) + info_(cref_list_of<1>(1)) { linearTerm().setZero(); constantTerm() = 0.0; @@ -110,10 +110,10 @@ HessianFactor::HessianFactor() : /* ************************************************************************* */ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : - GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) { if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); info_(0,0) = G; info_(0,1) = g; info_(1,1)(0,0) = f; @@ -123,11 +123,11 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : - GaussianFactor(cref_list_of<1>(j)), - info_(cref_list_of<2> (Sigma.cols()) (1) ) + GaussianFactor(cref_list_of<1>(j)), + info_(cref_list_of<2> (Sigma.cols()) (1) ) { if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); info_(0,0) = Sigma.inverse(); // G info_(0,1) = info_(0,0) * mu; // g info_(1,1)(0,0) = mu.dot(info_(0,1).col(0)); // f @@ -135,10 +135,10 @@ HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : /* ************************************************************************* */ HessianFactor::HessianFactor(Key j1, Key j2, - const Matrix& G11, const Matrix& G12, const Vector& g1, - const Matrix& G22, const Vector& g2, double f) : -GaussianFactor(cref_list_of<2>(j1)(j2)), - info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) + const Matrix& G11, const Matrix& G12, const Vector& g1, + const Matrix& G22, const Vector& g2, double f) : + GaussianFactor(cref_list_of<2>(j1)(j2)), + info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) { info_(0,0) = G11; info_(0,1) = G12; @@ -150,14 +150,14 @@ GaussianFactor(cref_list_of<2>(j1)(j2)), /* ************************************************************************* */ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, - const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, - const Matrix& G22, const Matrix& G23, const Vector& g2, - const Matrix& G33, const Vector& g3, double f) : -GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), - info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) + const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, + const Matrix& G22, const Matrix& G23, const Vector& g2, + const Matrix& G33, const Vector& g3, double f) : + GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), + info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) { if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || - G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) + G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); info_(0,0) = G11; info_(0,1) = G12; @@ -173,13 +173,13 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), /* ************************************************************************* */ namespace { - DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +DenseIndex _getSizeHF(const Vector& m) { return m.size(); } } /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f) : -GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1))) + const std::vector& gs, double f) : + GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1))) { // Get the number of variables size_t variable_count = js.size(); @@ -222,33 +222,33 @@ GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne( /* ************************************************************************* */ namespace { - void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) +void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) +{ + gttic(HessianFactor_fromJacobian); + const SharedDiagonal& jfModel = jf.get_model(); + if(jfModel) { - gttic(HessianFactor_fromJacobian); - const SharedDiagonal& jfModel = jf.get_model(); - if(jfModel) - { - if(jf.get_model()->isConstrained()) - throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - info.full().noalias() = jf.matrixObject().full().transpose() * + if(jf.get_model()->isConstrained()) + throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); + info.full().noalias() = jf.matrixObject().full().transpose() * (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * jf.matrixObject().full(); - } else { - info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); - } + } else { + info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); } } +} /* ************************************************************************* */ HessianFactor::HessianFactor(const JacobianFactor& jf) : - GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) + GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) { _FromJacobianHelper(jf, info_); } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactor& gf) : - GaussianFactor(gf) + GaussianFactor(gf) { // Copy the matrix data depending on what type of factor we're copying from if(const JacobianFactor* jf = dynamic_cast(&gf)) @@ -268,12 +268,12 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : /* ************************************************************************* */ namespace { - DenseIndex _dimFromScatterEntry(const Scatter::value_type& key_slotentry) { - return key_slotentry.second.dimension; } } +DenseIndex _dimFromScatterEntry(const Scatter::value_type& key_slotentry) { + return key_slotentry.second.dimension; } } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter) + boost::optional scatter) { boost::optional computedScatter; if(!scatter) { @@ -413,7 +413,7 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt // This function updates 'combined' with the information in 'update'. // 'scatter' maps variables in the update factor to slots in the combined // factor. - + gttic(updateATA); if(update.rows() > 0) @@ -478,7 +478,7 @@ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFr VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(info_, varDim); Ab.full() = info_.range(0, nrFrontals, 0, info_.nBlocks()); GaussianConditional::shared_ptr conditional = boost::make_shared( - keys_, nrFrontals, Ab); + keys_, nrFrontals, Ab); gttoc(Construct_conditional); gttic(Remaining_factor); @@ -503,11 +503,25 @@ GaussianFactor::shared_ptr HessianFactor::negate() const void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) { + for(size_t posRow=0; posRow yi = y.tryInsert(keys_[posRow], Vector()); + if(yi.second) + yi.first->second = Vector::Zero(getDim(begin() + posRow)); + + for(size_t posCol=0; posColsecond); + } + } } /* ************************************************************************* */ std::pair, boost::shared_ptr > - EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) +EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateCholesky); @@ -517,8 +531,8 @@ std::pair, boost::shared_ptr(factors, Scatter(factors, keys)); } catch(std::invalid_argument&) { throw InvalidDenseElimination( - "EliminateCholesky was called with a request to eliminate variables that are not\n" - "involved in the provided factors."); + "EliminateCholesky was called with a request to eliminate variables that are not\n" + "involved in the provided factors."); } // Do dense elimination @@ -533,7 +547,7 @@ std::pair, boost::shared_ptr, boost::shared_ptr > - EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) +EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminatePreferCholesky); diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 583df240a..79331611e 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -158,6 +158,15 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { return fg; } + +/* ************************************************************************* */ +static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { + GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); + fg += HessianFactor(1, 2, 100*ones(2,2), 200*ones(2,2), (Vec(2) << 0.0, 1.0), + 400*ones(2,2), (Vec(2) << 1.0, 1.0), 0.0); + return fg; +} + /* ************************************************************************* */ TEST( GaussianFactorGraph, gradient ) { @@ -242,6 +251,26 @@ TEST( GaussianFactorGraph, multiplyHessian ) EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +//TEST( GaussianFactorGraph, multiplyHessian2 ) +//{ +// GaussianFactorGraph A = createGaussianFactorGraphWithHessianFactor(); +// +// VectorValues x = map_list_of +// (0, (Vec(2) << 1,2)) +// (1, (Vec(2) << 3,4)) +// (2, (Vec(2) << 5,6)); +// +// // expected from matlab: -450 -450 2900 2900 6750 6850 +// VectorValues expected; +// expected.insert(0, (Vec(2) << -450, -450)); +// expected.insert(1, (Vec(2) << 2900, 2900)); +// expected.insert(2, (Vec(2) << 6750, 6850)); +// +// VectorValues actual = A.multiplyHessian(x); +// EXPECT(assert_equal(expected, actual)); +//} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */