diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 99509303d95573811c52c9158b4ff7f5dcafcc3b..84011a2b8cce60be106c564abf75ad4ee593851c 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/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
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index f5fb41a75ca1aa9091896abe1eb088f2d45a77c7..40f6ab0a4427003381aed3864094a74eab465906 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/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();
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index cf1aa928b6f351a6283e65fe70527e75ead23706..9f657911b7d5b729e1add83e62a2bbc097e17619 100644
--- a/test/gtest_state_block.cpp
+++ b/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);