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

Use consistent check and refactored to avoid sqrt

release/4.3a0
Frank Dellaert 2015-07-05 15:40:42 -07:00
parent c978935e8e
commit ecfa459590
1 changed files with 11 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/geometry/Pose3.cpp
View File

@ -32,7 +32,7 @@ GTSAM_CONCEPT_POSE_INST(Pose3);
/* ************************************************************************* */ /* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) : Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( R_(Rot3::Rodrigues(0, 0, pose2.theta())), t_(
Point3(pose2.x(), pose2.y(), 0)) { Point3(pose2.x(), pose2.y(), 0)) {
} }
@ -112,24 +112,20 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */ /** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
if (H) { if (H) *H = ExpmapDerivative(xi);
*H = ExpmapDerivative(xi);
}
// get angular velocity omega and translational velocity v from twist xi // get angular velocity omega and translational velocity v from twist xi
Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
double theta = w.norm(); Rot3 R = Rot3::Expmap(omega.vector());
if (theta < 1e-10) { double theta2 = omega.dot(omega);
static const Rot3 I; if (theta2 > std::numeric_limits<double>::epsilon()) {
return Pose3(I, v); double omega_v = omega.dot(v); // translation parallel to axis
} else { Point3 omega_cross_v = omega.cross(v); // points towards axis
Point3 n(w / theta); // axis unit vector Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2;
Rot3 R = Rot3::rodriguez(n.vector(), theta);
double vn = n.dot(v); // translation parallel to n
Point3 n_cross_v = n.cross(v); // points towards axis
Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n;
return Pose3(R, t); return Pose3(R, t);
} else {
return Pose3(R, v);
} }
} }