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

ChartOrigin is only meant to generate a Lie group class

release/4.3a0
dellaert 2015-07-16 23:46:39 -07:00
parent 03be928065
commit f5c8b07f66
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose3.cpp
View File

@ -157,7 +157,7 @@ Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
return Expmap(xi, H); return Expmap(xi, H);
#else #else
Matrix3 DR; Matrix3 DR;
Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); Rot3 R = Rot3::ChartAtOrigin::Retract(xi.head<3>(), H ? &DR : 0);
if (H) { if (H) {
*H = I_6x6; *H = I_6x6;
H->topLeftCorner<3,3>() = DR; H->topLeftCorner<3,3>() = DR;
@ -172,7 +172,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
return Logmap(T, H); return Logmap(T, H);
#else #else
Matrix3 DR; Matrix3 DR;
Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); Vector3 omega = Rot3::ChartAtOrigin::Local(T.rotation(), H ? &DR : 0);
if (H) { if (H) {
*H = I_6x6; *H = I_6x6;
H->topLeftCorner<3,3>() = DR; H->topLeftCorner<3,3>() = DR;