Added a 3d version of point_transform_affine and also some functions

for making 3d rotations.

dlib/geometry/point_transforms.h

@@ -629,6 +629,145 @@ namespace dlib
         return m;
     }
 
+// ----------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------
+
+    class point_transform_affine3d
+    {
+    public:
+
+        point_transform_affine3d (
+        )
+        {
+            m = identity_matrix<double>(3);
+            b.x() = 0;
+            b.y() = 0;
+        }
+
+        point_transform_affine3d (
+            const matrix<double,3,3>& m_,
+            const dlib::vector<double,3>& b_
+        ) :m(m_), b(b_)
+        {
+        }
+
+        const dlib::vector<double,3> operator() (
+            const dlib::vector<double,3>& p
+        ) const
+        {
+            return m*p + b;
+        }
+
+        const matrix<double,3,3>& get_m(
+        ) const { return m; }
+
+        const dlib::vector<double,3>& get_b(
+        ) const { return b; }
+
+        inline friend void serialize (const point_transform_affine3d& item, std::ostream& out)
+        {
+            serialize(item.m, out);
+            serialize(item.b, out);
+        }
+
+        inline friend void deserialize (point_transform_affine3d& item, std::istream& in)
+        {
+            deserialize(item.m, in);
+            deserialize(item.b, in);
+        }
+
+    private:
+        matrix<double,3,3> m;
+        dlib::vector<double,3> b;
+    };
+
+// ----------------------------------------------------------------------------------------
+
+    inline point_transform_affine3d operator* (
+        const point_transform_affine3d& lhs,
+        const point_transform_affine3d& rhs
+    )
+    {
+        return point_transform_affine3d(lhs.get_m()*rhs.get_m(), lhs.get_m()*rhs.get_b()+lhs.get_b());
+    }
+
+// ----------------------------------------------------------------------------------------
+
+    inline point_transform_affine3d inv (
+        const point_transform_affine3d& trans
+    )
+    {
+        matrix<double,3,3> im = inv(trans.get_m());
+        return point_transform_affine3d(im, -im*trans.get_b());
+    }
+
+// ----------------------------------------------------------------------------------------
+
+    inline point_transform_affine3d rotate_around_x (
+        double angle
+    )
+    {
+        const double ca = std::cos(angle);
+        const double sa = std::sin(angle);
+
+        matrix<double,3,3> m;
+        m = 1,  0,  0,
+            0, ca, -sa,
+            0, sa, ca;
+
+        vector<double,3> b;
+
+        return point_transform_affine3d(m,b);
+    }
+
+// ----------------------------------------------------------------------------------------
+
+    inline point_transform_affine3d rotate_around_y (
+        double angle
+    )
+    {
+        const double ca = std::cos(angle);
+        const double sa = std::sin(angle);
+
+        matrix<double,3,3> m;
+        m = ca,  0, sa,
+             0,  0, 0,
+            -sa, 0, ca;
+
+        vector<double,3> b;
+
+        return point_transform_affine3d(m,b);
+    }
+
+// ----------------------------------------------------------------------------------------
+
+    inline point_transform_affine3d rotate_around_z (
+        double angle
+    )
+    {
+        const double ca = std::cos(angle);
+        const double sa = std::sin(angle);
+
+        matrix<double,3,3> m;
+        m = ca, -sa, 0,
+            sa, ca,  0,
+            0,   0,  0;
+
+        vector<double,3> b;
+
+        return point_transform_affine3d(m,b);
+    }
+
+// ----------------------------------------------------------------------------------------
+
+    inline point_transform_affine3d translate_point (
+        const vector<double,3>& delta
+    )
+    {
+        return point_transform_affine3d(identity_matrix<double>(3),delta);
+    }
+
 // ----------------------------------------------------------------------------------------
 
 }

dlib/geometry/point_transforms_abstract.h

@@ -408,6 +408,142 @@ namespace dlib
               the origin in a counter-clockwise direction.
     !*/
 
