diff --git a/CMakeLists.txt b/CMakeLists.txt
index ff3477c7..a0813441 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -94,10 +94,9 @@ endif()
 # Tests
 option(WITH_TESTS "Build test suite" ON)
 if(WITH_TESTS)
-    enable_testing()
-    set(GOOGLETEST_VERSION 1.10.0)
-    add_subdirectory("${PROJECT_SOURCE_DIR}/vendor/googletest-release-1.10.0/googletest")
-    add_subdirectory(tests)
+  find_package(GTest REQUIRED)
+  enable_testing()
+  add_subdirectory(tests)
 endif()
 
 
diff --git a/cmake/cpd_test.cmake b/cmake/cpd_test.cmake
index 52963336..d7e26d15 100644
--- a/cmake/cpd_test.cmake
+++ b/cmake/cpd_test.cmake
@@ -4,6 +4,6 @@ function(cpd_test name)
     add_executable(${target} ${src})
     set_target_properties(${target} PROPERTIES OUTPUT_NAME ${name})
     add_test(NAME ${name} COMMAND ${target})
-    target_link_libraries(${target} PRIVATE Library-C++ ${ARGN} gtest_main)
+    target_link_libraries(${target} PRIVATE Library-C++ ${ARGN} ${GTEST_LIBRARIES} GTest::gtest_main)
     target_include_directories(${target} PRIVATE "${PROJECT_BINARY_DIR}")
 endfunction()
diff --git a/components/jsoncpp/include/cpd/jsoncpp.hpp b/components/jsoncpp/include/cpd/jsoncpp.hpp
index b173a28a..cf555776 100644
--- a/components/jsoncpp/include/cpd/jsoncpp.hpp
+++ b/components/jsoncpp/include/cpd/jsoncpp.hpp
@@ -30,4 +30,4 @@ Json::Value to_json(const RigidResult& result);
 Json::Value to_json(const AffineResult& result);
 Json::Value to_json(const NonrigidResult& result);
 Json::Value to_json(const Matrix& matrix);
-}
+} // namespace cpd
diff --git a/components/jsoncpp/src/jsoncpp.cpp b/components/jsoncpp/src/jsoncpp.cpp
index c1638410..9acf0251 100644
--- a/components/jsoncpp/src/jsoncpp.cpp
+++ b/components/jsoncpp/src/jsoncpp.cpp
@@ -64,4 +64,4 @@ Json::Value to_json(const Matrix& matrix) {
 std::ostream& operator<<(std::ostream& ostream, const Result& result) {
     return ostream;
 }
-}
+} // namespace cpd
diff --git a/components/jsoncpp/tests/jsoncpp.cpp b/components/jsoncpp/tests/jsoncpp.cpp
index 02d80898..1a6e4e07 100644
--- a/components/jsoncpp/tests/jsoncpp.cpp
+++ b/components/jsoncpp/tests/jsoncpp.cpp
@@ -25,4 +25,4 @@ TEST(RigidResult, ConvertsToJson) {
     RigidResult result;
     Json::Value json = cpd::to_json(result);
 }
-}
+} // namespace cpd
diff --git a/examples/callback.cpp b/examples/callback.cpp
index 1a7a704f..70d44b87 100644
--- a/examples/callback.cpp
+++ b/examples/callback.cpp
@@ -4,12 +4,12 @@
 #include <cpd/nonrigid.hpp>
 #include <cpd/rigid.hpp>
 
