diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 4f9dafacc..83e483351 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -85,7 +85,7 @@ void NonlinearISAM::reorder_relinearize() { /* ************************************************************************* */ template -Values NonlinearISAM::estimate() { +Values NonlinearISAM::estimate() const { if(isam_.size() > 0) return linPoint_.expmap(optimize(isam_), ordering_); else @@ -94,7 +94,7 @@ Values NonlinearISAM::estimate() { /* ************************************************************************* */ template -Matrix NonlinearISAM::marginalCovariance(const Symbol& key) { +Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { Matrix covariance; Vector mean; boost::tie(mean, covariance) = isam_.marginal(ordering_[key]); return covariance; diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 2b7a8a8e4..3178cba16 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -66,7 +66,7 @@ public: void update(const Factors& newFactors, const Values& initialValues); /** Return the current solution estimate */ - Values estimate(); + Values estimate() const; /** Relinearization and reordering of variables */ void reorder_relinearize(); @@ -78,7 +78,7 @@ public: void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; } /** find the marginal covariance for a single variable */ - Matrix marginalCovariance(const Symbol& key); + Matrix marginalCovariance(const Symbol& key) const; // access