fixed StateHomogeneous3d::transform and added tests

include/core/state_block/state_homogeneous_3d.h

@@ -37,6 +37,9 @@ class StateHomogeneous3d : public StateBlock
     void            setIdentity(bool _notify = true) override;
     Eigen::VectorXd identity() const override;
 
+    Eigen::Vector3d point() const;
+    void setStatePoint(const Eigen::Vector3d&);
+
     virtual void transform(const VectorComposite& _transformation) override;
 };
 
@@ -74,11 +77,20 @@ inline Eigen::VectorXd StateHomogeneous3d::Identity()
     return Eigen::Quaterniond::Identity().coeffs();
 }
 
+inline Eigen::Vector3d StateHomogeneous3d::point() const
+{
+    return state_.head<3>() / state_(3);
+}
+
+inline void StateHomogeneous3d::setStatePoint(const Eigen::Vector3d& _p)
+{
+    state_ << _p, 1;
+}
+
 inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
 {
     if (transformable_)
-        setState((Eigen::Quaterniond(_transformation.at('O').data()) * Eigen::Quaterniond(getState().data()))
-                     .coeffs());  // TODO is this correct?????? probably not!!!
+        setStatePoint(_transformation.at('P') + Eigen::Quaterniond(_transformation.at('O').data()) * point());
 }
 
 }  // namespace wolf

include/core/state_block/state_quaternion.h

