Pose3 internal representation

src/aliceVision/geometry/CMakeLists.txt

@@ -2,6 +2,7 @@
 set(geometry_files_headers
     Frustum.hpp
     HalfPlane.hpp
+    lie.hpp
     Pose3.hpp
     rigidTransformation3D.hpp
     Similarity3.hpp
@@ -25,6 +26,7 @@ alicevision_add_library(aliceVision_geometry
 # Unit tests
 alicevision_add_test(rigidTransformation3D_test.cpp NAME "geometry_rigidTransformation3D" LINKS aliceVision_geometry)
 alicevision_add_test(halfSpaceIntersection_test.cpp NAME "geometry_halfSpaceIntersection" LINKS aliceVision_geometry)
+alicevision_add_test(Pose3d_test.cpp NAME "geometry_pose3D" LINKS aliceVision_geometry)
 alicevision_add_test(frustumIntersection_test.cpp
   NAME "geometry_frustumIntersection"
   LINKS aliceVision_geometry

src/aliceVision/geometry/Pose3.hpp

@@ -9,85 +9,103 @@
 
 #include <aliceVision/numeric/numeric.hpp>
 #include <boost/math/constants/constants.hpp>
+#include <aliceVision/geometry/lie.hpp>
+
 
 namespace aliceVision {
 namespace geometry {
 
-// Define a 3D Pose as a 3D transform: [R|C] t = -RC
+// Define a 3D Pose as a SE3 matrix
 class Pose3
 {
-  protected:
-    Mat3 _rotation;
-    Vec3 _center;
-
-  public:
-    // Constructors
-    Pose3() : _rotation(Mat3::Identity()), _center(Vec3::Zero()) {}
-    Pose3(const Mat3& r, const Vec3& c) : _rotation(r), _center(c) {}
-    Pose3(const Mat34& Rt)
-    : _rotation(Rt.block<3,3>(0,0))
+protected:
+    SE3::Matrix _homogeneous{SE3::Matrix::Identity()};
+
+public:
+    Pose3() = default;
+
+    Pose3(const Mat3& c_R_w, const Vec3& w_c)
+    {
+        _homogeneous.setIdentity();
+        _homogeneous.block<3, 3>(0, 0) = c_R_w;
+        _homogeneous.block<3, 1>(0, 3) = - c_R_w * w_c;
+    }
+
+    Pose3(const Mat4& T) : _homogeneous(T)
     {
-      Vec3 t = Rt.block<3, 1>(0,3);
-      _center = -_rotation.transpose() * t;
     }
 
     // Accessors
-    const Mat3& rotation() const { return _rotation; }
-    Mat3& rotation() { return _rotation; }
-    const Vec3& center() const { return _center; }
-    Vec3& center() { return _center; }
+    Mat3 rotation() const 
+    { 
+        return _homogeneous.block<3, 3>(0, 0); 
+    }
+
+    void setRotation(const Mat3& rotation)
+    {
+        _homogeneous.block<3, 3>(0, 0) = rotation;
+    }
+
+    Vec3 translation() const
+    { 
+        return _homogeneous.block<3, 1>(0, 3); 
+    }
+
+    Vec3 center() const
+    { 
+        return - rotation().transpose() * translation(); 
+    }
 
-    // Translation vector t = -RC
-    inline Vec3 translation() const { return -(_rotation * _center); }
+    void setCenter(const Vec3& center)
+    {
+        _homogeneous.block<3, 1>(0, 3) = - rotation() * center;
+    }
 
     // Apply pose
     inline Mat3X operator () (const Mat3X& p) const
     {
-      return _rotation * (p.colwise() - _center);
+        return rotation() * (p.colwise() - center());
     }
 
     // Composition
     Pose3 operator * (const Pose3& P) const
     {
-      return Pose3(_rotation * P._rotation, P._center + P._rotation.transpose() * _center );
+        return Pose3(_homogeneous * P._homogeneous);
     }
 
     // Operator ==
     bool operator==(const Pose3& other) const
     {
-      return AreMatNearEqual(_rotation, other._rotation, 1e-6) &&
-              AreVecNearEqual(_center, other._center, 1e-6);
+        return AreMatNearEqual(rotation(), other.rotation(), 1e-6) &&
+               AreVecNearEqual(center(), other.center(), 1e-6);
     }
 
     // Inverse
     Pose3 inverse() const
     {
-      return Pose3(_rotation.transpose(),  -(_rotation * _center));
+        // parameter is center ... which will inverse
+        return Pose3(rotation().transpose(),  translation() );
     }
 
     /// Return the depth (distance) of a point respect to the camera center
-    double depth(const Vec3 &X) const
+    double depth(const Vec3& X) const
     {
-      return (_rotation * (X - _center))[2];
+        return (rotation() * (X - center()))[2];
     }
 
     /// Return the pose with the Scale, Rotation and translation applied.
-    Pose3 transformSRt(const double S, const Mat3 & R, const Vec3 & t) const
+    Pose3 transformSRt(const double S, const Mat3& R, const Vec3& t) const
     {
-      Pose3 pose;
-      pose._center = S * R * _center + t;
-      pose._rotation = _rotation * R.transpose();
-      return pose;
+        Pose3 pose;
+        pose.setCenter(S * R * center() + t);
+        pose.setRotation(rotation() * R.transpose());
+
+        return pose;
     }
 
-    Mat4 getHomogeneous() const 
+    const SE3::Matrix & getHomogeneous() const 
     {
-      Mat4 ret = Mat4::Identity();
-
-      ret.block<3, 3>(0, 0) = _rotation;
-      ret.block<3, 1>(0, 3) = translation();
-
-      return ret;
+        return _homogeneous;
     }
 
 public:
@@ -102,17 +120,14 @@ class Pose3
  */
 inline Pose3 poseFromRT(const Mat3& R, const Vec3& t) 
 {
-  return Pose3(R, -R.transpose()*t);
+    return Pose3(R, -R.transpose() * t);
 }
 
 inline Pose3 randomPose()
 {
-    using namespace Eigen;
-    Vec3 rAngles = Vec3::Random() * boost::math::constants::pi<double>();
-    Mat3 R(AngleAxisd(rAngles(0), Vec3::UnitZ())
-          * AngleAxisd(rAngles(1), Vec3::UnitY())
-          * AngleAxisd(rAngles(2), Vec3::UnitZ()));
-    return geometry::Pose3(R, Vec3::Random());
+    Vec3 vecR = Vec3::Random().normalized() * boost::math::constants::pi<double>();
+
+    return geometry::Pose3(SO3::expm(vecR), Vec3::Random());
 }
 
 } // namespace geometry

src/aliceVision/geometry/Pose3d_test.cpp

@@ -0,0 +1,205 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2023 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#define BOOST_TEST_MODULE Pose3d
+
+#include <aliceVision/geometry/Pose3.hpp>
+#include <aliceVision/unitTest.hpp>
+#include <boost/test/unit_test.hpp>
+#include <boost/test/floating_point_comparison.hpp>
+
+using namespace aliceVision;
+
+BOOST_AUTO_TEST_CASE(DefaultConstructorTest)
+{
+    geometry::Pose3 pose;
+    const double precision{1e-5};
+    EXPECT_MATRIX_NEAR(pose.rotation(), Mat3::Identity(), precision);
+    EXPECT_MATRIX_NEAR(pose.center(), Vec3::Zero(), precision);
+    EXPECT_MATRIX_NEAR(pose.translation(), Vec3::Zero(), precision);
+}
+
+BOOST_AUTO_TEST_CASE(ConstructorTest)
+{
+    const Mat3 rotation = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    //  rotation << 1, 0, 0,
+    //              0, 1, 0,
+    //              0, 0, 1;
+    const Vec3 center(-1.34, 2.432, 3.396);
+    const double precision{1e-5};
+
+    geometry::Pose3 pose(rotation, center);
+
+    EXPECT_MATRIX_NEAR(pose.rotation(), rotation, precision);
+    EXPECT_MATRIX_NEAR(pose.center(), center, precision);
+    EXPECT_MATRIX_NEAR(pose.translation(), Vec3(-rotation*center), precision);
+}
+
+BOOST_AUTO_TEST_CASE(RotationAccessorsTest)
+{
+    const Mat3 rotation = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    const double precision{1e-5};
+    geometry::Pose3 pose;
+    pose.setRotation(rotation);
+
+    EXPECT_MATRIX_NEAR(pose.rotation(), rotation, precision);
+}
+
+BOOST_AUTO_TEST_CASE(CenterAccessorsTest)
+{
+    const double precision{1e-5};
+    const Vec3 center(-1.34, 2.432, 3.396);
+
+    geometry::Pose3 pose;
+    pose.setCenter(center);
+
+    EXPECT_MATRIX_NEAR(pose.center(), center, precision);
+}
+
+BOOST_AUTO_TEST_CASE(TranslationTest)
+{
+    const double precision{1e-5};
+    const Mat3 rotation = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    const Vec3 center(-1.34, 2.432, 3.396);
+    const Vec3 translation = -rotation * center;
+
+    geometry::Pose3 pose(rotation, center);
+
+    EXPECT_MATRIX_NEAR(pose.translation(), translation, precision);
+}
+
+BOOST_AUTO_TEST_CASE(ApplyPoseTest)
+{
+    const double precision{1e-5};
+    const Mat3 rotation = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    const Vec3 center(-1.34, 2.432, 3.396);
+    const Vec3 translation = -rotation * center;
+
+    const Mat4 transform = Eigen::Affine3d(Eigen::Translation3d(translation) * rotation).matrix();
+
+    const geometry::Pose3 pose(rotation, center);
+
+    const Mat3X points = Mat3X::Random(3, 10);
+
+    const Mat3X result = pose(points);
+
+    const Mat3X expected = (transform * points.colwise().homogeneous()).colwise().hnormalized();
+
+    EXPECT_MATRIX_NEAR(result, expected, precision);
+}
+
+BOOST_AUTO_TEST_CASE(CompositionTest)
+{
+    const double precision{1e-5};
+    const Mat3 rotation1 = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    const Vec3 center1(-1.34, 2.432, 3.396);
+    const Vec3 translation1 = -rotation1 * center1;
+    const Mat4 homogeneous1 = Eigen::Affine3d(Eigen::Translation3d(translation1) * rotation1).matrix();
+
+    const Mat3 rotation2 = Eigen::AngleAxisd(-0.965, Vec3(-0.034, 1, 0.2).normalized()).toRotationMatrix();
+    const Vec3 center2(10.4, -25.2, 9.5);
+    const Vec3 translation2 = -rotation2 * center2;
+    const Mat4 homogeneous2 = Eigen::Affine3d(Eigen::Translation3d(translation2) * rotation2).matrix();
+
+    const geometry::Pose3 pose1(rotation1, center1);
+    const geometry::Pose3 pose2(rotation2, center2);
+
+    const geometry::Pose3 result = pose1 * pose2;
+    const Mat4 expected = homogeneous1 * homogeneous2;
+
+    EXPECT_MATRIX_NEAR(result.getHomogeneous(), expected, precision);
+}
+
+BOOST_AUTO_TEST_CASE(EqualityTest)
+{
+    const Mat3 rotation = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    const Vec3 center(-1.34, 2.432, 3.396);
+
+    const geometry::Pose3 pose(rotation, center);
+
+    BOOST_CHECK(pose == pose);
+}
+
+BOOST_AUTO_TEST_CASE(InverseTest)
+{
+    const double precision{1e-5};
+    const Mat3 rotation = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    const Vec3 center(-1.34, 2.432, 3.396);
+
+    const geometry::Pose3 pose(rotation, center);
+    const geometry::Pose3 inverse = pose.inverse();
+
+    const Mat3 expectedRotation = rotation.transpose();
+    const Vec3 expectedCenter = pose.translation();
+
+    EXPECT_MATRIX_NEAR(inverse.rotation(), expectedRotation, precision);
+    EXPECT_MATRIX_NEAR(inverse.center(), expectedCenter, precision);
+    EXPECT_MATRIX_NEAR((inverse * pose).getHomogeneous(), Mat4::Identity(), precision);
+
+}
+
+BOOST_AUTO_TEST_CASE(DepthTest)
+{
+    const double precision{1e-5};
+    const Mat3 rotation = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    const Vec3 center(-1.34, 2.432, 3.396);
+    const Vec3 translation = -rotation * center;
+    const Mat4 homogeneous = Eigen::Affine3d(Eigen::Translation3d(translation) * rotation).matrix();
+
+    geometry::Pose3 pose(rotation, center);
+
+    const Mat3X points = Mat3X::Random(3, 10);
+
+    for(Eigen::Index i{0}; i < points.cols(); ++i)
+    {
+        const auto& point = points.col(i);
+        const auto& transformedPoint = (homogeneous * point.homogeneous()).hnormalized();
+        const double expectedDepth = transformedPoint(2);
+        const double depth = pose.depth(point);
+
+        BOOST_CHECK_CLOSE(depth, expectedDepth, precision);
+    }
+}
+
+//BOOST_AUTO_TEST_CASE(TransformSRtTest)
+//{
+//    const double precision{1e-5};
+//    Mat3 rotation;
+//    rotation << 1, 0, 0, 0, 1, 0, 0, 0, 1;
+//    const Vec3 center(1, 2, 3);
+//
+//    const geometry::Pose3 pose(rotation, center);
+//
+//    double scale{2.0};
+//    Mat3 newRotation;
+//    newRotation << 0, -1, 0, 1, 0, 0, 0, 0, 1;
+//    const Vec3 translation(4, 5, 6);
+//
+//    const geometry::Pose3 transformedPose = pose.transformSRt(scale, newRotation, translation);
+//
+//    const Vec3 expectedCenter = scale * newRotation * center + translation;
+//    const Mat3 expectedRotation = rotation * newRotation.transpose();
+//
+//    EXPECT_MATRIX_NEAR(transformedPose.center(), expectedCenter, precision);
+//    EXPECT_MATRIX_NEAR(transformedPose.rotation(), expectedRotation, precision);
+//}
+
+BOOST_AUTO_TEST_CASE(GetHomogeneousTest)
+{
+    const double precision{1e-5};
+    const Mat3 rotation = Eigen::AngleAxisd(-0.324, Vec3(-0.236, 1, 0.874).normalized()).toRotationMatrix();
+    const Vec3 center(-1.34, 2.432, 3.396);
+    const Vec3 translation = -rotation * center;
+
+    const geometry::Pose3 pose(rotation, center);
+
+    const Mat4 expectedHomogeneous = Eigen::Affine3d(Eigen::Translation3d(translation) * rotation).matrix();
+
+    const Mat4 homogeneous = pose.getHomogeneous();
+
+    EXPECT_MATRIX_NEAR(homogeneous, expectedHomogeneous, precision);
+}
+