12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Changed argument and implementation of correct() in Mechanization_bRn2

release/4.3a0
Alex Hagiopol 2016-03-12 20:44:05 -05:00
parent d45a1ecda3
commit ae19e078c7
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -66,14 +66,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { Mechanization_bRn2 Mechanization_bRn2::correct(const Vector9& dx) const {
Vector3 rho = dx.segment(0,3); Vector3 rho = dx.segment<3>(0);
Rot3 delta_nRn = Rot3::Rodrigues(rho); Rot3 delta_nRn = Rot3::Rodrigues(rho);
Rot3 bRn = bRn_ * delta_nRn; Rot3 bRn = bRn_ * delta_nRn;
Vector3 x_g = x_g_ + dx.segment(3,3); Vector3 x_g = x_g_ + dx.segment<3>(3);
Vector3 x_a = x_a_ + dx.segment(6,3); Vector3 x_a = x_a_ + dx.segment<3>(6);
return Mechanization_bRn2(bRn, x_g, x_a); return Mechanization_bRn2(bRn, x_g, x_a);
} }

2
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -68,7 +68,7 @@ public:
* @param obj The current state * @param obj The current state
* @param dx The error used to correct and return a new 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 * Integrate to get new state