diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 658ab9a0d4..16d913a967 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -299,17 +299,14 @@ weightedPseudoinverse(const Vector& a, const Vector& weights) { } /* ************************************************************************* */ -Vector concatVectors(const std::list& vs) -{ +Vector concatVectors(const std::list& vs) { size_t dim = 0; - for(Vector v: vs) - dim += v.size(); + for (const Vector& v : vs) dim += v.size(); Vector A(dim); size_t index = 0; - for(Vector v: vs) { - for(int d = 0; d < v.size(); d++) - A(d+index) = v(d); + for (const Vector& v : vs) { + for (int d = 0; d < v.size(); d++) A(d + index) = v(d); index += v.size(); } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 8bcef6fcce..7108a7660f 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -47,8 +47,9 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { /* ************************************************************************* */ // check *above the diagonal* for non-zero entries -boost::optional checkIfDiagonal(const Matrix M) { +boost::optional checkIfDiagonal(const Matrix& M) { size_t m = M.rows(), n = M.cols(); + assert(m > 0); // check all non-diagonal entries bool full = false; size_t i, j; @@ -92,7 +93,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { return Diagonal::Sigmas(diagonal->array().inverse(), true); } // NOTE(frank): only reaches here if !(smart && diagonal) - return shared_ptr(new Gaussian(R.rows(), R)); + return boost::make_shared(R.rows(), R); } /* ************************************************************************* */ @@ -108,7 +109,7 @@ Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart else { Eigen::LLT llt(information); Matrix R = llt.matrixU(); - return shared_ptr(new Gaussian(n, R)); + return boost::make_shared(n, R); } } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 5a29e5d7dd..7bcf808e50 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -732,7 +732,7 @@ namespace gtsam { }; // Helper function - GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix M); + GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix& M); } // namespace noiseModel diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index f7a1c14a03..b3dc49342b 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -245,7 +245,7 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, // Back-substitute fastBackSubstitute(delta); - count += conditional_->nrFrontals(); + *count += conditional_->nrFrontals(); if (valuesChanged(replaced, originalValues, *delta, threshold)) { markFrontalsAsChanged(changed);