From 5ea99c4f42a8a2304b9f30445dd550a959da8bfa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 7 Apr 2021 16:45:05 -0400 Subject: [PATCH] bunch of minor fixes --- gtsam/navigation/ImuFactor.cpp | 1 + gtsam/navigation/ImuFactor.h | 2 ++ gtsam/nonlinear/GncOptimizer.h | 2 +- 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 98b1e6f9d..28c0461b1 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -106,6 +106,7 @@ void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& preintMeasCov_ = P + *H2 * pim12.preintMeasCov_ * H2->transpose(); } #endif + //------------------------------------------------------------------------------ // ImuFactor methods //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 29bdb2a99..35207e452 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -192,6 +192,8 @@ public: * @param pose_j Current pose key * @param vel_j Current velocity key * @param bias Previous bias key + * @param preintegratedMeasurements The preintegreated measurements since the + * last pose. */ ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedImuMeasurements& preintegratedMeasurements); diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d319fd5fd..e1efe7132 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -298,7 +298,7 @@ class GncOptimizer { break; case GncLossType::TLS: weightsConverged = true; - for (size_t i = 0; i < weights.size(); i++) { + for (int i = 0; i < weights.size(); i++) { if (std::fabs(weights[i] - std::round(weights[i])) > params_.weightsTol) { weightsConverged = false;