ossimThinPlateSpline.cpp

Go to the documentation of this file.

/******************************************************************************
 * $Id: thinplatespline.cpp 15547 2008-10-17 17:45:03Z rouault $
 *
 * Project:  GDAL Warp API
 * Purpose:  Implemenentation of 2D Thin Plate Spline transformer. 
 * Author:   VIZRT Development Team.
 *
 * This code was provided by Gilad Ronnen (gro at visrt dot com) with 
 * permission to reuse under the following license.
 * 
 ******************************************************************************
 * Copyright (c) 2004, VIZRT Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 ****************************************************************************/


#include <cstdio>
#include <ossim/base/ossimConstants.h>
#include <ossim/base/ossimCommon.h>
#include <ossim/base/ossimNotify.h>
#include <ossim/base/ossimThinPlateSpline.h>

#ifndef FLT_MAX
#  define FLT_MAX 1e+37
#  define FLT_MIN 1e-37
#endif


#define A(r,c) _AA[ _nof_eqs * (r) + (c) ]
#define Ainv(r,c) _Ainv[ _nof_eqs * (r) + (c) ]


#define VIZ_GEOREF_SPLINE_DEBUG 0

static int matrixInvert( int N, double input[], double output[] );

void ossimThinPlateSpline::growPoints()

{
   int new_max = _max_nof_points*2 + 2 + 3;
   int i;
   
   x.resize( new_max );
   y.resize( new_max );
   u.resize( new_max );
   unused.resize( new_max );
   index.resize( new_max );
   for( i = 0; i < _nof_vars; i++ )
   {
      rhs[i].resize( new_max );
      coef[i].resize( new_max );
   }
   
   _max_nof_points = new_max - 3;
}

int ossimThinPlateSpline::addPoint( const double Px, const double Py, const double *Pvars )
{
   type = VIZ_GEOREF_SPLINE_POINT_WAS_ADDED;
   int i;
   
   if( _nof_points == _max_nof_points )
      growPoints();
   
   i = _nof_points;
   //A new point is added
   x[i] = Px;
   y[i] = Py;
   for ( int j = 0; j < _nof_vars; j++ )
      rhs[j][i+3] = Pvars[j];
   _nof_points++;
   return 1;
}

bool ossimThinPlateSpline::changePoint(int index, double Px, double Py, double* Pvars)
{
   if ( index < _nof_points )
   {
      int i = index;
      x[i] = Px;
      y[i] = Py;
      for ( int j = 0; j < _nof_vars; j++ )
         rhs[j][i+3] = Pvars[j];
   }
   
   return( true );
}

bool ossimThinPlateSpline::getXy(int index, double& outX, double& outY)const
{
   bool ok;
   
   if ( index < _nof_points )
   {
      ok = true;
      outX = x[index];
      outY = y[index];
   }
   else
   {
      ok = false;
      outX = outY = 0.0f;
   }
   
   return(ok);
}

int ossimThinPlateSpline::deletePoint(const double Px, const double Py )
{
   for ( int i = 0; i < _nof_points; i++ )
   {
      if ( ( ossim::abs(Px - x[i]) <= _tx ) && ( ossim::abs(Py - y[i]) <= _ty ) )
      {
         for ( int j = i; j < _nof_points - 1; j++ )
         {
            x[j] = x[j+1];
            y[j] = y[j+1];
            for ( int k = 0; k < _nof_vars; k++ )
               rhs[k][j+3] = rhs[k][j+3+1];
         }
         _nof_points--;
         type = VIZ_GEOREF_SPLINE_POINT_WAS_DELETED;
         return(1);
      }
   }
   return(0);
}