@@ -45,7 +45,7 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b
     : StateBlock("StateQuaternion",
                  _quaternion.coeffs(),
                  _fixed,
-                 std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
+                 std::make_shared<LocalParametrizationQuaternionLocal>(),
                  _transformable)
 {
     state_.normalize();

test/gtest_state_block.cpp

@@ -22,6 +22,7 @@
 
 #include "core/state_block/state_block.h"
 #include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_homogeneous_3d.h"
 #include "core/state_block/state_angle.h"
 #include "core/state_block/state_block_derived.h"
 #include "core/composite/vector_composite.h"
@@ -32,7 +33,8 @@ using namespace Eigen;
 using namespace std;
 using namespace wolf;
 
-TEST(StateBlock, point_perturb)
+// PERTURB ----------------------------------------------
+TEST(StatePoint3d, perturb)
 {
     Vector3d     x(10, 20, 30);
     StatePoint3d sb(x);
@@ -45,7 +47,7 @@ TEST(StateBlock, point_perturb)
     ASSERT_MATRIX_APPROX(x, sb.getState(), 0.5 * 4);  // 4-sigma test...
 }
 
-TEST(StateBlock, angle_perturb)
+TEST(StateAngle, perturb)
 {
     Vector1d   x(1.0);
     StateAngle sb(x(0));
@@ -58,7 +60,7 @@ TEST(StateBlock, angle_perturb)
     ASSERT_MATRIX_APPROX(x, sb.getState(), 0.1 * 4);  // 4-sigma test...
 }
 
-TEST(StateBlock, quaternion_perturb)
+TEST(StateQuaternion, perturb)
 {
     Vector4d        x(0.5, 0.5, 0.5, 0.5);
     StateQuaternion sb(x);
@@ -71,6 +73,20 @@ TEST(StateBlock, quaternion_perturb)
     ASSERT_MATRIX_APPROX(x, sb.getState(), 0.01 * 4);        // 4-sigma test...
 }
 
+TEST(StateHomogeneous, perturb)
+{
+    Vector4d           x(0.5, 0.5, 0.5, 0.5);
+    StateHomogeneous3d sb(x);
+
+    sb.perturb(0.01);
+
+    WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+    ASSERT_LT((sb.getState().transpose() * x).norm(), 1.0);  // quaternions are not parallel ==> not equal
+    ASSERT_MATRIX_APPROX(x, sb.getState(), 0.01 * 4);        // 4-sigma test...
+}
+
+// TRANSFORMABLE ----------------------------------------------
 TEST(StatePoint3d, transformable)
 {
     Vector3d x(1, 2, 3);
@@ -88,6 +104,7 @@ TEST(StatePoint3d, transformable)
     ASSERT_TRUE(sb_tr2.isTransformable());
 }
 
+// TRANSFORM ----------------------------------------------
 TEST(StatePoint3d, transform_identity)
 {
     Vector3d     x0(1, 2, 3);
@@ -151,6 +168,104 @@ TEST(StatePoint3d, non_transformable_translation_rotation)
     ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
 }
 
+TEST(StateQuaternion, transform_identity)
+{
+    Vector4d        x0(0.5, 0.5, 0.5, 0.5); // Euler XYZ : 90º, 90º, 0º
+    StateQuaternion sb(x0);
+
+    VectorComposite trf({{'P', Vector3d::Zero()}, {'O', Quaterniond::Identity().coeffs()}});
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x0, 1e-15);
+}
+
+TEST(StateQuaternion, transform_translation)
+{
+    Vector4d        x0(0.5, 0.5, 0.5, 0.5); // Euler XYZ : 90º, 90º, 0º
+    StateQuaternion sb(x0);
+
+    VectorComposite trf({{'P', Vector3d(4, 5, 6)}, {'O', Quaterniond::Identity().coeffs()}});
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x0, 1e-15);
+}
+
+TEST(StateQuaternion, transform_rotation_90x)
+{
+    Vector4d        x0(0.5, 0.5, 0.5, 0.5); // Euler XYZ : 90º, 90º, 0º
+    StateQuaternion sb(x0);
+
+    VectorComposite trf({{'P', Vector3d::Zero()}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
+
+    Vector4d x(sqrt(2) / 2, 0, sqrt(2) / 2, 0);  // x0 rotated (90,0,0): XYZ : 180º, 90º, 0º
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateQuaternion, transform_translation_1y_rotation_90x)
+{
+    Vector4d        x0(0.5, 0.5, 0.5, 0.5); // Euler XYZ : 90º, 90º, 0º
+    StateQuaternion sb(x0);
+
+    VectorComposite trf({{'P', Vector3d(0, 1, 0)}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
+
+    Vector4d x(sqrt(2) / 2, 0, sqrt(2) / 2, 0);  // x0 rotated (90,0,0): XYZ : 180º, 90º, 0º
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateHomogeneous3d, transform_identity)
+{
+    Vector4d     x0(1, 2, 3, 1);
+    StateHomogeneous3d sb(x0);
+
+    VectorComposite trf({{'P', Vector3d::Zero()}, {'O', Quaterniond::Identity().coeffs()}});
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState().head<3>() / sb.getState()(3), x0.head<3>(), 1e-15);
+}
+
+TEST(StateHomogeneous3d, transform_translation_4x5y6z)
+{
+    Vector4d     x0(1, 2, 3, 1);
+    StateHomogeneous3d sb(x0);
+
+    VectorComposite trf({{'P', Vector3d(4, 5, 6)}, {'O', Quaterniond::Identity().coeffs()}});
+
+    Vector3d x(5, 7, 9);  // x0 translated (4,5,6)
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState().head<3>() / sb.getState()(3), x, 1e-15);
+}
+
+TEST(StateHomogeneous3d, transform_rotation_90x)
+{
+    Vector4d     x0(1, 2, 3, 1);
+    StateHomogeneous3d sb(x0);
+
+    VectorComposite trf({{'P', Vector3d::Zero()}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
+
+    Vector3d x(1, -3, 2);  // x0 rotated (90,0,0)
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState().head<3>() / sb.getState()(3), x, 1e-15);
+}
+
+TEST(StateHomogeneous3d, transform_translation_1y_rotation_90x)
+{
+    Vector4d     x0(1, 2, 3, 1);
+    StateHomogeneous3d sb(x0);
+
+    VectorComposite trf({{'P', Vector3d(0, 1, 0)}, {'O', (Vector4d() << sqrt(2) / 2, 0, 0, sqrt(2) / 2).finished()}});
+
+    Vector3d x(1, -2, 2);  // x0 translated (0,1,0) and rotated (90,0,0)
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState().head<3>() / sb.getState()(3), x, 1e-12);
+}
+
 TEST(StateVector3d, transform_translation_1y_rotation_90x)
 {
     Vector3d      x0(1, 2, 3);
@@ -203,6 +318,19 @@ TEST(StateParams4, transform_translation_1y_rotation_90x)
     ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
 }
 
+TEST(StateHomogeneous3d, point)
+{
+    Vector4d     x0(1, 2, 3, 1);
+    StateHomogeneous3d sb(x0);
+
+    ASSERT_MATRIX_APPROX(sb.point(), x0.head<3>() / x0(3), 1e-15);
+
+    sb.setStatePoint(Vector3d(3, 2, 1));
+
+    ASSERT_MATRIX_APPROX(sb.getState(), Vector4d(3, 2, 1, 1), 1e-15);
+    ASSERT_MATRIX_APPROX(sb.point(), Vector3d(3, 2, 1), 1e-15);
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);