diff --git a/.cproject b/.cproject index eb83cb85b..706f18e26 100644 --- a/.cproject +++ b/.cproject @@ -317,6 +317,7 @@ make + clean true true @@ -476,7 +477,6 @@ make - testBayesTree.run true false @@ -484,6 +484,7 @@ make + testSymbolicBayesNet.run true false @@ -491,7 +492,6 @@ make - testSymbolicFactorGraph.run true false @@ -683,7 +683,6 @@ make - testGraph.run true false @@ -739,6 +738,7 @@ make + testSimulated2D.run true false @@ -786,7 +786,6 @@ make - testErrors.run true false @@ -794,7 +793,6 @@ make - testDSF.run true true @@ -810,12 +808,19 @@ make - testConstraintOptimizer.run true true true + +make + +testBTree.run +true +true +true + make -j2 diff --git a/cpp/BTree.h b/cpp/BTree.h index 24f45a1af..13ede8783 100644 --- a/cpp/BTree.h +++ b/cpp/BTree.h @@ -41,15 +41,15 @@ namespace gtsam { * leaf node with height 1 */ Node(const value_type& keyValue) : - keyValue_(keyValue), height_(1) { + height_(1), keyValue_(keyValue) { } /** * Create a node from two subtrees and a key value pair */ Node(const BTree& l, const value_type& keyValue, const BTree& r) : - left(l), keyValue_(keyValue), right(r), - height_(l.height() >= r.height() ? l.height() + 1 : r.height() + 1) { + height_(l.height() >= r.height() ? l.height() + 1 : r.height() + 1), + keyValue_(keyValue), left(l), right(r) { } inline const Key& key() const { return keyValue_.first;} diff --git a/cpp/BinaryConditional.h b/cpp/BinaryConditional.h index 2aceb7ea3..20a64643f 100644 --- a/cpp/BinaryConditional.h +++ b/cpp/BinaryConditional.h @@ -56,7 +56,7 @@ namespace gtsam { BinaryConditional(const Symbol& key, const Symbol& parent, const std::vector& cpt) : Conditional(key) { parents_.push_back(parent); - for( int i = 0 ; i < cpt.size() ; i++ ) + for( size_t i = 0 ; i < cpt.size() ; i++ ) cpt_.push_back(1-cpt[i]); // p(!x|parents) cpt_.insert(cpt_.end(),cpt.begin(),cpt.end()); // p(x|parents) } diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 2087b6f96..b988e9aa2 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -105,7 +105,7 @@ void FactorGraph::push_back(const FactorGraph& factors) { /* ************************************************************************* */ template -void FactorGraph::replace(int index, sharedFactor factor) { +void FactorGraph::replace(size_t index, sharedFactor factor) { if(index >= factors_.size()) throw invalid_argument(boost::str(boost::format( "Factor graph does not contain a factor with index %d.") % index)); @@ -321,7 +321,7 @@ template void FactorGraph::checkGraphConsistency() const { // Make sure each factor listed for a variable actually involves that variable BOOST_FOREACH(const SymbolMap >::value_type& var, indices_) { - BOOST_FOREACH(int i, var.second) { + BOOST_FOREACH(size_t i, var.second) { if(i >= factors_.size()) { cout << "*** Factor graph inconsistency: " << (string)var.first << " lists factor " << i << " but the graph does not contain this many factors." << endl; @@ -380,8 +380,8 @@ void FactorGraph::split(const PredecessorMap& tree, FactorGraphkey1(); Key key2 = factor2->key2(); // if the tree contains the key - if (tree.find(key1) != tree.end() && tree.find(key1)->second.compare(key2) == 0 || - tree.find(key2) != tree.end() && tree.find(key2)->second.compare(key1) == 0) + if ((tree.find(key1) != tree.end() && tree.find(key1)->second.compare(key2) == 0) || + (tree.find(key2) != tree.end() && tree.find(key2)->second.compare(key1) == 0)) Ab1.push_back(factor2); else Ab2.push_back(factor2); diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index a78c798fd..6487f4b08 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -87,7 +87,7 @@ namespace gtsam { void push_back(const FactorGraph& factors); /** replace a factor by index */ - void replace(int index, sharedFactor factor); + void replace(size_t index, sharedFactor factor); /** return keys in some random order */ Ordering keys() const; diff --git a/cpp/ISAM-inl.h b/cpp/ISAM-inl.h index 45fb70452..c984d5c2e 100644 --- a/cpp/ISAM-inl.h +++ b/cpp/ISAM-inl.h @@ -59,7 +59,6 @@ namespace gtsam { for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) this->insert(*rit, index); - int count = 0; // add orphans to the bottom of the new tree BOOST_FOREACH(sharedClique orphan, orphans) { diff --git a/cpp/Makefile.am b/cpp/Makefile.am index f1fe84634..0ce9c4ac5 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -306,8 +306,8 @@ AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serializati if USE_BLAS_LINUX AM_CXXFLAGS += -DGT_USE_CBLAS libgtsam_la_CPPFLAGS += -DGT_USE_CBLAS -AM_LDFLAGS += $(BLAS_LIBS) $(LIBS) $(FLIBS) -libgtsam_la_LDFLAGS += $(BLAS_LIBS) $(LIBS) $(FLIBS) +AM_LDFLAGS += -lcblas -latlas # If getting from script: $(BLAS_LIBS) $(LIBS) $(FLIBS) +libgtsam_la_LDFLAGS += -lcblas -latlas # $(BLAS_LIBS) $(LIBS) $(FLIBS) endif if USE_BLAS_MACOS diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index a92351e09..81e8c4a76 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -267,11 +267,11 @@ void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { // ublas x += prod(trans(A),e) is terribly slow // TODO: use BLAS - size_t m = A.size1(), n = A.size2(); - for (int j = 0; j < n; j++) { + size_t m = A.size1(), n = A.size2(); + for (size_t j = 0; j < n; j++) { const double * ei = e.data().begin(); const double * aij = A.data().begin() + j; - for (int i = 0; i < m; i++, aij+=n, ei++) + for (size_t i = 0; i < m; i++, aij+=n, ei++) x(j) += alpha * (*aij) * (*ei); } } @@ -293,13 +293,6 @@ Vector column_(const Matrix& A, size_t j) { // throw invalid_argument("Column index out of bounds!"); return column(A,j); // real boost version - - // TODO: improve this -// size_t m = A.size1(); -// Vector a(m); -// for (size_t i=0; i& matrices, size_t m, size_t n) // if we have known and constant dimensions, use them size_t dimA1 = m; size_t dimA2 = n*matrices.size(); - if (!m && !n) + if (!m && !n) { BOOST_FOREACH(const Matrix* M, matrices) { - dimA1 = M->size1(); // TODO: should check if all the same ! - dimA2 += M->size2(); + dimA1 = M->size1(); // TODO: should check if all the same ! + dimA2 += M->size2(); + } } // memcpy version @@ -896,11 +893,11 @@ Matrix RtR(const Matrix &A) /* ************************************************************************* */ Vector solve_ldl(const Matrix& M, const Vector& rhs) { - int N = M.size1(); // size of the matrix + unsigned int N = M.size1(); // size of the matrix // count the nonzero entries above diagonal double thresh = 1e-9; - size_t nrANZ = 0; // # of nonzeros on diagonal and upper triangular part of A + unsigned int nrANZ = 0; // # of nonzeros on diagonal and upper triangular part of A for (size_t i=0; i thresh) @@ -950,7 +947,7 @@ Vector solve_ldl(const Matrix& M, const Vector& rhs) { double * Lx = new double[nrLNZ]; int * Li = new int [nrLNZ]; - int d = LDL_numeric (N, Ap, Ai, Ax, Lp, Parent, Lnz, Li, Lx, D, Y, Pattern, + size_t d = LDL_numeric (N, Ap, Ai, Ax, Lp, Parent, Lnz, Li, Lx, D, Y, Pattern, Flag, NULL, NULL); if (d == N) { diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 5f55b48f7..c480e60c3 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -49,7 +49,7 @@ NonlinearConstraint1::NonlinearConstraint1( const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, isEquality), - g_(boost::bind(g, _1)), G_(boost::bind(gradG, _1)), key_(key) + G_(boost::bind(gradG, _1)), g_(boost::bind(g, _1)), key_(key) { this->keys_.push_back(key); } @@ -64,7 +64,7 @@ NonlinearConstraint1::NonlinearConstraint1( const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, isEquality), - g_(g), G_(gradG), key_(key) + G_(gradG), g_(g), key_(key) { this->keys_.push_back(key); } @@ -144,7 +144,7 @@ NonlinearConstraint2::NonlinearConstraint2( const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, isEquality), - g_(boost::bind(g, _1)), G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), + G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)), key1_(key1), key2_(key2) { this->keys_.push_back(key1); @@ -163,7 +163,7 @@ NonlinearConstraint2::NonlinearConstraint2( const LagrangeKey& lagrange_key, bool isEquality) : NonlinearConstraint(lagrange_key, dim_constraint, isEquality), - g_(g), G1_(G1), G2_(G2), + G1_(G1), G2_(G2), g_(g), key1_(key1), key2_(key2) { this->keys_.push_back(key1); diff --git a/cpp/Simulated3D.h b/cpp/Simulated3D.h index 6c373013a..b5673d96b 100644 --- a/cpp/Simulated3D.h +++ b/cpp/Simulated3D.h @@ -67,9 +67,8 @@ namespace simulated3D { Simulated3DMeasurement(const Vector& z, const SharedGaussian& model, PoseKey& j1, PointKey j2) : - z_(z), - NonlinearFactor2 ( - model, j1, j2) { + NonlinearFactor2 ( + model, j1, j2), z_(z) { } Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< diff --git a/cpp/numericalDerivative.h b/cpp/numericalDerivative.h index 9dcbca748..880eb42cf 100644 --- a/cpp/numericalDerivative.h +++ b/cpp/numericalDerivative.h @@ -22,7 +22,6 @@ namespace gtsam { */ template Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { - double hx = h(x); double factor = 1.0/(2.0*delta); const size_t n = x.dim(); Vector d(n,0.0), g(n,0.0); diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index 974ba31b3..0431f2e36 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -78,7 +78,7 @@ namespace gtsam { GenericOdometry(const Point2& z, const SharedGaussian& model, const Key& i1, const Key& i2) : - z_(z), NonlinearFactor2 (model, i1, i2) { + NonlinearFactor2 (model, i1, i2), z_(z) { } Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< @@ -100,7 +100,7 @@ namespace gtsam { GenericMeasurement(const Point2& z, const SharedGaussian& model, const XKey& i, const LKey& j) : - z_(z), NonlinearFactor2 (model, i, j) { + NonlinearFactor2 (model, i, j), z_(z) { } Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 4beffbda5..0a10cc4dc 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -460,8 +460,8 @@ TEST( GaussianFactorGraph, combine) GaussianFactorGraph fg2 = createGaussianFactorGraph(); // get sizes - int size1 = fg1.size(); - int size2 = fg2.size(); + size_t size1 = fg1.size(); + size_t size2 = fg2.size(); // combine them fg1.combine(fg2); @@ -479,8 +479,8 @@ TEST( GaussianFactorGraph, combine2) GaussianFactorGraph fg2 = createGaussianFactorGraph(); // get sizes - int size1 = fg1.size(); - int size2 = fg2.size(); + size_t size1 = fg1.size(); + size_t size2 = fg2.size(); // combine them GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2); @@ -491,7 +491,7 @@ TEST( GaussianFactorGraph, combine2) /* ************************************************************************* */ // print a vector of ints if needed for debugging void print(vector v) { - for (int k = 0; k < v.size(); k++) + for (size_t k = 0; k < v.size(); k++) cout << v[k] << " "; cout << endl; } diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp index 05ca90bdd..4694d81ab 100644 --- a/cpp/testMatrix.cpp +++ b/cpp/testMatrix.cpp @@ -795,19 +795,19 @@ TEST( matrix, svd_sort ) // update A, b // A' \define A_{S}-ar and b'\define b-ad // __attribute__ ((noinline)) // uncomment to prevent inlining when profiling -static void updateAb(Matrix& A, Vector& b, int j, const Vector& a, - const Vector& r, double d) { - const size_t m = A.size1(), n = A.size2(); - for (int i = 0; i < m; i++) { // update all rows - double ai = a(i); - b(i) -= ai * d; - double *Aij = A.data().begin() + i * n + j + 1; - const double *rptr = r.data().begin() + j + 1; - // A(i,j+1:end) -= ai*r(j+1:end) - for (int j2 = j + 1; j2 < n; j2++, Aij++, rptr++) - *Aij -= ai * (*rptr); - } -} +//static void updateAb(Matrix& A, Vector& b, int j, const Vector& a, +// const Vector& r, double d) { +// const size_t m = A.size1(), n = A.size2(); +// for (int i = 0; i < m; i++) { // update all rows +// double ai = a(i); +// b(i) -= ai * d; +// double *Aij = A.data().begin() + i * n + j + 1; +// const double *rptr = r.data().begin() + j + 1; +// // A(i,j+1:end) -= ai*r(j+1:end) +// for (int j2 = j + 1; j2 < n; j2++, Aij++, rptr++) +// *Aij -= ai * (*rptr); +// } +//} /* ************************************************************************* */ TEST( matrix, weighted_elimination ) diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp index 5ba7bdc73..2f4e6ec76 100644 --- a/cpp/testNoiseModel.cpp +++ b/cpp/testNoiseModel.cpp @@ -53,7 +53,6 @@ TEST(NoiseModel, constructors) m.push_back(Isotropic::Precision(3, prc)); // test whiten - int i=0; BOOST_FOREACH(Gaussian::shared_ptr mi, m) CHECK(assert_equal(whitened,mi->whiten(unwhitened))); diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 5c59795e1..50ecc94ba 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -139,7 +139,6 @@ namespace test2 { /** jacobian for y, jacobianG(x,y) in y: -1 */ Matrix G2(const VecConfig& config, const list& keys) { - double x = config[keys.back()](0); return Matrix_(1, 1, -1.0); } diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index 305ee7b8e..3aab814cc 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -78,7 +78,6 @@ TEST( Pose2Graph, linearization ) 0.0, 2.0, 0.0, 0.0, 0.0, 10.0); - double sigma = 1; Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); SharedDiagonal probModel1 = noiseModel::Unit::Create(3); lfg_expected.add("x1", A1, "x2", A2, b, probModel1); diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 9ff0e738b..f1915a197 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -915,7 +915,6 @@ boost::shared_ptr linearMapWarpGraph() { /* ********************************************************************* */ TEST ( SQPOptimizer, map_warp_initLam ) { - bool verbose = false; // get a graph boost::shared_ptr graph = linearMapWarpGraph(); diff --git a/cpp/visualSLAM.h b/cpp/visualSLAM.h index 2395bc47a..42d61bac2 100644 --- a/cpp/visualSLAM.h +++ b/cpp/visualSLAM.h @@ -65,7 +65,7 @@ namespace gtsam { namespace visualSLAM { GenericProjectionFactor(const Point2& z, const SharedGaussian& model, PosK j_pose, LmK j_landmark, const shared_ptrK& K) : - z_(z), K_(K), Base(model, j_pose, j_landmark) { + Base(model, j_pose, j_landmark), z_(z), K_(K) { } /**