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

renaming variables

release/4.3a0
dellaert 2016-01-28 01:26:57 -08:00
parent 73f9b9d3fb
commit 27405fc185
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Pose3.cpp
View File

@ -120,9 +120,9 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
Rot3 R = Rot3::Expmap(omega.vector()); Rot3 R = Rot3::Expmap(omega.vector());
double theta2 = omega.dot(omega); double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) { if (theta2 > std::numeric_limits<double>::epsilon()) {
double omega_v = omega.dot(v); // translation parallel to axis Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Point3 omega_cross_v = omega.cross(v); // points towards axis Point3 omega_cross_v = omega.cross(v); // points towards axis
Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2; Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t); return Pose3(R, t);
} else { } else {
return Pose3(R, v); return Pose3(R, v);