Add new transform types and utility functions (#69)

Author: csparker247

core/include/vc/core/types/Transforms.hpp

@@ -46,6 +46,9 @@ namespace volcart
 class Transform3D
 {
 public:
+    /** @brief Transform type string constant */
+    static constexpr std::string_view TYPE{"Transform3D"};
+
     /** Pointer type */
     using Pointer = std::shared_ptr<Transform3D>;
 
@@ -60,14 +63,16 @@ class Transform3D
     auto operator=(Transform3D&& other) -> Transform3D& = delete;
 
     /** @brief Return a string representation of the transform type */
-    [[nodiscard]] virtual auto type() const -> std::string = 0;
+    [[nodiscard]] virtual auto type() const -> std::string_view = 0;
     /** @brief Clone the transform */
     [[nodiscard]] virtual auto clone() const -> Pointer = 0;
 
     /** @brief Return whether the underlying transform is invertible */
     [[nodiscard]] virtual auto invertible() const -> bool;
     /** @brief Return the inverted transform */
     [[nodiscard]] virtual auto invert() const -> Pointer;
+    /** @brief Return whether the underlying transform is composable */
+    [[nodiscard]] virtual auto composable() const -> bool;
 
     /**
      * @brief Reset the transform parameters
@@ -79,7 +84,7 @@ class Transform3D
      */
     virtual void reset() = 0;
     /** @brief Clears all parameters and properties of the transform */
-    virtual void clear();
+    void clear();
 
     /**
      * @brief Set the identifier for the source space
@@ -134,6 +139,33 @@ class Transform3D
     [[nodiscard]] auto applyPointAndNormal(
         const cv::Vec6d& ptN, bool normalize = true) const -> cv::Vec6d;
 
+    /**
+     * @brief Compose two transforms into a single new transform
+     *
+     * Returns a pair of transform pointers. If the composition fails, the pair
+     * will contain pointers to both of the original inputs. If the second
+     * value in the pair is nullptr, then the composition was successful, and
+     * the new transform is available from the first pointer.
+     *
+     * @code{.cpp}
+     * // Two transforms
+     * Transform3D::Pointer lhs = AffineTransform::New();
+     * Transform3D::Pointer rhs = AffineTransform::New();
+     *
+     * // Compose and assign the results to existing variables
+     * std::tie(lhs, rhs) = Transform3D::Compose(lhs, rhs);
+     *
+     * // Check the result
+     * if(rhs) {
+     *   std::cout << "Failed to compose transforms!\n";
+     * } else {
+     *   std::cout << "Composition successful!\n";
+     * }
+     * @endcode
+     */
+    static auto Compose(const Pointer& lhs, const Pointer& rhs)
+        -> std::pair<Pointer, Pointer>;
+
     /** @brief Save a transform to a JSON file */
     static void Save(const filesystem::path& path, const Pointer& transform);
     /** @brief Load a transform from a JSON file */
@@ -147,8 +179,20 @@ class Transform3D
     /** Only derived classes can copy */
     auto operator=(const Transform3D& other) -> Transform3D& = default;
 
+    /**
+     * Helper compose function. The base implementation returns a nullptr.
+     * Implementing derived classes should set the returned transform's source
+     * to this->source() and the target to rhs->target().
+     */
+    [[nodiscard]] virtual auto compose_(const Transform3D::Pointer& rhs) const
+        -> Transform3D::Pointer;
+
     /** On-disk metadata type */
     using Metadata = nlohmann::ordered_json;
+    /** Serialize the transform to metadata */
+    static auto Serialize(const Pointer& transform) -> Metadata;
+    /** Deserialize the transform from metadata */
+    static auto Deserialize(const Metadata& meta) -> Pointer;
     /** Serialize the derived class parameters */
     virtual void to_meta_(Metadata& meta) = 0;
     /** Deserialize the derived class parameters */
@@ -161,6 +205,18 @@ class Transform3D
     std::string tgt_;
 };
 
+/**
+ * @brief Compose transform convenience operator
+ *
+ * Same as Transform3D::Compose but only returns the composed transform. If
+ * composition fails for any reason, will throw an exception.
+ *
+ * @throws std::invalid_argument if lhs or rhs are not composable
+ * @throws std::runtime_error if transform composition failed
+ */
+auto operator*(const Transform3D::Pointer& lhs, const Transform3D::Pointer& rhs)
+    -> Transform3D::Pointer;
+
 /**
  * @brief 3D affine transform
  *
@@ -169,7 +225,7 @@ class Transform3D
  * transform with the stored affine transform. For example, the following
  * transform will scale, rotate, and translate the 3D point, in that order:
  *
- * @code
+ * @code{.cpp}
  * auto tfm = AffineTransform::New();
  * // scale by 5
  * tfm->scale(5);
@@ -184,6 +240,9 @@ class Transform3D
 class AffineTransform : public Transform3D
 {
 public:
+    /** @copydoc Transform3D::TYPE */
+    static constexpr std::string_view TYPE{"AffineTransform"};
+
     /** Parameters type: 4x4 matrix */
     using Parameters = cv::Matx<double, 4, 4>;
 
@@ -194,17 +253,17 @@ class AffineTransform : public Transform3D
     static auto New() -> Pointer;
 
     /** @copydoc Transform3D::type() */
-    [[nodiscard]] auto type() const -> std::string final;
+    [[nodiscard]] auto type() const -> std::string_view final;
     /** @copydoc Transform3D::clone() */
     [[nodiscard]] auto clone() const -> Transform3D::Pointer final;
     /** @copydoc Transform3D::invertible() */
     [[nodiscard]] auto invertible() const -> bool final;
     /** @copydoc Transform3D::invert() */
     [[nodiscard]] auto invert() const -> Transform3D::Pointer final;
+    /** @copydoc Transform3D::composable() */
+    [[nodiscard]] auto composable() const -> bool final;
     /** @copydoc Transform3D::reset() */
     void reset() final;
-    /** @copydoc Transform3D::clear() */
-    void clear() final;
 
     /** @copydoc Transform3D::applyPoint() */
     [[nodiscard]] auto applyPoint(const cv::Vec3d& point) const
@@ -269,6 +328,161 @@ class AffineTransform : public Transform3D
     AffineTransform() = default;
     /** Current parameters */
     Parameters params_{cv::Matx<double, 4, 4>::eye()};
+    /** @copydoc Transform3D::compose_() */
+    [[nodiscard]] auto compose_(const Transform3D::Pointer& rhs) const
+        -> Transform3D::Pointer final;
+    /** @copydoc Transform3D::to_meta_() */
+    void to_meta_(Metadata& meta) final;
+    /** @copydoc Transform3D::from_meta_() */
+    void from_meta_(const Metadata& meta) final;
+};
+
+/**
+ * @brief Identity transform
+ *
+ * Identity transform that simply returns input parameters. Useful for
+ * creating explicit mappings between a source and a target which share the
+ * same coordinate space.
+ *
+ * @code{.cpp}
+ * auto tfm = IdentityTransform::New();
+ * auto pt = tfm->applyPoint({0, 1, 0}); // {0, 1, 0}
+ * @endcode
+ */
+class IdentityTransform : public Transform3D
+{
+public:
+    /** @copydoc Transform3D::TYPE */
+    static constexpr std::string_view TYPE{"IdentityTransform"};
+
+    /** Pointer type */
+    using Pointer = std::shared_ptr<IdentityTransform>;
+
+    /** @brief Create a new IdentityTransform */
+    static auto New() -> Pointer;
+
+    /** @copydoc Transform3D::type() */
+    [[nodiscard]] auto type() const -> std::string_view final;
+    /** @copydoc Transform3D::clone() */
+    [[nodiscard]] auto clone() const -> Transform3D::Pointer final;
+    /** @copydoc Transform3D::invertible() */
+    [[nodiscard]] auto invertible() const -> bool final;
+    /** @copydoc Transform3D::invert() */
+    [[nodiscard]] auto invert() const -> Transform3D::Pointer final;
+    /** @copydoc Transform3D::composable() */
+    [[nodiscard]] auto composable() const -> bool final;
+    /** @copydoc Transform3D::reset() */
+    void reset() final;
+
+    /** @copydoc Transform3D::applyPoint() */
+    [[nodiscard]] auto applyPoint(const cv::Vec3d& point) const
+        -> cv::Vec3d final;
+    /** @copydoc Transform3D::applyVector() */
+    [[nodiscard]] auto applyVector(const cv::Vec3d& vector) const
+        -> cv::Vec3d final;
+
+private:
+    /** Don't allow construction on the stack */
+    IdentityTransform() = default;
+    /** @copydoc Transform3D::compose_() */
+    [[nodiscard]] auto compose_(const Transform3D::Pointer& rhs) const
+        -> Transform3D::Pointer final;
+    /** @copydoc Transform3D::to_meta_() */
+    void to_meta_(Metadata& meta) final;
+    /** @copydoc Transform3D::from_meta_() */
+    void from_meta_(const Metadata& meta) final;
+};
+
+/**
+ * @brief Collection of transforms
+ *
+ * A convenience class which holds a list of transforms. When transforming
+ * points and vectors, each transform is applied sequentially to the input.
+ *
+ * @code{.cpp}
+ * // New transform
+ * auto tfm = CompositeTransform::New();
+ *
+ * // Add some transforms
+ * auto t = AffineTransform::New();
+ * t->translate(1, 2, 3);
+ * tfm->push_back(t);
+ * t->reset();
+ * t->scale(4);
+ * tfm->push_back(t);
+ *
+ * // Apply all transforms to an input
+ * auto pt = tfm->applyPoint({0, 1, 0}); // {4, 12, 12}
+ * @endcode
+ *
+ * It can often be preferable, for both performance and numerical stability, to
+ * simplify all adjacent, composable transforms (e.g. AffineTransform,
+ * IdentityTransform) into a single transform.
+ *
+ * @code{.cpp}
+ * // Add some composable transforms
+ * tfm->push_back(AffineTransform::New());
+ * tfm->push_back(IdentityTransform::New());
+ * tfm->push_back(AffineTransform::New());
+ * tfm->size(); // 3
+ *
+ * // Simplify the transform
+ * tfm->simplify();
+ * tfm->size(); // 1
+ * @endcode
+ */
+class CompositeTransform : public Transform3D
+{
+public:
+    /** @copydoc Transform3D::TYPE */
+    static constexpr std::string_view TYPE{"CompositeTransform"};
+
+    /** Pointer type */
+    using Pointer = std::shared_ptr<CompositeTransform>;
+
+    /** @brief Create a new CompositeTransform */
+    static auto New() -> Pointer;
+
+    /** @copydoc Transform3D::type() */
+    [[nodiscard]] auto type() const -> std::string_view final;
+    /** @copydoc Transform3D::clone() */
+    [[nodiscard]] auto clone() const -> Transform3D::Pointer final;
+    /** @copydoc Transform3D::reset() */
+    void reset() final;
+
+    /** @copydoc Transform3D::applyPoint() */
+    [[nodiscard]] auto applyPoint(const cv::Vec3d& point) const
+        -> cv::Vec3d final;
+    /** @copydoc Transform3D::applyVector() */
+    [[nodiscard]] auto applyVector(const cv::Vec3d& vector) const
+        -> cv::Vec3d final;
+
+    /**
+     * @brief Add a transform to the end of the composite transform stack
+     *
+     * The transform is cloned before being added to the transform stack. If
+     * the transform is also a CompositeTransform, its transform stack is
+     * expanded and copied to the end of this transform's stack.
+     */
+    void push_back(const Transform3D::Pointer& t);
+
+    /** @brief Get the number of transforms in the composite transform */
+    [[nodiscard]] auto size() const noexcept -> std::size_t;
+
+    /**
+     * @brief Compose all composable transforms
+     *
+     * Simplifies the transform by composing all adjacent, composable
+     * transforms in the composite transform list. This can lead to better
+     * runtime performance and numerical stability for the apply functions.
+     */
+    void simplify();
+
+private:
+    /** Don't allow construction on the stack */
+    CompositeTransform() = default;
+    /** Transform list */
+    std::vector<Transform3D::Pointer> tfms_;
     /** @copydoc Transform3D::to_meta_() */
     void to_meta_(Metadata& meta) final;
     /** @copydoc Transform3D::from_meta_() */