int ossimThinPlateSpline::solve(void)
{
   int r, c, v;
   int p;
    
   //   No points at all
   if ( _nof_points < 1 )
   {
      type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
      return(0);
   }
    
   // Only one point
   if ( _nof_points == 1 )
   {
      type = VIZ_GEOREF_SPLINE_ONE_POINT;
      return(1);
   }
   // Just 2 points - it is necessarily 1D case
   if ( _nof_points == 2 )
   {
      _dx = x[1] - x[0];
      _dy = y[1] - y[0];     
      double fact = 1.0 / ( _dx * _dx + _dy * _dy );
      _dx *= fact;
      _dy *= fact;
        
      type = VIZ_GEOREF_SPLINE_TWO_POINTS;
      return(2);
   }
    
   // More than 2 points - first we have to check if it is 1D or 2D case
   
   double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
   double delx, dely;
   double xx, yy;
   double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
   double SSxx, SSyy, SSxy;
    
   for ( p = 0; p < _nof_points; p++ )
   {
      xx = x[p];
      yy = y[p];
        
      xmax = ossim::max( xmax, xx );
      xmin = ossim::min( xmin, xx );
      ymax = ossim::max( ymax, yy );
      ymin = ossim::min( ymin, yy );
        
      sumx  += xx;
      sumx2 += xx * xx;
      sumy  += yy;
      sumy2 += yy * yy;
      sumxy += xx * yy;
   }
   delx = xmax - xmin;
   dely = ymax - ymin;
    
   SSxx = sumx2 - sumx * sumx / _nof_points;
   SSyy = sumy2 - sumy * sumy / _nof_points;
   SSxy = sumxy - sumx * sumy / _nof_points;
    
   if ( delx < 0.001 * dely || dely < 0.001 * delx || 
       fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
   {
      int p1;
        
      type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
        
      _dx = _nof_points * sumx2 - sumx * sumx;
      _dy = _nof_points * sumy2 - sumy * sumy;
      double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
      _dx *= fact;
      _dy *= fact;
        
      for ( p = 0; p < _nof_points; p++ )
      {
         double dxp = x[p] - x[0];
         double dyp = y[p] - y[0];
         u[p] = _dx * dxp + _dy * dyp;
         unused[p] = 1;
      }
        
      for ( p = 0; p < _nof_points; p++ )
      {
         int min_index = -1;
         double min_u = 0;
         for ( p1 = 0; p1 < _nof_points; p1++ )
         {
            if ( unused[p1] )
            {
               if ( min_index < 0 || u[p1] < min_u )
               {
                  min_index = p1;
                  min_u = u[p1];
               }
            }
         }
         index[p] = min_index;
         unused[min_index] = 0;
      }
        
      return(3);
   }
    
   type = VIZ_GEOREF_SPLINE_FULL;
   
   _nof_eqs = _nof_points + 3;
    
   _AA.resize( _nof_eqs * _nof_eqs );
   _Ainv.resize(_nof_eqs * _nof_eqs );
    
   // Calc the values of the matrix A
   for ( r = 0; r < 3; r++ )
      for ( c = 0; c < 3; c++ )
         A(r,c) = 0.0;
   
   for ( c = 0; c < _nof_points; c++ )
   {
      A(0,c+3) = 1.0;
      A(1,c+3) = x[c];
      A(2,c+3) = y[c];
      
      A(c+3,0) = 1.0;
      A(c+3,1) = x[c];
      A(c+3,2) = y[c];
   }
   
   for ( r = 0; r < _nof_points; r++ )
      for ( c = r; c < _nof_points; c++ )
      {
         A(r+3,c+3) = baseFunc( x[r], y[r], x[c], y[c] );
         if ( r != c )
            A(c+3,r+3 ) = A(r+3,c+3);
      }
   
   
   // Invert the matrix
   int status = matrixInvert( _nof_eqs, &_AA.front(), &_Ainv.front() );
   
   if ( !status )
   {
      ossimNotify(ossimNotifyLevel_WARN)
         << " There is a problem to invert the interpolation matrix\n"
         << std::endl;
      return 0;
   }
   
   // calc the coefs
   for ( v = 0; v < _nof_vars; v++ )
      for ( r = 0; r < _nof_eqs; r++ )
      {
         coef[v][r] = 0.0;
         for ( c = 0; c < _nof_eqs; c++ )
            coef[v][r] += Ainv(r,c) * rhs[v][c];
      }
   
   return(4);
}

int ossimThinPlateSpline::getPoint( const double Px, const double Py, double *vars )const
{
    int v, r;
    double tmp, Pu;
    double fact;
    int leftP=0, rightP=0, found = 0;
    
    switch ( type )
    {
      case VIZ_GEOREF_SPLINE_ZERO_POINTS :
         for ( v = 0; v < _nof_vars; v++ )
            vars[v] = 0.0;
         break;
      case VIZ_GEOREF_SPLINE_ONE_POINT :
         for ( v = 0; v < _nof_vars; v++ )
            vars[v] = rhs[v][3];
         break;
      case VIZ_GEOREF_SPLINE_TWO_POINTS :
         fact = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
         for ( v = 0; v < _nof_vars; v++ )
            vars[v] = ( 1 - fact ) * rhs[v][3] + fact * rhs[v][4];
         break;
      case VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL :
         Pu = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
         if ( Pu <= u[index[0]] )
         {
            leftP = index[0];
            rightP = index[1];
         }
         else if ( Pu >= u[index[_nof_points-1]] )
         {
            leftP = index[_nof_points-2];
            rightP = index[_nof_points-1];
         }
         else
         {
            for ( r = 1; !found && r < _nof_points; r++ )
            {
               leftP = index[r-1];
               rightP = index[r];                   
               if ( Pu >= u[leftP] && Pu <= u[rightP] )
                  found = 1;
            }
         }
         
         fact = ( Pu - u[leftP] ) / ( u[rightP] - u[leftP] );
         for ( v = 0; v < _nof_vars; v++ )
            vars[v] = ( 1.0 - fact ) * rhs[v][leftP+3] +
            fact * rhs[v][rightP+3];
         break;
      case VIZ_GEOREF_SPLINE_FULL :
         for ( v = 0; v < _nof_vars; v++ )
            vars[v] = coef[v][0] + coef[v][1] * Px + coef[v][2] * Py;
         
         for ( r = 0; r < _nof_points; r++ )
         {
            tmp = baseFunc( Px, Py, x[r], y[r] );
            for ( v= 0; v < _nof_vars; v++ )
               vars[v] += coef[v][r+3] * tmp;
         }
         break;
      case VIZ_GEOREF_SPLINE_POINT_WAS_ADDED :
         fprintf(stderr, " A point was added after the last solve\n");
         fprintf(stderr, " NO interpolation - return values are zero\n");
         for ( v = 0; v < _nof_vars; v++ )
            vars[v] = 0.0;
         return(0);
         break;
      case VIZ_GEOREF_SPLINE_POINT_WAS_DELETED :
         fprintf(stderr, " A point was deleted after the last solve\n");
         fprintf(stderr, " NO interpolation - return values are zero\n");
         for ( v = 0; v < _nof_vars; v++ )
            vars[v] = 0.0;
         return(0);
         break;
      default :
         return(0);
         break;
    }
    return(1);
}

double ossimThinPlateSpline::baseFunc( const double x1, const double y1,
                                    const double x2, const double y2 )const
{
    if ( ( x1 == x2 ) && (y1 == y2 ) )
        return 0.0;
    
    double dist  = ( x2 - x1 ) * ( x2 - x1 ) + ( y2 - y1 ) * ( y2 - y1 );
    
    return dist * log( dist );  
}

static int matrixInvert( int N, double input[], double output[] )
{
   // Receives an array of dimension NxN as input.  This is passed as a one-
   // dimensional array of N-squared size.  It produces the inverse of the
   // input matrix, returned as output, also of size N-squared.  The Gauss-
   // Jordan Elimination method is used.  (Adapted from a BASIC routine in
   // "Basic Scientific Subroutines Vol. 1", courtesy of Scott Edwards.)
    
   // Array elements 0...N-1 are for the first row, N...2N-1 are for the
   // second row, etc.
    
   // We need to have a temporary array of size N x 2N.  We'll refer to the
   // "left" and "right" halves of this array.
    
   int row, col;
    
#if 0
   fprintf(stderr, "Matrix Inversion input matrix (N=%d)\n", N);
   for ( row=0; row<N; row++ )
   {
      for ( col=0; col<N; col++ )
      {
         fprintf(stderr, "%5.2f ", input[row*N + col ]  );
      }
      fprintf(stderr, "\n");
   }
#endif
    
   int tempSize = 2 * N * N;
   std::vector<double> temp(tempSize);
   double ftemp;
    
   if (temp.empty()) {
        
      fprintf(stderr, "matrixInvert(): ERROR - memory allocation failed.\n");
      return false;
   }
    
   // First create a double-width matrix with the input array on the left
   // and the identity matrix on the right.
    
   for ( row=0; row<N; row++ )
   {
      for ( col=0; col<N; col++ )
      {
         // Our index into the temp array is X2 because it's twice as wide
         // as the input matrix.
            
         temp[ 2*row*N + col ] = input[ row*N+col ];    // left = input matrix
         temp[ 2*row*N + col + N ] = 0.0f;          // right = 0
      }
      temp[ 2*row*N + row + N ] = 1.0f;     // 1 on the diagonal of RHS
   }
    
   // Now perform row-oriented operations to convert the left hand side
   // of temp to the identity matrix.  The inverse of input will then be
   // on the right.
    
   int max;
   int k=0;
   for (k = 0; k < N; k++)
   {
      if (k+1 < N)  // if not on the last row
      {              
         max = k;
         for (row = k+1; row < N; row++) // find the maximum element
         {  
            if (fabs( temp[row*2*N + k] ) > fabs( temp[max*2*N + k] ))
            {
               max = row;
            }
         }
            
         if (max != k)  // swap all the elements in the two rows
         {        
            for (col=k; col<2*N; col++)
            {
               ftemp = temp[k*2*N + col];
               temp[k*2*N + col] = temp[max*2*N + col];
               temp[max*2*N + col] = ftemp;
            }
         }
      }
        
      ftemp = temp[ k*2*N + k ];
      if ( ftemp == 0.0f ) // matrix cannot be inverted
      {
         return false;
      }
        
      for ( col=k; col<2*N; col++ )
      {
         temp[ k*2*N + col ] /= ftemp;
      }
        
      for ( row=0; row<N; row++ )
      {
         if ( row != k )
         {
            ftemp = temp[ row*2*N + k ];
            for ( col=k; col<2*N; col++ ) 
            {
               temp[ row*2*N + col ] -= ftemp * temp[ k*2*N + col ];
            }
         }
      }
   }
    
   // Retrieve inverse from the right side of temp
    
   for (row = 0; row < N; row++)
   {
      for (col = 0; col < N; col++)
      {
         output[row*N + col] = temp[row*2*N + col + N ];
      }
   }
    
    
   return true;
}