From 8a1c6fe77fae9298985d9ea429a6bf8bcb259435 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 27 May 2014 18:23:57 -0400 Subject: [PATCH] added comment --- gtsam/nonlinear/LagoInitializer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/nonlinear/LagoInitializer.cpp index f7d7f7ce7..9acd52f96 100644 --- a/gtsam/nonlinear/LagoInitializer.cpp +++ b/gtsam/nonlinear/LagoInitializer.cpp @@ -145,6 +145,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spann double key1_DeltaTheta_key2 = deltaTheta(0); double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord double k = round(k2pi_noise/(2*M_PI)); + //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI); lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); }