thin_plate_spline.h

#pragma once
#include <cgv/math/mat.h>
#include <cgv/math/vec.h>
#include <cgv/math/lin_solve.h>
 
namespace cgv {
        namespace math {
 
template <typename T>
struct thin_plate_spline
{
        mat<T> controlpoints;
        mat<T> weights;
        mat<T> affine_transformation;
 
        vec<T> map_position(const vec<T>& p)
        {
                assert(p.size() == 2);
                vec<T> r(2);
                r(0) = affine_transformation(0,0)
                        +affine_transformation(1,0)*p(0)
                        +affine_transformation(2,0)*p(1);
                r(1) = affine_transformation(0,1)
                        +affine_transformation(1,1)*p(0)
                        +affine_transformation(2,1)*p(1);
                for(unsigned i = 0;i < weights.nrows();i++)
                {
                        T u_sqr_dist=U(sqr_length(p-controlpoints.col(i)));
                        r(0)+=weights(i,0)*u_sqr_dist;
                        r(1)+=weights(i,1)*u_sqr_dist;
                }
                return r;               
        }
 
        vec<T> map_affine_position(const vec<T>& p)
        {
                assert(p.size() == 2);
                vec<T> r(2);
                
                vec<T> temp(4);
                        temp(0) =               (affine_transformation(1,0) + affine_transformation(2,1))/2;
                        temp(1) =               (affine_transformation(2,0) - affine_transformation(1,1))/2;
                        temp(2) =        -  temp(1);
                        temp(3) =               temp(0);
 
                r(0) = affine_transformation(0,0)
                        +temp(0) * p(0)
                        +temp(1) * p(1);
                r(1) = affine_transformation(0,1)
                        +temp(2) * p(0)
                        +temp(3) * p(1);
 
                return r;               
        }
 
        mat<T> map_positions(const mat<T>& points)
        {
                assert(points.nrows() == 2);
                mat<T> rpoints(points.nrows(),points.ncols());
 
                for(unsigned i = 0; i < points.ncols(); i++)
                        rpoints.set_col(i,map_position(points.col(i))); 
                
                return rpoints;         
        }
 
        static T U(const T& sqr_dist)
        {
                static const T factor = (T)(1.0/(2.0*log((double)10)));
                if(sqr_dist == 0)
                        return 0;
                return sqr_dist*log(sqr_dist)*factor;
        }
 
};
 
 
 
 
 
template <typename T>
void find_nonrigid_transformation(const mat<T>& points1,
                                                                  const mat<T>& points2,
                                                                  thin_plate_spline<T>& spline)
{
        assert(points1.nrows() == 2 && points2.nrows() == 2);
        assert(points1.ncols() == points2.ncols());
        assert(points1.ncols() > 2);//at least three points
 
        int n = points1.ncols();
        
        mat<T> L(n+3,n+3);
        
 
        for(int i = 0; i < n;i++)
                for(int j = 0; j < n;j++)
                {
                        T sqr_dist = sqr_length(points1.col(i)-points1.col(j));
                        L(i,j)=thin_plate_spline<T>::U(sqr_dist);               
                }
 
        for(int i = 0; i < n; i++)
        {
                L(i,n) = 1;
                L(i,n+1) = points1.col(i)(0);
                L(i,n+2) = points1.col(i)(1);
                L(n,i) = 1;
                L(n+1,i) = points1.col(i)(0);
                L(n+2,i) = points1.col(i)(1);
        }
 
        for(int i = n; i < n+3;i++)
        {
                for(int j = n; j < n+3;j++)
                {
                        L(i,j)=0;
                }
 
        }
 
        mat<T> V(n+3,2);
        for(int i =0;i < n; i++)
        {
                V(i,0)=points2(0,i);
                V(i,1)=points2(1,i);
        }
        V(n,0)=V(n,1)=V(n+1,0)=V(n+1,1)=V(n+2,0)=V(n+2,1)=0;
        mat<T> W(n+3,2);
 
        svd_solve(L,V,W);
        
        spline.controlpoints=points1;
        spline.weights=W.sub_mat(0,0,n,2);
        
        spline.affine_transformation =W.sub_mat(n,0,3,2);
                
}
 
template <typename T>
struct thin_hyper_plate_spline
{
        mat<T> controlpoints;
        mat<T> weights;
        mat<T> affine_transformation;
 
