From ae19e078c7849a86b1a05638813742e7c7ec822a Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 12 Mar 2016 20:44:05 -0500 Subject: [PATCH] Changed argument and implementation of correct() in Mechanization_bRn2 --- gtsam_unstable/slam/Mechanization_bRn2.cpp | 8 ++++---- gtsam_unstable/slam/Mechanization_bRn2.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index edd92b580..94126a68c 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -66,14 +66,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, } /* ************************************************************************* */ -Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { - Vector3 rho = dx.segment(0,3); +Mechanization_bRn2 Mechanization_bRn2::correct(const Vector9& dx) const { + Vector3 rho = dx.segment<3>(0); Rot3 delta_nRn = Rot3::Rodrigues(rho); Rot3 bRn = bRn_ * delta_nRn; - Vector3 x_g = x_g_ + dx.segment(3,3); - Vector3 x_a = x_a_ + dx.segment(6,3); + Vector3 x_g = x_g_ + dx.segment<3>(3); + Vector3 x_a = x_a_ + dx.segment<3>(6); return Mechanization_bRn2(bRn, x_g, x_a); } diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4c85614d4..4268aa0e5 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -68,7 +68,7 @@ public: * @param obj The current state * @param dx The error used to correct and return a new state */ - Mechanization_bRn2 correct(const Vector3& dx) const; + Mechanization_bRn2 correct(const Vector9& dx) const; /** * Integrate to get new state