-void RigidCallback(const cpd::Result &r) {
-  std::cout << r.points << std::endl << std::endl;
+void RigidCallback(const cpd::Result& r) {
+    std::cout << r.points << std::endl << std::endl;
 }
 
-void NonrigidCallback(const cpd::NonrigidResult &r) {
-  std::cout << r.points << std::endl << std::endl;
+void NonrigidCallback(const cpd::NonrigidResult& r) {
+    std::cout << r.points << std::endl << std::endl;
 }
 
 int main(int argc, char** argv) {
@@ -26,12 +26,12 @@ int main(int argc, char** argv) {
 
     if (method == "rigid") {
         cpd::Rigid rigid;
-        auto *cb = RigidCallback;
+        auto* cb = RigidCallback;
         rigid.add_callback(cb);
         auto rigid_result = rigid.run(fixed, moving);
     } else if (method == "nonrigid") {
         cpd::Nonrigid nonrigid;
-        auto *cb = NonrigidCallback;
+        auto* cb = NonrigidCallback;
         nonrigid.add_callback(cb);
         auto nonrigid_result = nonrigid.run(fixed, moving);
     } else {
@@ -41,5 +41,3 @@ int main(int argc, char** argv) {
     std::cout << "Registration completed OK" << std::endl;
     return 0;
 }
-
-
diff --git a/include/cpd/affine.hpp b/include/cpd/affine.hpp
index 4a50639f..a6148f7d 100644
--- a/include/cpd/affine.hpp
+++ b/include/cpd/affine.hpp
@@ -26,31 +26,34 @@
 namespace cpd {
 
 /// The result of a affine coherent point drift run.
-struct AffineResult : public Result {
+template <typename M, typename V>
+class AffineResult : public Result<M, V> {
+public:
     /// The affine transformation.
-    Matrix transform;
+    M transform;
 
     /// The translation vector.
-    Vector translation;
+    V translation;
 
     /// Returns the transform and the translation as one matrix.
-    Matrix matrix() const;
+    M matrix() const;
 
     /// Denormalize this result.
-    void denormalize(const Normalization& normalization);
+    void denormalize(const Normalization<M, V>& normalization);
 };
 
 /// Affine coherent point drift.
-class Affine : public Transform<AffineResult> {
+template <typename M, typename V>
+class Affine : public Transform<M, V, AffineResult> {
 public:
     Affine()
-      : Transform()
+      : Transform<M, V, AffineResult>()
       , m_linked(DEFAULT_LINKED) {}
 
     /// Computes one iteration of the affine transformation.
-    AffineResult compute_one(const Matrix& fixed, const Matrix& moving,
-                             const Probabilities& probabilities,
-                             double sigma2) const;
+    AffineResult<M, V> compute_one(const M& fixed, const M& moving,
+                                   const Probabilities<M, V>& probabilities,
+                                   typename M::Scalar sigma2) const;
 
     /// Sets whether the scalings of the two datasets are linked.
     Affine& linked(bool linked) {
@@ -65,5 +68,6 @@ class Affine : public Transform<AffineResult> {
 };
 
 /// Runs a affine registration on two matrices.
-AffineResult affine(const Matrix& fixed, const Matrix& moving);
+template <typename M, typename V>
+AffineResult<M, V> affine(const M& fixed, const M& moving);
 } // namespace cpd
diff --git a/include/cpd/gauss_transform.hpp b/include/cpd/gauss_transform.hpp
index 6960f1b2..f1117ed6 100644
--- a/include/cpd/gauss_transform.hpp
+++ b/include/cpd/gauss_transform.hpp
@@ -29,36 +29,41 @@ namespace cpd {
 
 /// Probability matrices produced by comparing two data sets with a
 /// `GaussTransform`.
+template <typename M, typename V>
 struct Probabilities {
     /// The probability matrix, multiplied by the identity matrix.
-    Vector p1;
+    V p1;
     /// The probability matrix, transposes, multiplied by the identity matrix.
-    Vector pt1;
+    V pt1;
     /// The probability matrix multiplied by the fixed points.
-    Matrix px;
+    M px;
     /// The total error.
-    double l;
+    typename M::Scalar l;
     /// The correspondence vector between the two datasets.
     IndexVector correspondence;
 };
 
 /// Abstract base class for Gauss transforms.
+template <typename M, typename V>
 class GaussTransform {
 public:
     /// Returns the default Gauss transform as a unique ptr.
-    static std::unique_ptr<GaussTransform> make_default();
+    static std::unique_ptr<GaussTransform<M, V>> make_default();
 
     /// Computes the Gauss transform.
-    virtual Probabilities compute(const Matrix& fixed, const Matrix& moving,
-                                  double sigma2, double outliers) const = 0;
+    virtual Probabilities<M, V> compute(const M& fixed, const M& moving,
+                                        typename M::Scalar sigma2,
+                                        typename M::Scalar outliers) const = 0;
 
     virtual ~GaussTransform() {}
 };
 
 /// The direct Gauss transform.
-class GaussTransformDirect : public GaussTransform {
+template <typename M, typename V>
+class GaussTransformDirect : public GaussTransform<M, V> {
 public:
-    Probabilities compute(const Matrix& fixed, const Matrix& moving,
-                          double sigma2, double outliers) const;
+    Probabilities<M, V> compute(const M& fixed, const M& moving,
+                                typename M::Scalar sigma2,
+                                typename M::Scalar outliers) const;
 };
 } // namespace cpd
diff --git a/include/cpd/gauss_transform_fgt.hpp b/include/cpd/gauss_transform_fgt.hpp
index d303db34..8d2afdd5 100644
--- a/include/cpd/gauss_transform_fgt.hpp
+++ b/include/cpd/gauss_transform_fgt.hpp
@@ -39,12 +39,13 @@ enum FgtMethod {
 /// The default fgt method
 const FgtMethod DEFAULT_FGT_METHOD = FgtMethod::DirectTree;
 /// The default switched fgt breakpoint.
-const double DEFAULT_BREAKPOINT = 0.2;
+const typename M::Scalar DEFAULT_BREAKPOINT = 0.2;
 /// The default fgt epsilon.
-const double DEFAULT_EPSILON = 1e-4;
+const typename M::Scalar DEFAULT_EPSILON = 1e-4;
 
 /// The Gauss transform using the fgt library.
-class GaussTransformFgt : public GaussTransform {
+template <typename M, typename V>
+class GaussTransformFgt : public GaussTransform<M, V> {
 public:
     GaussTransformFgt()
       : GaussTransform()
@@ -53,13 +54,13 @@ class GaussTransformFgt : public GaussTransform {
       , m_method(DEFAULT_FGT_METHOD) {}
 
     /// Sets the ifgt->direct-tree breakpoint.
-    GaussTransformFgt& breakpoint(double breakpoint) {
+    GaussTransformFgt& breakpoint(typename M::Scalar breakpoint) {
         m_breakpoint = breakpoint;
         return *this;
     }
 
     /// Sets the epsilon.
-    GaussTransformFgt& epsilon(double epsilon) {
+    GaussTransformFgt& epsilon(typename M::Scalar epsilon) {
         m_epsilon = epsilon;
         return *this;
     }
@@ -70,15 +71,16 @@ class GaussTransformFgt : public GaussTransform {
         return *this;
     }
 
-    Probabilities compute(const Matrix& fixed, const Matrix& moving,
-                          double sigma2, double outliers) const;
+    Probabilities compute(const M& fixed, const M& moving,
+                          typename M::Scalar sigma2,
+                          typename M::Scalar outliers) const;
 
 private:
-    std::unique_ptr<fgt::Transform> create_transform(const Matrix& points,
-                                                     double bandwidth) const;
+    std::unique_ptr<fgt::Transform> create_transform(
+        const M& points, typename M::Scalar bandwidth) const;
 
-    double m_breakpoint;
-    double m_epsilon;
+    typename M::Scalar m_breakpoint;
+    typename M::Scalar m_epsilon;
     FgtMethod m_method;
 };
 } // namespace cpd
diff --git a/include/cpd/matrix.hpp b/include/cpd/matrix.hpp
index bd3c5ff9..1180f6f5 100644
--- a/include/cpd/matrix.hpp
+++ b/include/cpd/matrix.hpp
@@ -27,18 +27,22 @@ namespace cpd {
 
 /// Our base matrix class.
 typedef Eigen::MatrixXd Matrix;
+typedef Eigen::MatrixXf MatrixF;
 
 /// Typedef for our specific type of vector.
 typedef Eigen::VectorXd Vector;
+typedef Eigen::VectorXf VectorF;
+
+/// Typedef for our specific type of array. (TODO: Support this)
+typedef Eigen::ArrayXd Array;
+typedef Eigen::ArrayXf ArrayF;
 
 /// Typedef for an index vector, used to index other matrices.
 typedef Eigen::Matrix<Matrix::Index, Eigen::Dynamic, 1> IndexVector;
 
-/// Typedef for our specific type of array.
-typedef Eigen::ArrayXd Array;
-
 /// Apply a transformation matrix to a set of points.
 ///
-/// The transformation matrix should be one column wider than the point matrix.
-Matrix apply_transformation_matrix(Matrix points, const Matrix& transform);
+/// The transformation matrix should be one column wider than the point matrix
+template <typename M, typename V>
+M apply_transformation_matrix(M points, const M& transform);
 } // namespace cpd
diff --git a/include/cpd/nonrigid.hpp b/include/cpd/nonrigid.hpp
index 6ab02f42..045dad5d 100644
--- a/include/cpd/nonrigid.hpp
+++ b/include/cpd/nonrigid.hpp
@@ -26,47 +26,49 @@
 namespace cpd {
 
 /// Default value for beta.
-const double DEFAULT_BETA = 3.0;
+const Matrix::Scalar DEFAULT_BETA = 3.0;
 /// Default value for lambda.
-const double DEFAULT_LAMBDA = 3.0;
+const Matrix::Scalar DEFAULT_LAMBDA = 3.0;
 
 /// The result of a nonrigid coherent point drift run.
-struct NonrigidResult : public Result {};
+template <typename M, typename V>
+class NonrigidResult : public Result<M, V> {};
 
 /// Nonrigid coherent point drift.
-class Nonrigid : public Transform<NonrigidResult> {
+template <typename M, typename V>
+class Nonrigid : public Transform<M, V, NonrigidResult> {
 public:
     Nonrigid()
-      : Transform()
+      : Transform<M, V, NonrigidResult>()
       , m_lambda(DEFAULT_LAMBDA)
       , m_beta(DEFAULT_BETA)
       , m_linked(DEFAULT_LINKED) {}
 
     /// Initialize this transform for the provided matrices.
-    void init(const Matrix& fixed, const Matrix& moving);
+    void init(const M& fixed, const M& moving);
 
     /// Modifies the probabilities with some affinity and weight information.
-    void modify_probabilities(Probabilities& probabilities) const;
+    void modify_probabilities(Probabilities<M, V>& probabilities) const;
 
     /// Sets the beta.
-    Nonrigid& beta(double beta) {
+    Nonrigid<M, V>& beta(double beta) {
         m_beta = beta;
         return *this;
     }
 
     /// Sets the lambda.
-    Nonrigid& lambda(double lambda) {
+    Nonrigid<M, V>& lambda(double lambda) {
         m_lambda = lambda;
         return *this;
     }
 
     /// Computes one iteration of the nonrigid transformation.
-    NonrigidResult compute_one(const Matrix& fixed, const Matrix& moving,
-                               const Probabilities& probabilities,
-                               double sigma2) const;
+    NonrigidResult<M, V> compute_one(const M& fixed, const M& moving,
+                                     const Probabilities<M, V>& probabilities,
+                                     typename M::Scalar sigma2) const;
 
     /// Sets whether the scalings of the two datasets are linked.
-    Nonrigid& linked(bool linked) {
+    Nonrigid<M, V>& linked(bool linked) {
         m_linked = linked;
         return *this;
     }
@@ -74,13 +76,14 @@ class Nonrigid : public Transform<NonrigidResult> {
     virtual bool linked() const { return m_linked; }
 
 private:
-    Matrix m_g;
-    Matrix m_w;
+    M m_g;
+    M m_w;
     double m_lambda;
     double m_beta;
     bool m_linked;
 };
 
 /// Runs a nonrigid registration on two matrices.
-NonrigidResult nonrigid(const Matrix& fixed, const Matrix& moving);
+template <typename M, typename V>
+NonrigidResult<M, V> nonrigid(const M& fixed, const M& moving);
 } // namespace cpd
diff --git a/include/cpd/normalization.hpp b/include/cpd/normalization.hpp
index 28033a7d..16edaad9 100644
--- a/include/cpd/normalization.hpp
+++ b/include/cpd/normalization.hpp
@@ -27,19 +27,20 @@
 namespace cpd {
 
 /// The results of normalizing data to a unit cube (or whatever dimensionality).
+template <typename M, typename V>
 struct Normalization {
     /// The average of the fixed points, that was subtracted from those data.
-    Vector fixed_mean;
+    V fixed_mean;
     /// The fixed points.
-    Matrix fixed;
+    M fixed;
     /// The scaling factor for the fixed points.
-    double fixed_scale;
+    typename M::Scalar fixed_scale;
     /// The average of the moving points, that was subtracted from those data.
-    Vector moving_mean;
+    V moving_mean;
     /// The moving points.
-    Matrix moving;
+    M moving;
     /// The scaling factor for the moving points.
-    double moving_scale;
+    typename M::Scalar moving_scale;
 
     /// Creates a new normalization for the provided matrices.
     ///
@@ -49,7 +50,6 @@ struct Normalization {
     /// seperately.
     ///
     /// Myronenko's original implementation only had `linked = false` logic.
-    Normalization(const Matrix& fixed, const Matrix& moving,
-                  bool linked = true);
+    Normalization(const M& fixed, const M& moving, bool linked = true);
 };
 } // namespace cpd
diff --git a/include/cpd/rigid.hpp b/include/cpd/rigid.hpp
index a443c2e2..2a8524b7 100644
--- a/include/cpd/rigid.hpp
+++ b/include/cpd/rigid.hpp
@@ -31,28 +31,30 @@ const bool DEFAULT_REFLECTIONS = false;
 const bool DEFAULT_SCALE = !DEFAULT_LINKED;
 
 /// The result of a rigid coherent point drift run.
-struct RigidResult : public Result {
+template <typename M, typename V>
+struct RigidResult : public Result<M, V> {
     /// The rotation component of the transformation.
-    Matrix rotation;
+    M rotation;
     /// The translation component of the transformation.
-    Vector translation;
+    V translation;
     /// The scaling component of the transformation.
     double scale;
 
     /// Returns a single matrix that contains all the transformation
     /// information.
-    Matrix matrix() const;
+    M matrix() const;
 
-    void denormalize(const Normalization& normalization);
+    void denormalize(const Normalization<M, V>& normalization);
 };
 
 /// Rigid coherent point drift.
 ///
 /// Scaling and reflections can be turned on and off.
-class Rigid : public Transform<RigidResult> {
+template <typename M, typename V>
+class Rigid : public Transform<M, V, RigidResult> {
 public:
     Rigid()
-      : Transform()
+      : Transform<M, V, RigidResult>()
       , m_reflections(DEFAULT_REFLECTIONS)
       , m_scale(DEFAULT_SCALE) {}
 
@@ -69,9 +71,9 @@ class Rigid : public Transform<RigidResult> {
     }
 
     /// Computes one iteration of the rigid transformation.
-    RigidResult compute_one(const Matrix& fixed, const Matrix& moving,
-                            const Probabilities& probabilities,
-                            double sigma2) const;
+    RigidResult<M, V> compute_one(const M& fixed, const M& moving,
+                                  const Probabilities<M, V>& probabilities,
+                                  typename M::Scalar sigma2) const;
 
     virtual bool linked() const { return !m_scale; }
 
@@ -81,5 +83,6 @@ class Rigid : public Transform<RigidResult> {
 };
 
 /// Runs a rigid registration on two matrices.
-RigidResult rigid(const Matrix& fixed, const Matrix& moving);
+template <typename M, typename V>
+RigidResult<M, V> rigid(const M& fixed, const M& moving);
 } // namespace cpd
diff --git a/include/cpd/transform.hpp b/include/cpd/transform.hpp
index 7cdcd3e4..8a3d8830 100644
--- a/include/cpd/transform.hpp
+++ b/include/cpd/transform.hpp
@@ -25,8 +25,8 @@
 #pragma once
 
 #include <chrono>
-#include <memory>
 #include <functional>
+#include <memory>
 #include <vector>
 
 #include <cpd/gauss_transform.hpp>
@@ -37,26 +37,28 @@
 namespace cpd {
 
 /// The default number of iterations allowed.
-const size_t DEFAULT_MAX_ITERATIONS = 150;
+const size_t DEFAULT_MAX_ITERATIONS = 30;
 /// Whether points should be normalized by default.
 const bool DEFAULT_NORMALIZE = true;
 /// The default outlier weight.
-const double DEFAULT_OUTLIERS = 0.1;
+const Matrix::Scalar DEFAULT_OUTLIERS = 0.1;
 /// The default tolerance.
-const double DEFAULT_TOLERANCE = 1e-5;
+const Matrix::Scalar DEFAULT_TOLERANCE = 1e-5;
 /// The default sigma2.
-const double DEFAULT_SIGMA2 = 0.0;
+const Matrix::Scalar DEFAULT_SIGMA2 = 0.0;
 /// Whether correspondence vector should be computed by default.
 const bool DEFAULT_CORRESPONDENCE = false;
 /// Are the scalings of the two datasets linked by default?
 const bool DEFAULT_LINKED = true;
 
 /// The result of a generic transform run.
-struct Result {
+template <typename M, typename V>
+class Result {
+public:
     /// The final moved points.
-    Matrix points;
+    M points;
     /// The final sigma2 value.
-    double sigma2;
+    typename M::Scalar sigma2;
     /// The correspondence vector.
     IndexVector correspondence;
     /// The runtime.
@@ -68,25 +70,25 @@ struct Result {
     ///
     /// Generally, this scales the points back, and sometimes adjust transforms
     /// or shifts or the like.
-    virtual void denormalize(const Normalization& normalization);
+    virtual void denormalize(const Normalization<M, V>& normalization);
 };
 
 /// Generic coherent point drift transform.
 ///
 /// An abstract base class for real transforms, e.g. `Rigid` or `Nonrigid`.
-template <typename Result>
+template <typename M, typename V, template <typename, typename> class Result>
 class Transform {
 public:
     Transform()
       : m_correspondence(DEFAULT_CORRESPONDENCE)
-      , m_gauss_transform(GaussTransform::make_default())
+      , m_gauss_transform(GaussTransform<M, V>::make_default())
       , m_max_iterations(DEFAULT_MAX_ITERATIONS)
       , m_normalize(DEFAULT_NORMALIZE)
       , m_outliers(DEFAULT_OUTLIERS)
       , m_sigma2(DEFAULT_SIGMA2)
       , m_tolerance(DEFAULT_TOLERANCE) {}
 
-    using TCallback = std::function<void(const Result&)>;
+    using TCallback = std::function<void(const Result<M, V>&)>;
     using TCallbackVector = std::vector<TCallback>;
 
     /// Add a callback (function pointer, member function, or lambda).
@@ -103,7 +105,7 @@ class Transform {
 
     /// Sets the gauss transform.
     Transform& gauss_transform(
-        std::unique_ptr<GaussTransform> gauss_transform) {
+        std::unique_ptr<GaussTransform<M, V>> gauss_transform) {
         m_gauss_transform = std::move(gauss_transform);
         return *this;
     }
@@ -138,10 +140,59 @@ class Transform {
         return *this;
     }
 
+    Result<M, V> constRun(const M& fixed, const M& moving) {
+        auto tic = std::chrono::high_resolution_clock::now();
+        Normalization<M, V> normalization(fixed, moving, linked());
+
+        this->init(fixed, moving);
+
+        Result<M, V> result;
+        result.points = moving;
+        if (m_sigma2 == 0.0) {
+            result.sigma2 = cpd::default_sigma2(fixed, moving);
+        } else {
+            result.sigma2 = m_sigma2;
+        }
+
+        size_t iter = 0;
+        typename M::Scalar ntol = m_tolerance + 10.0;
+        typename M::Scalar l = 0.;
+        while (iter < m_max_iterations && ntol > m_tolerance &&
+               result.sigma2 >
+                   10 * std::numeric_limits<typename M::Scalar>::epsilon()) {
+            Probabilities<M, V> probabilities = m_gauss_transform->compute(
+                fixed, result.points, result.sigma2, m_outliers);
+            this->modify_probabilities(probabilities);
+
+            ntol = std::abs((probabilities.l - l) / probabilities.l);
+            l = probabilities.l;
+
+            result =
+                this->compute_one(fixed, moving, probabilities, result.sigma2);
+            for (const auto& cb : this->m_callbacks) {
+                cb(result);
+            }
+            ++iter;
+        }
+
+        if (m_correspondence) {
+            GaussTransformDirect<M, V> direct;
+            Probabilities<M, V> probabilities =
+                direct.compute(fixed, result.points, result.sigma2, m_outliers);
+            result.correspondence = probabilities.correspondence;
+            assert(result.correspondence.rows() > 0);
+        }
+        auto toc = std::chrono::high_resolution_clock::now();
+        result.runtime =
+            std::chrono::duration_cast<std::chrono::microseconds>(toc - tic);
+        result.iterations = iter;
+        return result;
+    }
+
     /// Runs this transform for the provided matrices.
-    Result run(Matrix fixed, Matrix moving) {
+    Result<M, V> run(M fixed, M moving) {
         auto tic = std::chrono::high_resolution_clock::now();
-        Normalization normalization(fixed, moving, linked());
+        Normalization<M, V> normalization(fixed, moving, linked());
         if (m_normalize) {
             fixed = normalization.fixed;
             moving = normalization.moving;
@@ -149,7 +200,7 @@ class Transform {
 
         this->init(fixed, moving);
 
-        Result result;
+        Result<M, V> result;
         result.points = moving;
         if (m_sigma2 == 0.0) {
             result.sigma2 = cpd::default_sigma2(fixed, moving);
@@ -160,11 +211,12 @@ class Transform {
         }
 
         size_t iter = 0;
-        double ntol = m_tolerance + 10.0;
-        double l = 0.;
+        typename M::Scalar ntol = m_tolerance + 10.0;
+        typename M::Scalar l = 0.;
         while (iter < m_max_iterations && ntol > m_tolerance &&
-               result.sigma2 > 10 * std::numeric_limits<double>::epsilon()) {
-            Probabilities probabilities = m_gauss_transform->compute(
+               result.sigma2 >
+                   10 * std::numeric_limits<typename M::Scalar>::epsilon()) {
+            Probabilities<M, V> probabilities = m_gauss_transform->compute(
                 fixed, result.points, result.sigma2, m_outliers);
             this->modify_probabilities(probabilities);
 
@@ -173,7 +225,7 @@ class Transform {
 
             result =
                 this->compute_one(fixed, moving, probabilities, result.sigma2);
-            for (const auto &cb : this->m_callbacks) {
+            for (const auto& cb : this->m_callbacks) {
                 cb(result);
             }
             ++iter;
@@ -183,9 +235,9 @@ class Transform {
             result.denormalize(normalization);
         }
         if (m_correspondence) {
-            GaussTransformDirect direct;
-            Probabilities probabilities = direct.compute(
-                fixed, result.points, result.sigma2, m_outliers);
+            GaussTransformDirect<M, V> direct;
+            Probabilities<M, V> probabilities =
+                direct.compute(fixed, result.points, result.sigma2, m_outliers);
             result.correspondence = probabilities.correspondence;
             assert(result.correspondence.rows() > 0);
         }
@@ -201,18 +253,19 @@ class Transform {
     /// This is called before beginning each run, but after normalization. In
     /// general, transforms shouldn't need to be initialized, but the nonrigid
     /// does.
-    virtual void init(const Matrix& fixed, const Matrix& moving) {}
+    virtual void init(const M& fixed, const M& moving) {}
 
     /// Modifies `Probabilities` in some way.
     ///
     /// Some types of transform need to tweak the probabilities before moving on
     /// with an interation. The default behavior is to do nothing.
-    virtual void modify_probabilities(Probabilities& probabilities) const {}
+    virtual void modify_probabilities(
+        Probabilities<M, V>& probabilities) const {}
 
     /// Computes one iteration of the transform.
-    virtual Result compute_one(const Matrix& fixed, const Matrix& moving,
-                               const Probabilities& probabilities,
-                               double sigma2) const = 0;
+    virtual Result<M, V> compute_one(const M& fixed, const M& moving,
+                                     const Probabilities<M, V>& probabilities,
+                                     typename M::Scalar sigma2) const = 0;
 
     /// Returns true if the normalization should be linked.
     ///
@@ -221,12 +274,12 @@ class Transform {
 
 private:
     bool m_correspondence;
-    std::unique_ptr<GaussTransform> m_gauss_transform;
+    std::unique_ptr<GaussTransform<M, V>> m_gauss_transform;
     size_t m_max_iterations;
     bool m_normalize;
-    double m_outliers;
-    double m_sigma2;
-    double m_tolerance;
+    typename M::Scalar m_outliers;
+    typename M::Scalar m_sigma2;
+    typename M::Scalar m_tolerance;
     TCallbackVector m_callbacks;
 };
 } // namespace cpd
diff --git a/include/cpd/utils.hpp b/include/cpd/utils.hpp
index 05d580ac..13374a28 100644
--- a/include/cpd/utils.hpp
+++ b/include/cpd/utils.hpp
@@ -26,11 +26,16 @@
 namespace cpd {
 
 /// Loads a matrix from a delimited text file.
-Matrix matrix_from_path(const std::string& path);
+template <typename M>
+M matrix_from_path(const std::string& path);
 
 /// Computes the default sigma2 for the given matrices.
-double default_sigma2(const Matrix& fixed, const Matrix& moving);
+template <typename M>
+typename M::Scalar default_sigma2(const M& fixed, const M& moving);
+
+// typedef decltype(m1*m2)::Scalar ResScalar;
 
 /// Computes the affinity matrix between the two matrices.
-Matrix affinity(const Matrix& fixed, const Matrix& moving, double beta);
+template <typename M>
+M affinity(const M& fixed, const M& moving, typename M::Scalar beta);
 } // namespace cpd
diff --git a/src/affine.cpp b/src/affine.cpp
index 171573b3..20bcd607 100644
--- a/src/affine.cpp
+++ b/src/affine.cpp
@@ -19,21 +19,21 @@
 
 namespace cpd {
 
-AffineResult Affine::compute_one(const Matrix& fixed, const Matrix& moving,
-                                 const Probabilities& probabilities,
-                                 double sigma2) const {
-    double np = probabilities.p1.sum();
-    Vector mu_x = fixed.transpose() * probabilities.pt1 / np;
-    Vector mu_y = moving.transpose() * probabilities.p1 / np;
-    Matrix b1 =
-        probabilities.px.transpose() * moving - np * mu_x * mu_y.transpose();
-    Matrix b2 =
+template <typename M, typename V>
+AffineResult<M, V> Affine<M, V>::compute_one(
+    const M& fixed, const M& moving, const Probabilities<M, V>& probabilities,
+    typename M::Scalar sigma2) const {
+    typename M::Scalar np = probabilities.p1.sum();
+    V mu_x = fixed.transpose() * probabilities.pt1 / np;
+    V mu_y = moving.transpose() * probabilities.p1 / np;
+    M b1 = probabilities.px.transpose() * moving - np * mu_x * mu_y.transpose();
+    M b2 =
         (moving.array() * probabilities.p1.replicate(1, moving.cols()).array())
                 .transpose()
                 .matrix() *
             moving -
         np * mu_y * mu_y.transpose();
-    AffineResult result;
+    AffineResult<M, V> result;
     result.transform = b1 * b2.inverse();
     result.translation = mu_x - result.transform * mu_y;
     result.sigma2 =
@@ -48,12 +48,13 @@ AffineResult Affine::compute_one(const Matrix& fixed, const Matrix& moving,
     return result;
 }
 
-Matrix AffineResult::matrix() const {
-    Matrix::Index rows = transform.rows() + 1;
-    Matrix::Index cols = transform.cols() + 1;
-    Matrix matrix = Matrix::Zero(rows, cols);
-    for (Matrix::Index row = 0; row < transform.rows(); ++row) {
-        for (Matrix::Index col = 0; col < transform.cols(); ++col) {
+template <typename M, typename V>
+M AffineResult<M, V>::matrix() const {
+    typename M::Index rows = transform.rows() + 1;
+    typename M::Index cols = transform.cols() + 1;
+    M matrix = M::Zero(rows, cols);
+    for (typename M::Index row = 0; row < transform.rows(); ++row) {
+        for (typename M::Index col = 0; col < transform.cols(); ++col) {
             matrix(row, col) = transform(row, col);
         }
         matrix(row, cols - 1) = translation(row);
@@ -62,15 +63,29 @@ Matrix AffineResult::matrix() const {
     return matrix;
 }
 
-void AffineResult::denormalize(const Normalization& normalization) {
-    Result::denormalize(normalization);
-    translation = normalization.fixed_scale * translation + normalization.fixed_mean -
+template <typename M, typename V>
+void AffineResult<M, V>::denormalize(const Normalization<M, V>& normalization) {
+    Result<M, V>::denormalize(normalization);
+    translation = normalization.fixed_scale * translation +
+                  normalization.fixed_mean -
                   transform * normalization.moving_mean;
-    transform = transform * normalization.fixed_scale / normalization.moving_scale;
+    transform =
+        transform * normalization.fixed_scale / normalization.moving_scale;
 }
 
-AffineResult affine(const Matrix& fixed, const Matrix& moving) {
-    Affine affine;
+template <typename M, typename V>
+AffineResult<M, V> affine(const M& fixed, const M& moving) {
+    Affine<M, V> affine;
     return affine.run(fixed, moving);
 }
+
+template class Affine<Matrix, Vector>;
+template class AffineResult<Matrix, Vector>;
+template AffineResult<Matrix, Vector> affine(const Matrix& fixed,
+                                             const Matrix& moving);
+
+template class Affine<MatrixF, VectorF>;
+template class AffineResult<MatrixF, VectorF>;
+template AffineResult<MatrixF, VectorF> affine(const MatrixF& fixed,
+                                               const MatrixF& moving);
 } // namespace cpd
diff --git a/src/gauss_transform.cpp b/src/gauss_transform.cpp
index 20ad7b27..e2ca2a95 100644
--- a/src/gauss_transform.cpp
+++ b/src/gauss_transform.cpp
@@ -17,36 +17,38 @@
 
 #define _USE_MATH_DEFINES
 
-#include <cpd/gauss_transform.hpp>
 #include <cmath>
+#include <cpd/gauss_transform.hpp>
 
 namespace cpd {
-Probabilities GaussTransformDirect::compute(const Matrix& fixed,
-                                            const Matrix& moving, double sigma2,
-                                            double outliers) const {
-    double ksig = -2.0 * sigma2;
+template <typename M, typename V>
+Probabilities<M, V> GaussTransformDirect<M, V>::compute(
+    const M& fixed, const M& moving, typename M::Scalar sigma2,
+    typename M::Scalar outliers) const {
+    typename M::Scalar ksig = -2.0 * sigma2;
     size_t cols = fixed.cols();
-    double outlier_tmp =
+    typename M::Scalar outlier_tmp =
         (outliers * moving.rows() * std::pow(-ksig * M_PI, 0.5 * cols)) /
         ((1 - outliers) * fixed.rows());
-    Vector p = Vector::Zero(moving.rows());
-    Vector p1 = Vector::Zero(moving.rows());
-    Vector p1_max = Vector::Zero(moving.rows());
-    Vector pt1 = Vector::Zero(fixed.rows());
-    Matrix px = Matrix::Zero(moving.rows(), cols);
+    V p = V::Zero(moving.rows());
+    V p1 = V::Zero(moving.rows());
+    V p1_max = V::Zero(moving.rows());
+    V pt1 = V::Zero(fixed.rows());
+    M px = M::Zero(moving.rows(), cols);
     IndexVector correspondence = IndexVector::Zero(moving.rows());
-    double l = 0.0;
+    typename M::Scalar l = 0.0;
 
-    for (Matrix::Index i = 0; i < fixed.rows(); ++i) {
-        double sp = 0;
-        for (Matrix::Index j = 0; j < moving.rows(); ++j) {
-            double razn = (fixed.row(i) - moving.row(j)).array().pow(2).sum();
+    for (typename M::Index i = 0; i < fixed.rows(); ++i) {
+        typename M::Scalar sp = 0;
+        for (typename M::Index j = 0; j < moving.rows(); ++j) {
+            typename M::Scalar razn =
+                (fixed.row(i) - moving.row(j)).array().pow(2).sum();
             p(j) = std::exp(razn / ksig);
             sp += p(j);
         }
         sp += outlier_tmp;
         pt1(i) = 1 - outlier_tmp / sp;
-        for (Matrix::Index j = 0; j < moving.rows(); ++j) {
+        for (typename M::Index j = 0; j < moving.rows(); ++j) {
             p1(j) += p(j) / sp;
             px.row(j) += fixed.row(i) * p(j) / sp;
             if (p(j) / sp > p1_max(j)) {
@@ -59,4 +61,14 @@ Probabilities GaussTransformDirect::compute(const Matrix& fixed,
     l += cols * fixed.rows() * std::log(sigma2) / 2;
     return { p1, pt1, px, l, correspondence };
 }
+template struct Probabilities<Matrix, Vector>;
+template class GaussTransform<Matrix, Vector>;
+template class GaussTransformDirect<Matrix, Vector>;
+std::unique_ptr<GaussTransform<Matrix, Vector>> dummy;
+
+template struct Probabilities<MatrixF, VectorF>;
+template class GaussTransform<MatrixF, VectorF>;
+template class GaussTransformDirect<MatrixF, VectorF>;
+std::unique_ptr<GaussTransform<MatrixF, VectorF>> dummyF;
+
 } // namespace cpd
diff --git a/src/gauss_transform_fgt.cpp b/src/gauss_transform_fgt.cpp
index cb8bf5f8..0617a23e 100644
--- a/src/gauss_transform_fgt.cpp
+++ b/src/gauss_transform_fgt.cpp
@@ -17,39 +17,43 @@
 
 #define _USE_MATH_DEFINES
 
-#include <cpd/gauss_transform_fgt.hpp>
 #include <cmath>
+#include <cpd/gauss_transform_fgt.hpp>
 
 namespace cpd {
-std::unique_ptr<GaussTransform> GaussTransform::make_default() {
-    return std::unique_ptr<GaussTransform>(new GaussTransformFgt());
+template <typename M, typename V, typename A>
+std::unique_ptr<GaussTransform<M, V>> GaussTransform<M, V>::make_default() {
+    return std::unique_ptr<GaussTransform>(new GaussTransformFgt<M, V>());
 }
 
-Probabilities GaussTransformFgt::compute(const Matrix& fixed,
-                                         const Matrix& moving, double sigma2,
-                                         double outliers) const {
-    double bandwidth = std::sqrt(2.0 * sigma2);
-    size_t cols = fixed.cols();
+template <typename M, typename V, typename A>
+Probabilities GaussTransformFgt<M, V, A>::compute(
+    const M& fixed, const M& moving, typename M::Scalar sigma2,
+    typename M::Scalar outliers) const {
+    typename M::Scalar bandwidth = std::sqrt(2.0 * sigma2);
+    typename M::Index cols = fixed.cols();
     std::unique_ptr<fgt::Transform> transform =
         create_transform(moving, bandwidth);
     auto kt1 = transform->compute(fixed);
-    double ndi = outliers / (1.0 - outliers) * moving.rows() / fixed.rows() *
-                 std::pow(2.0 * M_PI * sigma2, 0.5 * cols);
+    typename M::Scalar ndi = outliers / (1.0 - outliers) * moving.rows() /
+                             fixed.rows() *
+                             std::pow(2.0 * M_PI * sigma2, 0.5 * cols);
     Array denom_p = kt1.array() + ndi;
-    Vector pt1 = 1 - ndi / denom_p;
+    V pt1 = 1 - ndi / denom_p;
     transform = create_transform(fixed, bandwidth);
-    Vector p1 = transform->compute(moving, 1 / denom_p);
-    Matrix px(moving.rows(), cols);
-    for (size_t i = 0; i < cols; ++i) {
+    V p1 = transform->compute(moving, 1 / denom_p);
+    M px(moving.rows(), cols);
+    for (typename M::Index i = 0; i < cols; ++i) {
         px.col(i) = transform->compute(moving, fixed.col(i).array() / denom_p);
     }
-    double l =
+    typename M::Scalar l =
         -denom_p.log().sum() + cols * fixed.rows() * std::log(sigma2) / 2;
     return { p1, pt1, px, l };
 }
 
+template <typename M, typename V>
 std::unique_ptr<fgt::Transform> GaussTransformFgt::create_transform(
-    const Matrix& points, double bandwidth) const {
+    const M& points, typename M::Scalar bandwidth) const {
     switch (m_method) {
         case FgtMethod::DirectTree:
             return std::unique_ptr<fgt::Transform>(
@@ -68,5 +72,7 @@ std::unique_ptr<fgt::Transform> GaussTransformFgt::create_transform(
     }
     return nullptr;
 }
+template class GaussTransformFgt<Matrix, Vector, Array>;
+template class GaussTransformFgt<MatrixF, VectorF, ArrayF>;
 
 } // namespace cpd
diff --git a/src/gauss_transform_make_default.cpp b/src/gauss_transform_make_default.cpp
index a7785597..83b1cd05 100644
--- a/src/gauss_transform_make_default.cpp
+++ b/src/gauss_transform_make_default.cpp
@@ -18,7 +18,14 @@
 #include <cpd/gauss_transform.hpp>
 
 namespace cpd {
-std::unique_ptr<GaussTransform> GaussTransform::make_default() {
-    return std::unique_ptr<GaussTransform>(new GaussTransformDirect());
+template <typename M, typename V>
+std::unique_ptr<GaussTransform<M, V>> GaussTransform<M, V>::make_default() {
+    return std::unique_ptr<GaussTransform<M, V>>(
+        new GaussTransformDirect<M, V>());
 }
+template std::unique_ptr<GaussTransform<Matrix, Vector>>
+GaussTransform<Matrix, Vector>::make_default();
+template std::unique_ptr<GaussTransform<MatrixF, VectorF>>
+GaussTransform<MatrixF, VectorF>::make_default();
+
 } // namespace cpd
diff --git a/src/matrix.cpp b/src/matrix.cpp
index 6c5824ed..a8697271 100644
--- a/src/matrix.cpp
+++ b/src/matrix.cpp
@@ -19,12 +19,19 @@
 
 namespace cpd {
 
-Matrix apply_transformation_matrix(Matrix points, const Matrix& transform) {
-    Matrix::Index rows = points.rows();
-    Matrix::Index cols = points.cols() + 1;
+template <typename M, typename V>
+M apply_transformation_matrix(M points, const M& transform) {
+    typename M::Index rows = points.rows();
+    typename M::Index cols = points.cols() + 1;
     points.conservativeResize(rows, cols);
-    points.col(cols - 1) = Vector::Ones(rows);
-    Matrix transformed_points = points * transform.transpose();
+    points.col(cols - 1) = V::Ones(rows);
+    M transformed_points = points * transform.transpose();
     return transformed_points.leftCols(cols - 1);
 }
+template Matrix apply_transformation_matrix<Matrix, Vector>(
+    Matrix points, const Matrix& transform);
+/*
+template MatrixF apply_transformation_matrix<MatrixF, VectorF>(
+  MatrixF points, const MatrixF& transform);
+*/
 } // namespace cpd
diff --git a/src/nonrigid.cpp b/src/nonrigid.cpp
index 28eedf9c..69a71a67 100644
--- a/src/nonrigid.cpp
+++ b/src/nonrigid.cpp
@@ -19,27 +19,31 @@
 
 namespace cpd {
 
-void Nonrigid::init(const Matrix& fixed, const Matrix& moving) {
+template <typename M, typename V>
+void Nonrigid<M, V>::init(const M& fixed, const M& moving) {
     m_g = affinity(moving, moving, m_beta);
-    m_w = Matrix::Zero(moving.rows(), moving.cols());
+    m_w = M::Zero(moving.rows(), moving.cols());
 }
 
-void Nonrigid::modify_probabilities(Probabilities& probabilities) const {
+template <typename M, typename V>
+void Nonrigid<M, V>::modify_probabilities(
+    Probabilities<M, V>& probabilities) const {
     probabilities.l += m_lambda / 2.0 * (m_w.transpose() * m_g * m_w).trace();
 }
 
-NonrigidResult Nonrigid::compute_one(const Matrix& fixed, const Matrix& moving,
-                                     const Probabilities& probabilities,
-                                     double sigma2) const {
+template <typename M, typename V>
+NonrigidResult<M, V> Nonrigid<M, V>::compute_one(
+    const M& fixed, const M& moving, const Probabilities<M, V>& probabilities,
+    typename M::Scalar sigma2) const {
     size_t cols = fixed.cols();
     auto dp = probabilities.p1.asDiagonal();
-    Matrix w = (dp * m_g + m_lambda * sigma2 *
-                               Matrix::Identity(moving.rows(), moving.rows()))
-                   .colPivHouseholderQr()
-                   .solve(probabilities.px - dp * moving);
-    NonrigidResult result;
+    M w = (dp * m_g +
+           m_lambda * sigma2 * M::Identity(moving.rows(), moving.rows()))
+              .colPivHouseholderQr()
+              .solve(probabilities.px - dp * moving);
+    NonrigidResult<M, V> result;
     result.points = moving + m_g * w;
-    double np = probabilities.p1.sum();
+    typename M::Scalar np = probabilities.p1.sum();
     result.sigma2 = std::abs(
         ((fixed.array().pow(2) * probabilities.pt1.replicate(1, cols).array())
              .sum() +
@@ -51,8 +55,17 @@ NonrigidResult Nonrigid::compute_one(const Matrix& fixed, const Matrix& moving,
     return result;
 }
 
-NonrigidResult nonrigid(const Matrix& fixed, const Matrix& moving) {
-    Nonrigid nonrigid;
+template <typename M, typename V>
+NonrigidResult<M, V> nonrigid(const M& fixed, const M& moving) {
+    Nonrigid<M, V> nonrigid;
     return nonrigid.run(fixed, moving);
 }
+template class Nonrigid<Matrix, Vector>;
+template class NonrigidResult<Matrix, Vector>;
+template NonrigidResult<Matrix, Vector> nonrigid(const Matrix& fixed,
+                                                 const Matrix& moving);
+template class Nonrigid<MatrixF, VectorF>;
+template class NonrigidResult<MatrixF, VectorF>;
+template NonrigidResult<MatrixF, VectorF> nonrigid(const MatrixF& fixed,
+                                                   const MatrixF& moving);
 } // namespace cpd
diff --git a/src/normalization.cpp b/src/normalization.cpp
index f91f6727..36fb5d44 100644
--- a/src/normalization.cpp
+++ b/src/normalization.cpp
@@ -19,7 +19,8 @@
 
 namespace cpd {
 
-Normalization::Normalization(const Matrix& f, const Matrix& m, bool linked)
+template <typename M, typename V>
+Normalization<M, V>::Normalization(const M& f, const M& m, bool linked)
   : fixed_mean(f.colwise().mean())
   , fixed(f - fixed_mean.transpose().replicate(f.rows(), 1))
   , fixed_scale(std::sqrt(fixed.array().pow(2).sum() / fixed.rows()))
@@ -27,11 +28,13 @@ Normalization::Normalization(const Matrix& f, const Matrix& m, bool linked)
   , moving(m - moving_mean.transpose().replicate(m.rows(), 1))
   , moving_scale(std::sqrt(moving.array().pow(2).sum() / moving.rows())) {
     if (linked) {
-        double scale = std::max(fixed_scale, moving_scale);
+        typename M::Scalar scale = std::max(fixed_scale, moving_scale);
         fixed_scale = scale;
         moving_scale = scale;
     }
     fixed /= fixed_scale;
     moving /= moving_scale;
 }
+template class Normalization<Matrix, Vector>;
+template class Normalization<MatrixF, VectorF>;
 } // namespace cpd
diff --git a/src/rigid.cpp b/src/rigid.cpp
index 37334f1c..9a7ccb83 100644
--- a/src/rigid.cpp
+++ b/src/rigid.cpp
@@ -20,24 +20,24 @@
 
 namespace cpd {
 
-RigidResult Rigid::compute_one(const Matrix& fixed, const Matrix& moving,
-                               const Probabilities& probabilities,
-                               double sigma2) const {
+template <typename M, typename V>
+RigidResult<M, V> Rigid<M, V>::compute_one(
+    const M& fixed, const M& moving, const Probabilities<M, V>& probabilities,
+    typename M::Scalar sigma2) const {
     size_t cols = fixed.cols();
-    double np = probabilities.pt1.sum();
-    Vector mu_x = fixed.transpose() * probabilities.pt1 / np;
-    Vector mu_y = moving.transpose() * probabilities.p1 / np;
-    Matrix a =
-        probabilities.px.transpose() * moving - np * mu_x * mu_y.transpose();
-    Eigen::JacobiSVD<Matrix, Eigen::NoQRPreconditioner> svd(
+    typename M::Scalar np = probabilities.pt1.sum();
+    V mu_x = fixed.transpose() * probabilities.pt1 / np;
+    V mu_y = moving.transpose() * probabilities.p1 / np;
+    M a = probabilities.px.transpose() * moving - np * mu_x * mu_y.transpose();
+    Eigen::JacobiSVD<M, Eigen::NoQRPreconditioner> svd(
         a, Eigen::ComputeThinU | Eigen::ComputeThinV);
-    Matrix s = svd.singularValues().asDiagonal();
-    Matrix c = Matrix::Identity(cols, cols);
+    M s = svd.singularValues().asDiagonal();
+    M c = M::Identity(cols, cols);
     if (!m_reflections) {
         c(cols - 1, cols - 1) =
             (svd.matrixU() * svd.matrixV().transpose()).determinant();
     }
-    RigidResult result;
+    RigidResult<M, V> result;
     result.rotation = svd.matrixU() * c * svd.matrixV().transpose();
     if (m_scale) {
         result.scale =
@@ -70,17 +70,19 @@ RigidResult Rigid::compute_one(const Matrix& fixed, const Matrix& moving,
     return result;
 }
 
-RigidResult rigid(const Matrix& fixed, const Matrix& moving) {
-    Rigid rigid;
+template <typename M, typename V>
+RigidResult<M, V> rigid(const M& fixed, const M& moving) {
+    Rigid<M, V> rigid;
     return rigid.run(fixed, moving);
 }
 
-Matrix RigidResult::matrix() const {
-    Matrix::Index rows = rotation.rows() + 1;
-    Matrix::Index cols = rotation.cols() + 1;
-    Matrix matrix = Matrix::Zero(rows, cols);
-    for (Matrix::Index row = 0; row < rotation.rows(); ++row) {
-        for (Matrix::Index col = 0; col < rotation.cols(); ++col) {
+template <typename M, typename V>
+M RigidResult<M, V>::matrix() const {
+    typename M::Index rows = rotation.rows() + 1;
+    typename M::Index cols = rotation.cols() + 1;
+    M matrix = M::Zero(rows, cols);
+    for (typename M::Index row = 0; row < rotation.rows(); ++row) {
+        for (typename M::Index col = 0; col < rotation.cols(); ++col) {
             matrix(row, col) = rotation(row, col) * scale;
         }
         matrix(row, cols - 1) = translation(row);
@@ -89,10 +91,23 @@ Matrix RigidResult::matrix() const {
     return matrix;
 }
 
-void RigidResult::denormalize(const Normalization& normalization) {
-    Result::denormalize(normalization);
+template <typename M, typename V>
+void RigidResult<M, V>::denormalize(const Normalization<M, V>& normalization) {
+    Result<M, V>::denormalize(normalization);
     scale = scale * normalization.fixed_scale / normalization.moving_scale;
-    translation = normalization.fixed_scale * translation + normalization.fixed_mean -
+    translation = normalization.fixed_scale * translation +
+                  normalization.fixed_mean -
                   scale * rotation * normalization.moving_mean;
 }
+
+template class Rigid<Matrix, Vector>;
+template class RigidResult<Matrix, Vector>;
+template RigidResult<Matrix, Vector> rigid(const Matrix& fixed,
+                                           const Matrix& moving);
+
+template class Rigid<MatrixF, VectorF>;
+template class RigidResult<MatrixF, VectorF>;
+template RigidResult<MatrixF, VectorF> rigid(const MatrixF& fixed,
+                                             const MatrixF& moving);
+
 } // namespace cpd
diff --git a/src/transform.cpp b/src/transform.cpp
index 7d1cc537..0f1e7a6d 100644
--- a/src/transform.cpp
+++ b/src/transform.cpp
@@ -19,8 +19,11 @@
 
 namespace cpd {
 
-void Result::denormalize(const Normalization& normalization) {
+template <typename M, typename V>
+void Result<M, V>::denormalize(const Normalization<M, V>& normalization) {
     points = points * normalization.fixed_scale +
              normalization.fixed_mean.transpose().replicate(points.rows(), 1);
 }
+template class Result<Matrix, Vector>;
+template class Result<MatrixF, VectorF>;
 } // namespace cpd
diff --git a/src/utils.cpp b/src/utils.cpp
index ab3dc2d6..6bdd0019 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -23,7 +23,8 @@
 
 namespace cpd {
 
-Matrix matrix_from_path(const std::string& path) {
+template <typename M>
+M matrix_from_path(const std::string& path) {
     std::ifstream file(path);
     if (!file.is_open()) {
         std::stringstream msg;
@@ -31,11 +32,11 @@ Matrix matrix_from_path(const std::string& path) {
         throw std::runtime_error(msg.str());
     }
     std::string line;
-    std::vector<std::vector<double>> rows;
+    std::vector<std::vector<typename M::Scalar>> rows;
     while (std::getline(file, line)) {
-        std::vector<double> row;
+        std::vector<typename M::Scalar> row;
         std::stringstream ss(line);
-        double n;
+        typename M::Scalar n;
         while (ss >> n) {
             row.push_back(n);
             // TODO support other delimiters than commas
@@ -52,11 +53,11 @@ Matrix matrix_from_path(const std::string& path) {
         rows.push_back(row);
     }
     if (rows.empty()) {
-        return Matrix(0, 0);
+        return M(0, 0);
     }
     size_t nrows = rows.size();
     size_t ncols = rows[0].size();
-    Matrix matrix(nrows, ncols);
+    M matrix(nrows, ncols);
     for (size_t i = 0; i < nrows; ++i) {
         for (size_t j = 0; j < ncols; ++j) {
             matrix(i, j) = rows[i][j];
@@ -65,20 +66,22 @@ Matrix matrix_from_path(const std::string& path) {
     return matrix;
 }
 
-double default_sigma2(const Matrix& fixed, const Matrix& moving) {
+template <typename M>
+typename M::Scalar default_sigma2(const M& fixed, const M& moving) {
     return ((moving.rows() * (fixed.transpose() * fixed).trace()) +
             (fixed.rows() * (moving.transpose() * moving).trace()) -
             2 * fixed.colwise().sum() * moving.colwise().sum().transpose()) /
            (fixed.rows() * moving.rows() * fixed.cols());
 }
 
-Matrix affinity(const Matrix& x, const Matrix& y, double beta) {
-    double k = -2.0 * beta * beta;
+template <typename M>
+M affinity(const M& x, const M& y, typename M::Scalar beta) {
+    typename M::Scalar k = -2.0 * beta * beta;
     size_t x_rows = x.rows();
     size_t y_rows = y.rows();
-    Matrix g;
+    M g;
     try {
-        g = Matrix(x_rows, y_rows);
+        g = M(x_rows, y_rows);
     } catch (const std::bad_alloc& err) {
         std::stringstream msg;
         msg << "Unable to allocate " << x_rows << " by " << y_rows
@@ -95,4 +98,15 @@ Matrix affinity(const Matrix& x, const Matrix& y, double beta) {
     }
     return g;
 }
+template Matrix matrix_from_path(const std::string& path);
+template Matrix affinity(const Matrix& x, const Matrix& y, Matrix::Scalar beta);
+template Matrix::Scalar default_sigma2(const Matrix& fixed,
+                                       const Matrix& moving);
+
+template MatrixF matrix_from_path(const std::string& path);
+template MatrixF affinity(const MatrixF& x, const MatrixF& y,
+                          MatrixF::Scalar beta);
+template MatrixF::Scalar default_sigma2(const MatrixF& fixed,
+                                        const MatrixF& moving);
+
 } // namespace cpd
diff --git a/tests/affine.cpp b/tests/affine.cpp
index e38674dc..1f340528 100644
--- a/tests/affine.cpp
+++ b/tests/affine.cpp
@@ -29,7 +29,8 @@ TEST_F(FishTest, MatchesReference) {
     translation << 1., 2.;
     m_fish_distorted = m_fish * transform +
                        translation.transpose().replicate(m_fish.rows(), 1);
-    AffineResult result = affine(m_fish_distorted, m_fish);
+    AffineResult<Matrix, Vector> result =
+        affine<Matrix, Vector>(m_fish_distorted, m_fish);
     ASSERT_EQ(result.points.rows(), m_fish.rows());
     EXPECT_TRUE(result.points.isApprox(m_fish_distorted, 1e-4));
     EXPECT_TRUE(result.transform.isApprox(transform.transpose(), 1e-4));
@@ -44,7 +45,7 @@ TEST_F(FaceTest, Affine) {
     translation << 1.0, 2.0, 3.0;
     m_face_distorted = m_face * transform +
                        translation.transpose().replicate(m_face.rows(), 1);
-    AffineResult result = affine(m_face_distorted, m_face);
+    AffineResult result = affine<Matrix, Vector>(m_face_distorted, m_face);
     ASSERT_EQ(result.points.rows(), m_face.rows());
     EXPECT_TRUE(result.points.isApprox(m_face_distorted, 1e-4));
     EXPECT_TRUE(result.transform.isApprox(transform.transpose(), 1e-4));
@@ -52,7 +53,7 @@ TEST_F(FaceTest, Affine) {
 }
 
 TEST(Affine, Linked) {
-    Affine affine;
+    Affine<Matrix, Vector> affine;
     affine.linked(true);
     EXPECT_TRUE(affine.linked());
     affine.linked(false);
@@ -60,18 +61,20 @@ TEST(Affine, Linked) {
 }
 
 TEST_F(FishTest, AffineMatrix) {
-    AffineResult result = affine(m_fish_distorted, m_fish);
+    AffineResult<Matrix, Vector> result =
+        affine<Matrix, Vector>(m_fish_distorted, m_fish);
     Matrix transform = result.matrix();
     ASSERT_EQ(transform.rows(), 3);
     ASSERT_EQ(transform.cols(), 3);
-    Matrix fish = apply_transformation_matrix(m_fish, transform);
+    Matrix fish =
+        apply_transformation_matrix<Matrix, Vector>(m_fish, transform);
     EXPECT_TRUE(result.points.isApprox(fish, 1e-4));
 }
 
 TEST_F(FishTest, Correspondences) {
-    Affine affine;
+    Affine<Matrix, Vector> affine;
     affine.correspondence(true);
-    AffineResult result = affine.run(m_fish_distorted, m_fish);
+    AffineResult<Matrix, Vector> result = affine.run(m_fish_distorted, m_fish);
     EXPECT_TRUE((result.correspondence.array() > 0).any());
 }
 } // namespace cpd
diff --git a/tests/gauss_transform.cpp b/tests/gauss_transform.cpp
index c987c6c4..1d5c26a5 100644
--- a/tests/gauss_transform.cpp
+++ b/tests/gauss_transform.cpp
@@ -24,8 +24,8 @@
 namespace cpd {
 
 TEST_F(FishTest, MatchesReference) {
-    GaussTransformDirect direct;
-    Probabilities probabilities =
+    GaussTransformDirect<Matrix, Vector> direct;
+    Probabilities<Matrix, Vector> probabilities =
         direct.compute(m_fish, m_fish_distorted, 1.0, 0.1);
     ASSERT_EQ(91, probabilities.p1.size());
     EXPECT_NEAR(0.6850, probabilities.p1(0), 1e-4);
@@ -41,8 +41,8 @@ TEST_F(FishTest, MatchesReference) {
 }
 
 TEST_F(FaceTest, MatchesReference) {
-    GaussTransformDirect direct;
-    Probabilities probabilities =
+    GaussTransformDirect<Matrix, Vector> direct;
+    Probabilities<Matrix, Vector> probabilities =
         direct.compute(m_face, m_face_distorted, 1.0, 0.1);
     ASSERT_EQ(392, probabilities.p1.size());
     EXPECT_NEAR(0.6171, probabilities.p1(0), 1e-4);
@@ -58,7 +58,7 @@ TEST_F(FaceTest, MatchesReference) {
 }
 
 TEST_F(FishTest, CorrespondencesMatchReference) {
-    GaussTransformDirect direct;
+    GaussTransformDirect<Matrix, Vector> direct;
     Probabilities probabilities =
         direct.compute(m_fish, m_fish_distorted, 1.0, 0.1);
     IndexVector correspondence =
diff --git a/tests/nonrigid.cpp b/tests/nonrigid.cpp
index 327d2617..272ad966 100644
--- a/tests/nonrigid.cpp
+++ b/tests/nonrigid.cpp
@@ -24,20 +24,20 @@
 namespace cpd {
 
 TEST_F(FishTest, Works) {
-    NonrigidResult result = nonrigid(m_fish, m_fish_distorted);
+    auto result = nonrigid<Matrix, Vector>(m_fish, m_fish_distorted);
     EXPECT_TRUE(result.points.isApprox(m_fish, 0.1));
 }
 
 TEST_F(FaceTest, Works) {
-    Nonrigid nonrigid;
+    Nonrigid<Matrix, Vector> nonrigid;
     nonrigid.normalize(false).sigma2(1.0).outliers(0.1);
-    NonrigidResult result = nonrigid.run(m_face, m_face_distorted);
+    auto result = nonrigid.run(m_face, m_face_distorted);
     EXPECT_TRUE(result.points.row(0).isApprox(m_face.row(0), 0.01));
     EXPECT_TRUE(result.points.row(391).isApprox(m_face.row(391), 0.5));
 }
 
 TEST(Nonrigid, Linked) {
-    Nonrigid nonrigid;
+    Nonrigid<Matrix, Vector> nonrigid;
     nonrigid.linked(true);
     EXPECT_TRUE(nonrigid.linked());
     nonrigid.linked(false);
@@ -45,7 +45,7 @@ TEST(Nonrigid, Linked) {
 }
 
 TEST_F(FishTest, Correspondences) {
-    Nonrigid nonrigid;
+    Nonrigid<Matrix, Vector> nonrigid;
     nonrigid.correspondence(true);
     NonrigidResult result = nonrigid.run(m_fish_distorted, m_fish);
     EXPECT_TRUE((result.correspondence.array() > 0).any());
diff --git a/tests/normalization.cpp b/tests/normalization.cpp
index ca120ec1..6b1f2208 100644
--- a/tests/normalization.cpp
+++ b/tests/normalization.cpp
@@ -22,7 +22,7 @@
 namespace cpd {
 
 TEST_F(FishTest, CanBeRetrieved) {
-    Normalization normalization(m_fish, m_fish);
+    Normalization<Matrix, Vector> normalization(m_fish, m_fish);
     ASSERT_EQ(m_fish.rows(), normalization.fixed.rows());
     EXPECT_TRUE(
         m_fish.isApprox(normalization.fixed * normalization.fixed_scale +
@@ -32,7 +32,7 @@ TEST_F(FishTest, CanBeRetrieved) {
 }
 
 TEST_F(FishTest, Unlinked) {
-    Normalization normalization(m_fish, m_fish * 2, false);
+    Normalization<Matrix, Vector> normalization(m_fish, m_fish * 2, false);
     EXPECT_NEAR(normalization.fixed_scale, normalization.moving_scale / 2.0,
                 1e-4);
 }
diff --git a/tests/rigid.cpp b/tests/rigid.cpp
index 1ad458a0..fd2b64a8 100644
--- a/tests/rigid.cpp
+++ b/tests/rigid.cpp
@@ -19,18 +19,19 @@
 
 #include "fixtures/fish.hpp"
 #include "gtest/gtest.h"
-#include <cpd/rigid.hpp>
 #include <cmath>
+#include <cpd/rigid.hpp>
 
+#include <iostream>
 namespace cpd {
 
 TEST_F(FishTest, ComputeOneMatchesReference) {
-    GaussTransformDirect direct;
+    GaussTransformDirect<Matrix, Vector> direct;
     Probabilities probabilities =
         direct.compute(m_fish, m_fish_distorted, 1.0, 0.1);
-    Rigid rigid;
+    Rigid<Matrix, Vector> rigid;
     rigid.scale(true);
-    RigidResult result =
+    RigidResult<Matrix, Vector> result =
         rigid.compute_one(m_fish, m_fish_distorted, probabilities, 1.0);
     EXPECT_NEAR(0.4165, result.sigma2, 1e-2);
 
@@ -41,24 +42,24 @@ TEST_F(FishTest, ComputeOneMatchesReference) {
 
 TEST_F(FishTest, Normalize) {
     m_fish = m_fish * 10.0;
-    Matrix rotation =
-        Eigen::Rotation2D<double>(M_PI * 20.0 / 180.0).toRotationMatrix();
+    Matrix rotation = Eigen::Rotation2D<Matrix::Scalar>(M_PI * 20.0 / 180.0)
+                          .toRotationMatrix();
     m_fish_distorted = m_fish * rotation;
-    RigidResult result = rigid(m_fish_distorted, m_fish);
+    auto result = rigid<Matrix, Vector>(m_fish_distorted, m_fish);
     EXPECT_TRUE(result.rotation.isApprox(rotation.transpose(), 1e-4));
     EXPECT_EQ(result.points.rows(), m_fish.rows());
 }
 
 TEST_F(FishTest, Scale) {
     m_fish_distorted = m_fish * 10.0;
-    Rigid rigid;
+    Rigid<Matrix, Vector> rigid;
     rigid.scale(true);
-    RigidResult result = rigid.run(m_fish_distorted, m_fish);
+    RigidResult<Matrix, Vector> result = rigid.run(m_fish_distorted, m_fish);
     EXPECT_NEAR(10.0, result.scale, 0.1);
 }
 
 TEST(Rigid, Linked) {
-    Rigid rigid;
+    Rigid<Matrix, Vector> rigid;
     rigid.scale(true);
     EXPECT_FALSE(rigid.linked());
     rigid.scale(false);
@@ -66,18 +67,20 @@ TEST(Rigid, Linked) {
 }
 
 TEST_F(FishTest, OneMatrix) {
-    RigidResult result = rigid(m_fish_distorted, m_fish);
+    RigidResult<Matrix, Vector> result =
+        rigid<Matrix, Vector>(m_fish_distorted, m_fish);
     Matrix transform = result.matrix();
     ASSERT_EQ(transform.rows(), 3);
     ASSERT_EQ(transform.cols(), 3);
-    Matrix fish = apply_transformation_matrix(m_fish, transform);
+    Matrix fish =
+        apply_transformation_matrix<Matrix, Vector>(m_fish, transform);
     EXPECT_TRUE(result.points.isApprox(fish, 1e-4));
 }
 
 TEST_F(FishTest, Correspondences) {
-    Rigid rigid;
+    Rigid<Matrix, Vector> rigid;
     rigid.correspondence(true);
-    RigidResult result = rigid.run(m_fish_distorted, m_fish);
+    RigidResult<Matrix, Vector> result = rigid.run(m_fish_distorted, m_fish);
     EXPECT_TRUE((result.correspondence.array() > 0).any());
 }
 } // namespace cpd
diff --git a/tests/support.hpp.in b/tests/support.hpp.in
index bb62bf11..c57e1c5d 100644
--- a/tests/support.hpp.in
+++ b/tests/support.hpp.in
@@ -23,7 +23,8 @@
 namespace cpd {
 namespace tests {
 Matrix fixture(const std::string& filename) {
-    return matrix_from_path("@PROJECT_SOURCE_DIR@/tests/fixtures/" + filename);
-}
-}
+    return matrix_from_path<Matrix>("@PROJECT_SOURCE_DIR@/tests/fixtures/" +
+                                    filename);
 }
+} // namespace tests
+} // namespace cpd
diff --git a/tests/utils.cpp b/tests/utils.cpp
index e14bfb01..3b744b85 100644
--- a/tests/utils.cpp
+++ b/tests/utils.cpp
@@ -21,12 +21,12 @@
 
 namespace cpd {
 TEST_F(FishTest, DefaultSigma2) {
-    double sigma2 = cpd::default_sigma2(m_fish, m_fish_distorted);
+    Matrix::Scalar sigma2 = cpd::default_sigma2(m_fish, m_fish_distorted);
     EXPECT_NEAR(1.0, sigma2, 0.1);
 }
 
 TEST_F(HelheimTest, DefaultSigma2) {
-    double sigma2 = cpd::default_sigma2(m_helheim, m_helheim);
+    Matrix::Scalar sigma2 = cpd::default_sigma2(m_helheim, m_helheim);
     EXPECT_NEAR(23235., sigma2, 1.0);
 }