        vec<T> map_position(const vec<T>& p)
        {
        
                assert(p.size() == 3);
                vec<T> r(3);
                r(0) = affine_transformation(0,0)
                          +affine_transformation(1,0)*p(0)
                          +affine_transformation(2,0)*p(1)
                          +affine_transformation(3,0)*p(2);
                
                r(1) = affine_transformation(0,1)
                          +affine_transformation(1,1)*p(0)
                          +affine_transformation(2,1)*p(1)
                          +affine_transformation(3,1)*p(2);
                
                r(2) = affine_transformation(0,2)
                          +affine_transformation(1,2)*p(0)
                          +affine_transformation(2,2)*p(1)
                          +affine_transformation(3,2)*p(2);
 
                for(unsigned i = 0;i < weights.nrows();i++)
                {
                        T u=length(p-controlpoints.col(i));
                        r(0)+=weights(i,0)*u;
                        r(1)+=weights(i,1)*u;
                        r(2)+=weights(i,2)*u;
                }
                return r;
                        
        }
 
        mat<T> map_positions(const mat<T>& points)
        {
                mat<T> rpoints(points.nrows(),points.ncols());
                assert(points.nrows() == 3);
                for(unsigned i = 0; i < points.ncols(); i++)
                {
                        rpoints.set_col(i,map_position(points.col(i))); 
                }
                return rpoints;
        }
 
};
 
 
template <typename T>
void find_nonrigid_transformation(const mat<T>& points1,
                                                                  const mat<T>& points2,
                                                                  thin_hyper_plate_spline<T>& spline)
{
        assert(points1.nrows() == 3 && points2.nrows()==3);
        assert(points1.ncols() == points2.ncols());     
        assert(points1.nrows()==4);
 
        int n = points1.ncols();
        
        mat<T> L(n+4,n+4);
        
 
        for(int i = 0; i < n;i++)
                for(int j = 0; j < n;j++)
                {
                        L(i,j)=length(points1.col(i)-points1.col(j));   
                }
 
        for(int i = 0; i < n; i++)
        {
                L(i,n) = 1;
                L(i,n+1) = points1.col(i)(0);
                L(i,n+2) = points1.col(i)(1);
                L(i,n+3) = points1.col(i)(2);
                L(n,i) = 1;
                L(n+1,i) = points1.col(i)(0);
                L(n+2,i) = points1.col(i)(1);
                L(n+3,i) = points1.col(i)(2);
 
        }
 
        for(int i = n; i < n+4;i++)
        {
                for(int j = n; j < n+4;j++)
                {
                        L(i,j)=0;
                }
        }
        
 
        mat<T> V(n+4,3);
        for(int i =0;i < n; i++)
        {
                V(i,0)=points2(0,i);
                V(i,1)=points2(1,i);
                V(i,2)=points2(2,i);
        }
        V(n,0)=V(n,1)=V(n,2)=V(n+1,0)=V(n+1,1)=V(n+1,2)=V(n+2,0)=V(n+2,1)=V(n+2,2)
                =V(n+3,0)=V(n+3,1)=V(n+3,2)=0;
        
        mat<T> W(n+4,3);
        svd_solve(L,V,W);
 
        spline.controlpoints=points1;
        spline.weights=W.sub_mat(0,0,n,3);
        spline.affine_transformation =W.sub_mat(n,0,4,3);
        
        
                
}
 
 
template <typename T>
void apply_nonrigid_transformation(const thin_plate_spline<T>& s, mat<T>& points)
{
        assert(points.nrows() == 2);
        for(unsigned i = 0; i < points.ncols(); i++)
        {
                points.set_col(i,s.map_position(points.col(i)));        
        }
}
 
template <typename T>
void apply_nonrigid_transformation(const thin_hyper_plate_spline<T>& s, mat<T>& points)
{
        assert(points.nrows() == 3);
        for(unsigned i = 0; i < points.ncols(); i++)
        {
                points.set_col(i,s.map_position(points.col(i)));        
        }
}
 
 
}
 
 
 
 
}