fix: minor isometry/quaternion API fixes

farm-ng · Nov 8, 2023 · fb77a1b · fb77a1b
1 parent 60bc810
commit fb77a1b
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 2 deletions.
diff --git a/cpp/sophus/lie/isometry2.h b/cpp/sophus/lie/isometry2.h
@@ -126,7 +126,7 @@ class Isometry2
     return this->rotation().unitComplex();
   }
 
-  auto setUnitComplex(Complex<Scalar> const& z) const -> void {
+  auto setUnitComplex(Complex<Scalar> const& z) -> void {
     this->setRotation(Rotation::fromUnitComplex(z));
   }
 

diff --git a/cpp/sophus/lie/isometry3.h b/cpp/sophus/lie/isometry3.h
@@ -156,7 +156,7 @@ class Isometry3
     return Quaternion<Scalar>::fromParams(this->params_.template head<4>());
   }
 
-  auto setUnitQuaternion(Quaternion<Scalar> const& z) const -> void {
+  auto setUnitQuaternion(Quaternion<Scalar> const& z) -> void {
     this->setRotation(Rotation::fromUnitQuaternion(z));
   }
 

diff --git a/cpp/sophus/manifold/quaternion.h b/cpp/sophus/manifold/quaternion.h
@@ -121,6 +121,11 @@ class Quaternion {
     return q;
   }
 
+  static auto from(Scalar real, Eigen::Vector<Scalar, 3> const& imag)
+      -> Quaternion {
+    return fromParams(Params(imag[0], imag[1], imag[2], real));
+  }
+
   static auto zero() -> Quaternion {
     return Quaternion::fromParams(Impl::zero());
   }