diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 1fe935720..fd3bd487c 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -85,7 +85,7 @@ bool FactorGraph::equals /* ************************************************************************* */ template size_t FactorGraph::nrFactors() const { - int size_ = 0; + size_t size_ = 0; for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) if (*factor != NULL) size_++; return size_; @@ -97,7 +97,7 @@ void FactorGraph::push_back(sharedFactor factor) { factors_.push_back(factor); // add the actual factor if (factor==NULL) return; - int i = factors_.size() - 1; // index of factor + size_t i = factors_.size() - 1; // index of factor associateFactor(i, factor); } @@ -150,16 +150,16 @@ std::pair, set > FactorGraph::removeSingleto // find all the singleton variables Ordering new_singletons; Symbol key; - list indices; + list indices; BOOST_FOREACH(boost::tie(key, indices), indices_) { // find out the number of factors associated with the current key - int numValidFactors = 0; - BOOST_FOREACH(const int& i, indices) + size_t numValidFactors = 0; + BOOST_FOREACH(const size_t& i, indices) if (factors_[i]!=NULL) numValidFactors++; if (numValidFactors == 1) { new_singletons.push_back(key); - BOOST_FOREACH(const int& i, indices) + BOOST_FOREACH(const size_t& i, indices) if (factors_[i]!=NULL) singletonGraph.push_back(factors_[i]); } } @@ -306,7 +306,7 @@ Ordering FactorGraph::getConstrainedOrdering(const set& lastKeys /** O(1) */ /* ************************************************************************* */ template -list FactorGraph::factors(const Symbol& key) const { +list FactorGraph::factors(const Symbol& key) const { return indices_.at(key); } @@ -322,10 +322,10 @@ Factors FactorGraph::findAndRemoveFactors(const Symbol& key) { throw std::invalid_argument( "FactorGraph::findAndRemoveFactors: key " + (string)key + " not found"); - const list& factorsAssociatedWithKey = it->second; + const list& factorsAssociatedWithKey = it->second; Factors found; - BOOST_FOREACH(const int& i, factorsAssociatedWithKey) { + BOOST_FOREACH(const size_t& i, factorsAssociatedWithKey) { sharedFactor& fi = factors_.at(i); // throws exception ! if(fi == NULL) continue; // skip NULL factors found.push_back(fi); // add to found @@ -339,7 +339,7 @@ Factors FactorGraph::findAndRemoveFactors(const Symbol& key) { /* ************************************************************************* */ template -void FactorGraph::associateFactor(int index, const sharedFactor& factor) { +void FactorGraph::associateFactor(size_t index, const sharedFactor& factor) { const list keys = factor->keys(); // get keys for factor // rtodo: Can optimize factor->keys to return a const reference @@ -368,7 +368,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(const SymbolMap >::value_type& var, indices_) { BOOST_FOREACH(size_t i, var.second) { if(i >= factors_.size()) { cout << "*** Factor graph inconsistency: " << (string)var.first << " lists factor " << diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 90fba540f..576d157aa 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -44,7 +44,7 @@ namespace gtsam { std::vector factors_; /** For each variable a list of factor indices connected to it */ - typedef SymbolMap > Indices; + typedef SymbolMap > Indices; Indices indices_; public: @@ -117,7 +117,7 @@ namespace gtsam { * Return indices for all factors that involve the given node * @param key the key for the given node */ - std::list factors(const Symbol& key) const; + std::list factors(const Symbol& key) const; /** * find all the factors that involve the given node and remove them @@ -151,7 +151,7 @@ namespace gtsam { private: /** Associate factor index with the variables connected to the factor */ - void associateFactor(int index, const sharedFactor& factor); + void associateFactor(size_t index, const sharedFactor& factor); /** Serialization function */ friend class boost::serialization::access; diff --git a/cpp/GaussianConditional.cpp b/cpp/GaussianConditional.cpp index d9f639f8e..26a5f5832 100644 --- a/cpp/GaussianConditional.cpp +++ b/cpp/GaussianConditional.cpp @@ -14,21 +14,21 @@ using namespace gtsam; /* ************************************************************************* */ GaussianConditional::GaussianConditional(const Symbol& key,Vector d, Matrix R, Vector sigmas) : - Conditional (key), R_(R),sigmas_(sigmas),d_(d) + Conditional (key), R_(R),d_(d),sigmas_(sigmas) { } /* ************************************************************************* */ GaussianConditional::GaussianConditional(const Symbol& key, Vector d, Matrix R, const Symbol& name1, Matrix S, Vector sigmas) : - Conditional (key), R_(R), sigmas_(sigmas), d_(d) { + Conditional (key), R_(R), d_(d), sigmas_(sigmas) { parents_.insert(make_pair(name1, S)); } /* ************************************************************************* */ GaussianConditional::GaussianConditional(const Symbol& key, Vector d, Matrix R, const Symbol& name1, Matrix S, const Symbol& name2, Matrix T, Vector sigmas) : - Conditional (key), R_(R),sigmas_(sigmas), d_(d) { + Conditional (key), R_(R), d_(d),sigmas_(sigmas) { parents_.insert(make_pair(name1, S)); parents_.insert(make_pair(name2, T)); } @@ -36,7 +36,7 @@ GaussianConditional::GaussianConditional(const Symbol& key, Vector d, Matrix R, /* ************************************************************************* */ GaussianConditional::GaussianConditional(const Symbol& key, const Vector& d, const Matrix& R, const SymbolMap& parents, Vector sigmas) : - Conditional (key), R_(R),sigmas_(sigmas), d_(d), parents_(parents) { + Conditional (key), R_(R), parents_(parents), d_(d),sigmas_(sigmas) { } /* ************************************************************************* */ @@ -64,7 +64,7 @@ bool GaussianConditional::equals(const Conditional &c, double tol) const { if (parents_.size() != p->parents_.size()) return false; // check if R_ and d_ are equal up to a sign - for (int i=0; iR_, i); if (!((::equal_with_abs_tol(row1, row2, tol) && fabs(d_(i) - p->d_(i)) < tol) || diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index 03f199355..0941b321f 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -77,7 +77,6 @@ GaussianFactor::GaussianFactor(const boost::shared_ptr& cg) for (; it != cg->parentsEnd(); it++) As_.insert(*it); // set sigmas from precisions - size_t n = b_.size(); model_ = noiseModel::Diagonal::Sigmas(cg->get_sigmas(), true); } @@ -167,7 +166,7 @@ bool GaussianFactor::equals(const Factor& f, double tol) const { if(As_.size() != lf->As_.size()) return false; // check whether each row is up to a sign - for (int i=0; i row1; list row2; row1.push_back(Vector_(1, b_(i))); @@ -442,7 +441,6 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model, pair GaussianFactor::eliminate(const Symbol& key) const { - bool verbose = false; // if this factor does not involve key, we exit with empty CG and LF const_iterator it = As_.find(key); if (it==As_.end()) { diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 6142e10c2..ded2654b2 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -146,12 +146,16 @@ public: /** * return the first key + * TODO: this function should be removed and the minimum spanning tree code + * modified accordingly. * @return The set of all variable keys */ Symbol key1() const { return As_.begin()->first; } /** * return the first key + * TODO: this function should be removed and the minimum spanning tree code + * modified accordingly. * @return The set of all variable keys */ Symbol key2() const { diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index 6121eb919..a68d865e3 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -166,7 +166,7 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) { if(m1!=m2 || n1!=n2) return false; - for(int i=0; i 1e-9)) goto full; @@ -128,7 +128,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { /* ************************************************************************* */ Diagonal::Diagonal(const Vector& sigmas) : - Gaussian(sigmas.size()), invsigmas_(reciprocal(sigmas)), sigmas_(sigmas) { + Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)) { } Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { @@ -174,7 +174,7 @@ void Diagonal::WhitenInPlace(Matrix& H) const { Vector Diagonal::sample() const { Vector result(dim_); - for (int i = 0; i < dim_; i++) { + for (size_t i = 0; i < dim_; i++) { typedef boost::normal_distribution Normal; Normal dist(0.0, this->sigmas_(i)); boost::variate_generator norm(generator, dist); @@ -315,7 +315,7 @@ Vector Isotropic::sample() const { Normal dist(0.0, this->sigma_); boost::variate_generator norm(generator, dist); Vector result(dim_); - for (int i = 0; i < dim_; i++) + for (size_t i = 0; i < dim_; i++) result(i) = norm(); return result; } diff --git a/cpp/SharedDiagonal.h b/cpp/SharedDiagonal.h index 7d4276e45..247b0b52e 100644 --- a/cpp/SharedDiagonal.h +++ b/cpp/SharedDiagonal.h @@ -38,8 +38,14 @@ namespace gtsam { // note, deliberately not in noiseModel namespace inline SharedDiagonal sharedSigmas(const Vector& sigmas) { return noiseModel::Diagonal::Sigmas(sigmas); } - inline SharedDiagonal sharedSigma(int dim, double sigma) { + inline SharedDiagonal sharedSigma(size_t dim, double sigma) { return noiseModel::Isotropic::Sigma(dim, sigma); } + inline SharedDiagonal sharedPrecisions(const Vector& precisions) { + return noiseModel::Diagonal::Precisions(precisions); + } + inline SharedDiagonal sharedPrecision(size_t dim, double precision) { + return noiseModel::Isotropic::Precision(dim, precision); + } } diff --git a/cpp/dataset.cpp b/cpp/dataset.cpp index 57c73fdcf..59ac37e66 100644 --- a/cpp/dataset.cpp +++ b/cpp/dataset.cpp @@ -87,7 +87,6 @@ pair load2D(const string& filename, string tag; // load the poses - bool firstPose; while (is) { is >> tag; @@ -200,7 +199,6 @@ bool load3D(const string& filename) { is.clear(); /* clears the end-of-file and error flags */ is.seekg(0, ios::beg); - bool edgesOk = true; while (is) { char buf[LINESIZE]; is.getline(buf, LINESIZE); diff --git a/cpp/graph-inl.h b/cpp/graph-inl.h index 842767e08..54471e0ec 100644 --- a/cpp/graph-inl.h +++ b/cpp/graph-inl.h @@ -120,8 +120,8 @@ predecessorMap2Graph(const PredecessorMap& p_map) { if (!foundRoot) throw invalid_argument("predecessorMap2Graph: invalid predecessor map!"); - - return boost::tuple >(g, root, key2vertex); + else + return boost::tuple >(g, root, key2vertex); } /* ************************************************************************* */ diff --git a/cpp/timeGaussianFactor.cpp b/cpp/timeGaussianFactor.cpp index ba3d9ea3e..de6ef671d 100644 --- a/cpp/timeGaussianFactor.cpp +++ b/cpp/timeGaussianFactor.cpp @@ -110,7 +110,7 @@ int main() size_t n1 = 10000000; timeLog = clock(); - for(int i = 0; i < n1; i++) + for(size_t i = 0; i < n1; i++) Matrix Ab = combined.matrix_augmented(ordering, true); timeLog2 = clock(); diff --git a/cpp/timeMatrix.cpp b/cpp/timeMatrix.cpp index 6f84cf2fe..bde581b7e 100644 --- a/cpp/timeMatrix.cpp +++ b/cpp/timeMatrix.cpp @@ -35,7 +35,7 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) { // fill the matrices with identities vector matrices; - for (int i=0; i(v); } + operator string() { return lexical_cast(v); } bool operator!=(const Value& vc) { return v != vc.v; } }; @@ -175,4 +175,6 @@ int main(int argc, char *argv[]) { cout << i << " values, avg " << (time/(double)i)*1e6 << " mu-s per lookup" << endl; } } + + return 0; } diff --git a/cpp/timeVectorConfig.cpp b/cpp/timeVectorConfig.cpp index fa21f908e..970e3bed1 100644 --- a/cpp/timeVectorConfig.cpp +++ b/cpp/timeVectorConfig.cpp @@ -17,7 +17,7 @@ using namespace gtsam; double time = t.elapsed(); \ cout << "Average elapsed time :" << 10e3 * time / n << "ms." << endl; } -#define TIME(STATEMENT) TIME1(for (int j = 0; j < n; ++j) STATEMENT;) +#define TIME(STATEMENT) TIME1(for (size_t j = 0; j < n; ++j) STATEMENT;) /* ************************************************************************* */ int main(int argc, char ** argv) { @@ -44,7 +44,7 @@ int main(int argc, char ** argv) { VectorBTree p, q; VectorMap old; cout << "Creating VectorBTree:" << endl; - TIME1(for (int i = 0; i < m; ++i) { + TIME1(for (size_t i = 0; i < m; ++i) { Vector v = zero(r); Symbol key('x', i); p.insert(key, v); @@ -59,10 +59,10 @@ int main(int argc, char ** argv) { TIME(axpy(alpha,p,q)); cout << "VectorBTree get:" << endl; - TIME1(for (int i = 0; i < m; ++i) p[Symbol('x', i)]); + TIME1(for (size_t i = 0; i < m; ++i) p[Symbol('x', i)]); cout << "VectorMap get:" << endl; - TIME1(for (int i = 0; i < m; ++i) old[Symbol('x', i)]); + TIME1(for (size_t i = 0; i < m; ++i) old[Symbol('x', i)]); } return 0;