1 files changed, 193 insertions, 0 deletions
diff --git a/meowpp/math/Transformations.hpp b/meowpp/math/Transformations.hpp
new file mode 100644
index 0000000..645b1a8
--- /dev/null
+++ b/meowpp/math/Transformations.hpp
@@ -0,0 +1,193 @@
+#include "Transformations.h"
+#include "utility.h"
+
+#include <cstdlib>
+
+namespace meow{
+  template<class Scalar>
+  inline
+  BallProjection<Scalar>::BallProjection(size_t __dimension):
+  Transformation(__dimension, 1, __dimension, 1, 1),
+  _dimension(__dimension),
+  _radius(1){
+  }
+  
+  
+  template<class Scalar>
+  inline
+  BallProjection<Scalar>::BallProjection(size_t __dimension,
+                                         Scalar const& __radius):
+  Transformation(__dimension, 1, __dimension, 1, 1),
+  _dimension(__dimension),
+  _radius(1){
+    radius(__radius);
+  }
+  
+  
+  template<class Scalar>
+  inline Scalar
+  BallProjection<Scalar>::parameter(size_t __i) const{
+    return radius();
+  }
+  
+  
+  template<class Scalar>
+  inline Scalar
+  BallProjection<Scalar>::parameter(size_t __i, Scalar const& __s){
+    return radius(__s);
+  }
+  
+  
+  template<class Scalar>
+  inline Scalar
+  BallProjection<Scalar>::radius() const{
+    return _radius;
+  }
+  
+  
+  template<class Scalar>
+  inline Scalar
+  BallProjection<Scalar>::radius(Scalar const& __radius){
+    _radius = __radius;
+    return radius();
+  }
+  
+  
+  template<class Scalar>
+  inline Matrix<Scalar>
+  BallProjection<Scalar>::transformate(Matrix<Scalar> const& __x) const{
+    Matrix<Scalar> ret(__x);
+    for(size_t c = 0, C = ret.cols(); c < C; c++){
+      Scalar sum(0);
+      for(size_t i = 0; i < _dimension; i++){
+        sum = sum + squ(ret(i, c));
+      }
+      Scalar len(sqrt(double(sum)));
+      for(size_t i = 0; i < _dimension; i++){
+        ret(i, c) = ret(i, c) * radius() / len;
+      }
+    }
+    return ret;
+  }
+  
+  
+  template<class Scalar>
+  inline Matrix<Scalar>
+  BallProjection<Scalar>::jacobian(Matrix<Scalar> const& __x) const{
+    Scalar sum(0);
+    for(size_t i = 0; i < _dimension; i++)
+      sum = sum + squ(__x(i, 0));
+    Scalar len(sqrt(double(sum)));
+    Matrix<Scalar> ret(_dimension, _dimension, Scalar(0.0));
+    for(size_t i = 0; i < _dimension; i++)
+      for(size_t j = 0; j < _dimension; j++)
+        ret(i, j) = ((i == j)
+                     ? 1.0 / len - squ(__x(i, 0)) / cub(len) * radius()
+                     : - __x(i, 0) * __x(j, 0) / cub(len) * radius()
+                     );
+    return ret;
+  }
+  
+  
+  template<class Scalar>
+  inline Matrix<Scalar>
+  BallProjection<Scalar>::jacobian(Matrix<Scalar> const& __x,
+                                   size_t                __i) const{
+    Matrix<Scalar> ret(_dimension, 1, Scalar(0.0));
+    Scalar sum(0);
+    for(size_t i = 0; i < _dimension; i++){
+      sum = sum + squ(__x(i, 0));
+    }
+    Scalar len(sqrt(double(sum)));
+    for(size_t i = 0; i < _dimension; i++){
+      ret(i, 0) = ret(i, 0) / len;
+    }
+    return ret;
+  }
+  
+  
+  template<class Scalar>
+  inline
+  PhotoProjection<Scalar>::PhotoProjection(size_t __dimension):
+  Transformation(__dimension, 1, __dimension, 1, 0),
+  _dimension(__dimension),
+  _focal(1){
+  }
+  
+  
+  template<class Scalar>
+  inline
+  PhotoProjection<Scalar>::PhotoProjection(size_t        __dimension,
+                                           Scalar const& __focal):
+  Transformation(__dimension, 1, __dimension, 1, 0),
+  _dimension(__dimension),
+  _focal(1){
+    focal(__focal);
+  }
+  
+  
+  template<class Scalar>
+  inline Scalar
+  PhotoProjection<Scalar>::parameter(size_t __i) const{
+    return focal();
+  }
+  
+  
+  template<class Scalar>
+  inline Scalar
+  PhotoProjection<Scalar>::parameter(size_t __i, Scalar const& __s){
+    return focal(__s);
+  }
+  
+  
+  template<class Scalar>
+  inline Scalar
+  PhotoProjection<Scalar>::focal() const{
+    return _focal;
+  }
+  
+  
+  template<class Scalar>
+  inline Scalar
+  PhotoProjection<Scalar>::focal(Scalar const& __focal){
+    _focal = __focal;
+    return focal();
+  }
+  
+  
+  template<class Scalar>
+  inline Matrix<Scalar>
+  PhotoProjection<Scalar>::transformate(Matrix<Scalar> const& __x) const{
+    Matrix<Scalar> ret(__x);
+    for(size_t c = 0, C = ret.cols(); c < C; c++){
+      for(size_t i = 0; i < _dimension; i++){
+        ret(i, c) = ret(i, c) * _focal / ret(_dimension - 1, c);
+      }
+    }
+    return ret;
+  }
+  
+  
+  template<class Scalar>
+  inline Matrix<Scalar>
+  PhotoProjection<Scalar>::jacobian(Matrix<Scalar> const& __x) const{
+    Matrix<Scalar> ret(_dimension, _dimension, Scalar(0.0));
+    for(ssize_t i = 0, I = (ssize_t)_dimension - 1; i < I; i++){
+      ret(i,              i) =  _focal /     __x(_dimension - 1, 0) ;
+      ret(i, _dimension - 1) = -_focal / squ(__x(_dimension - 1, 0));
+    }
+    return ret;
+  }
+  
+  
+  template<class Scalar>
+  inline Matrix<Scalar>
+  PhotoProjection<Scalar>::jacobian(Matrix<Scalar> const& __x,
+                                    size_t                __i) const{
+    Matrix<Scalar> ret(__dimension, 1, Scalar(0.0));
+    for(size_t i = 0; i < _dimension; i++){
+      ret(i, 0) = ret(i, 0) / ret(_dimension - 1, 0);
+    }
+    return ret;
+  }
+}