diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 6fb3aea8a..980d35de1 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -79,12 +79,12 @@ namespace gtsam { Rot3(); /** - * Constructor from columns + * Constructor from *columns* * @param r1 X-axis of rotated frame * @param r2 Y-axis of rotated frame * @param r3 Z-axis of rotated frame */ - Rot3(const Point3& r1, const Point3& r2, const Point3& r3); + Rot3(const Point3& col1, const Point3& col2, const Point3& col3); /** constructor from a rotation matrix, as doubles in *row-major* order !!! */ Rot3(double R11, double R12, double R13, @@ -97,9 +97,6 @@ namespace gtsam { /** constructor from a rotation matrix */ Rot3(const Matrix& R); -// /** constructor from a fixed size rotation matrix */ -// Rot3(const Matrix3& R); - /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion * @param q The quaternion diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 03e7c0e0d..99c5c619e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -36,10 +36,10 @@ static const Matrix3 I3 = Matrix3::Identity(); Rot3::Rot3() : rot_(Matrix3::Identity()) {} /* ************************************************************************* */ -Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) { - rot_.col(0) = r1.vector(); - rot_.col(1) = r2.vector(); - rot_.col(2) = r3.vector(); +Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { + rot_.col(0) = col1.vector(); + rot_.col(1) = col2.vector(); + rot_.col(2) = col3.vector(); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 4836e8680..26890c370 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -32,11 +32,11 @@ namespace gtsam { Rot3::Rot3() : quaternion_(Quaternion::Identity()) {} /* ************************************************************************* */ - Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : + Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) : quaternion_((Eigen::Matrix3d() << - r1.x(), r2.x(), r3.x(), - r1.y(), r2.y(), r3.y(), - r1.z(), r2.z(), r3.z()).finished()) {} + col1.x(), col2.x(), col3.x(), + col1.y(), col2.y(), col3.y(), + col1.z(), col2.z(), col3.z()).finished()) {} /* ************************************************************************* */ Rot3::Rot3(double R11, double R12, double R13,