+// ----------------------------------------------------------------------------------------
+
+    class point_transform_affine3d
+    {
+        /*!
+            WHAT THIS OBJECT REPRESENTS
+                This is an object that takes 3D points or vectors and 
+                applies an affine transformation to them.
+        !*/
+    public:
+
+        point_transform_affine3d (
+        );
+        /*!
+            ensures
+                - This object will perform the identity transform.  That is, given a point
+                  as input it will return the same point as output.
+        !*/
+
+        point_transform_affine3d (
+            const matrix<double,3,3>& m,
+            const dlib::vector<double,3>& b
+        );
+        /*!
+            ensures
+                - #get_m() == m
+                - #get_b() == b
+                - When (*this)(p) is invoked it will return a point P such that:
+                    - P == m*p + b
+        !*/
+
+        const dlib::vector<double,3> operator() (
+            const dlib::vector<double,3>& p
+        ) const;
+        /*!
+            ensures
+                - applies the affine transformation defined by this object's constructor
+                  to p and returns the result.
+        !*/
+
+        const matrix<double,3,3>& get_m(
+        ) const;
+        /*!
+            ensures
+                - returns the transformation matrix used by this object.
+        !*/
+
+        const dlib::vector<double,3>& get_b(
+        ) const;
+        /*!
+            ensures
+                - returns the offset vector used by this object.
+        !*/
+
+    };
+
+    void serialize   (const point_transform_affine3d& item, std::ostream& out);
+    void deserialize (point_transform_affine3d& item, std::istream& in);
+    /*!
+        provides serialization support
+    !*/
+
+// ----------------------------------------------------------------------------------------
+
+    point_transform_affine3d operator* (
+        const point_transform_affine3d& lhs,
+        const point_transform_affine3d& rhs
+    );
+    /*!
+        ensures
+            - returns a transformation TFORM(x) that is equivalent to lhs(rhs(x)).  That
+              is, for all valid x: TFORM(x) == lhs(rhs(x)).
+    !*/
+
+    // ----------------------------------------------------------------------------------------
+
+    point_transform_affine3d inv (
+        const point_transform_affine3d& trans
+    );
+    /*!
+        ensures
+            - If trans is an invertible transformation then this function returns a new
+              transformation that is the inverse of trans. 
+    !*/
+
+// ----------------------------------------------------------------------------------------
+
+    point_transform_affine3d rotate_around_x (
+        double angle
+    );
+    /*!
+        ensures
+            - Returns a transformation that rotates a point around the x axis in a
+              counter-clockwise direction by angle radians.  That is, the rotation appears
+              counter-clockwise when the x axis points toward the observer, the coordinate
+              system is right-handed, and the angle is positive.
+    !*/
+
+// ----------------------------------------------------------------------------------------
+
+    point_transform_affine3d rotate_around_y (
+        double angle
+    );
+    /*!
+        ensures
+            - Returns a transformation that rotates a point around the y axis in a
+              counter-clockwise direction by angle radians.  That is, the rotation appears
+              counter-clockwise when the y axis points toward the observer, the coordinate
+              system is right-handed, and the angle is positive.
+    !*/
+
+// ----------------------------------------------------------------------------------------
+
+    point_transform_affine3d rotate_around_z (
+        double angle
+    );
+    /*!
+        ensures
+            - Returns a transformation that rotates a point around the z axis in a
+              counter-clockwise direction by angle radians.  That is, the rotation appears
+              counter-clockwise when the z axis points toward the observer, the coordinate
+              system is right-handed, and the angle is positive.
+    !*/
+
+// ----------------------------------------------------------------------------------------
+
+    point_transform_affine3d translate_point (
+        const vector<double,3>& delta
+    );
+    /*!
+        ensures
+            - returns a transformation that simply translates points by adding delta to
+              them.  That is, this function returns:
+                point_transform_affine3d(identity_matrix<double>(3),delta);
+    !*/
+
 // ----------------------------------------------------------------------------------------
 
 }

dlib/test/geometry.cpp

@@ -805,6 +805,34 @@ namespace
         }
     }
 
+// ----------------------------------------------------------------------------------------
+
+    void test_affine3d()
+    {
+        const dlib::vector<double> x(1,0,0);
+        const dlib::vector<double> y(0,1,0);
+        const dlib::vector<double> z(0,0,1);
+
+        dlib::vector<double> w;
+
+        w = rotate_around_z(pi/2)(x);
+        DLIB_TEST(length(w-y) < 1e-12);
+
+        w = rotate_around_y(-pi/2)(x);
+        DLIB_TEST(length(w-z) < 1e-12);
+
+        w = rotate_around_x(pi/2)(y);
+        DLIB_TEST(length(w-z) < 1e-12);
+
+        w = translate_point(x)(y);
+        DLIB_TEST(length(w-x-y) < 1e-12);
+
+        point_transform_affine3d tform;
+        tform = rotate_around_x(pi/2)*rotate_around_z(pi/2)*translate_point(x);
+        DLIB_TEST(length(tform(dlib::vector<double>())-z) < 1e-12);
+        DLIB_TEST(length(inv(tform)(z)) < 1e-12);
+    }
+
 // ----------------------------------------------------------------------------------------
 
     class geometry_tester : public tester
@@ -819,6 +847,7 @@ namespace
         void perform_test (
         )
         {
+            test_affine3d();
             test_rect_to_drect();
             geometry_test();
             test_border_enumerator();