diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 31d63d312..de595239a 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -68,7 +68,8 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) } } -ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) +ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, + bool nearZeroApprox) : theta2(angle * angle), theta(angle) { const double ax = axis.x(), ay = axis.y(), az = axis.z(); K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; @@ -88,9 +89,9 @@ SO3 ExpmapFunctor::expmap() const { DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - if (nearZero) + if (nearZero) { dexp_ = I_3x3 - 0.5 * W; - else { + } else { a = one_minus_cos / theta; b = 1.0 - sin_theta / theta; dexp_ = I_3x3 - a * K + b * KK; @@ -296,9 +297,9 @@ static Vector9 vec3(const Matrix3& R) { } // so<3> generators -static const std::vector G3({SO3::Hat(Vector3::Unit(0)), - SO3::Hat(Vector3::Unit(1)), - SO3::Hat(Vector3::Unit(2))}); +static std::vector G3({SO3::Hat(Vector3::Unit(0)), + SO3::Hat(Vector3::Unit(1)), + SO3::Hat(Vector3::Unit(2))}); // vectorized generators static const Matrix93 P3 = diff --git a/gtsam/geometry/SO4.cpp b/gtsam/geometry/SO4.cpp index 93c5f7f8e..5660e85d5 100644 --- a/gtsam/geometry/SO4.cpp +++ b/gtsam/geometry/SO4.cpp @@ -138,7 +138,7 @@ static SO4::VectorN2 vec4(const Matrix4& Q) { } // so<4> generators -static const std::vector G4( +static std::vector G4( {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});