
/*

________________________________________________________________

        warp
        $Id: warp.c,v 1.28 1997/04/23 13:00:00 svein Exp $
        Copyright 1990, Blab, UiO
        Image processing lab, Department of Informatics
        University of Oslo
        E-mail: blab@ifi.uio.no
________________________________________________________________
  
  Permission to use, copy, modify and distribute this software and its
  documentation for any purpose and without fee is hereby granted, 
  provided that this copyright notice appear in all copies and that 
  both that copyright notice and this permission notice appear in supporting
  documentation and that the name of B-lab, Department of Informatics or
  University of Oslo not be used in advertising or publicity pertaining 
  to distribution of the software without specific, written prior permission.

  B-LAB DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING ALL
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL B-LAB
  BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
  OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN 
  CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
*/



/*P:warp*

________________________________________________________________

		warp
________________________________________________________________

Name:		warp - polynomial control point mapping

Syntax:		| warp [-ip <n>] [-bg <bg>] [-cp <cpfile>] [-p <poldeg>] 
                |      [-xstart <xstart>] [-xstop <xstop>]
		|      [-xsize <xsize>] [-ystart <ystart>]
		|      [-ystop <ystop>] [-ysize <xsize>]
                |      [-rss] [-qr] [ <inimage> <outimage> ]

Description:    Perform an output to input mapping (k,l) = M(i,j).
                Given an input image I(k, l), calculate O(i,j).
		| If 'poldeg' == 1 (default) :
		| k = Q(i, j) = q0 + i * q1 + j * q2
		| l = R(i, j) = r0 + i * r1 + j * r2
		| If 'poldeg' == 2 :
		| k = Q(i, j) = q0 + i*q1 + j*q2 + i*i*q3 + i*j*q4 + j*j*q5
		| l = R(i, j) = r0 + i*r1 + j*r2 + i*i*r3 + i*j*r4 + j*j*r5
		The coefficients 'q' and 'r' will be printed if the '-qr' flag
		is set.

		If '-rss' is set, the residuals r(m) will be calculated:
		| dk = k - Q(i,j)
		| dl = l - R(i,j)
		| r(m) = sqrt(dk*dk + dl*dl)

		The control point file 'cpfile' consists of pairs of points.
		Each line is a set of 'i', 'j', 'k' and 'l'.
		'i' and 'j' are coordinates in the reference image.
		'k' and 'l' are coordinates in the image to be transformed.
		Lines with # in coloumn 1 are treated as comment lines. 
		Blank lines are ignored. Pixels inside a line are separated
		by blanks, lines are separated by newline.
		| #
                | # Example 1
                | #
		| #   i   j    k   l
		| #------------------
		|    57  44   61 144
                |    27  72   38 180
                |   114 122  132 201
                |   180 199  213 255
                |   193 166  215 217
                |   191 288  241 336
		
		If 'inimage' and 'outimage' are specified, the transform
		will be performed.
		The interpolation method '-ip' is
		| -ip 0 : Nearest neighbor 
		| -ip 1 : Bilinear   interpolation
		|         g(x) = bi(f(x), f(x+1), d)
		|         bi(x, y, d) = x(1-d) + yd
		| -ip 3 ; Cubic      interpolation
		|         g(x) = cc(f(x-1), f(x), f(x+1), f(x+2), d)
		|         cc(x, y, z, v, d) = y + d((-x+z) +
		|                             d((2x-2y+z-v) + d(-x+y-z+v)))
		Default is bilinear interpolation (-ip 1).

		If the output image should have another size or startpoint
		than the input image, this may be specified by the
		'-xstart', '-xstop', '-xsize', '-ystart', '-ystop' and '-ysize'
		options. If start, stop as well as size options are given, and
		inconsistently, the stop option is ignored. The given
		sizes are clipped against the result of the transform.

		'-bg' is used to specify the background pixel value,
		default = 0.

Reference:	Wayne Niblack. Digital Image Processing, p 131-150

Restrictions:   Works on unsigned byte images only.

See also:       affine(1), quadratic(1), reorganize(1), rotate(1),
                shift_img(1), transpose_img(1)

Author:		Lennart Flem 1/3-91.

Modified:       Otto Milvang
Examples:
Id: 		$Id: warp.c,v 1.28 1997/04/23 13:00:00 svein Exp $
________________________________________________________________

*/

#include <stdlib.h>
#include <math.h>
#include <xite/includes.h>
#include <xite/biff.h>
#include XITE_STDIO_H
#include <xite/convert.h>
#include <xite/geometry.h>
#include <xite/message.h>
#include <xite/readarg.h>
#include XITE_MALLOC_H
#include "matrise.c"

#define EPS 0.001
#define KPUNKTBLOKK 100
#define DEFPOLGRAD 1
#define DEFGSTEP 8

#define round(X) ((int)((X)+0.5))
#define lip(V,W,F) ((V)*(1-(F))+(W)*(F))
#define bilip(OV,OH,NV,NH,XF,YF) lip(lip(OV,OH,XF),lip(NV,NH,XF),YF)

/*
static float polmap(q, polgrad, y, x)
float *q;
int polrad, y, x;
{
  int pg, ph, i = 0;
  float v = 0;
  for (pg=0; pg<=polgrad; pg++)
    for (ph=pg; ph>=0; ph--) 
      v += q[i++] * pow((float)x,(float)(ph)) * pow((float)y,(float)(pg-ph));
  return v;
}
*/

#ifndef FUNCPROTO
int main(argc, argv)
int argc;
char *argv[];
#else /* FUNCPROTO */
int main(int argc, char *argv[])
#endif /* FUNCPROTO */
{
  IMAGE imgi, imgo;
  IR_BAND rband;
  char *innfil, *utfil, *cpfile, *cxstart, *cxstop, *cxsize, *cystart,
      *cystop, *cysize, *args;
  int band, oargc, xstart, xsize, ystart, ysize, ind, npunkt, nallocpunkt,
      antelem, elem, pollen, i, rss, qr, ip, bg,
      polgrad = DEFPOLGRAD;
  float *ijdata, *kdata, *ldata, ir, jr, kr, lr, dk, dl, rs;
  double xxa[6], yya[6];
  Matrise Q,R,K,L,M,Mp;

  Iset_message(TRUE);
  Iset_abort(TRUE);

  InitMessage(&argc, argv, xite_app_std_usage_text(
       "Usage: %s [-rss] [-qr] [-ip <n>] [-cp <cpfile>] [-p <poldeg>]\n\
       [-xstart <xstart>] [-xstop <xstop>] [-xsize <xsize>]\n\
       [-ystart <ystart>] [-ystop <ystop>] [-ysize <xsize>]\n\
       [<inimage> <outimage>]\n"));

  if (argc == 1) Usage(1, NULL);
  args    = argvOptions(argc, argv);

  cpfile  = read_switch(&argc, argv, "-cp", 1, NULL);
  rss     = read_bswitch(&argc, argv, "-rss");
  qr      = read_bswitch(&argc, argv, "-qr");
  ip      = read_iswitch(&argc, argv, "-ip", 1);
  bg      = read_iswitch(&argc, argv, "-bg", 0);
  polgrad = read_iswitch(&argc, argv, "-p", 1);
  cxstart = read_switch(&argc, argv, "-xstart", 1, NULL);  
  cystart = read_switch(&argc, argv, "-ystart", 1, NULL);  
  oargc = argc;
  cxstop  = read_switch(&argc, argv, "-xstop",  1, NULL);  
  cxsize  = read_switch(&argc, argv, "-xsize",  1, NULL);  
  cystop  = read_switch(&argc, argv, "-ystop",  1, NULL);  
  cysize  = read_switch(&argc, argv, "-ysize",  1, NULL);  

  if (argc != 3) Usage(1, "Illegal number of arguments.\n");
  if (polgrad != 1 && polgrad != 2)
    Usage(2, "Polynomial degree must be 1 or 2.\n");

  pollen = (polgrad+1)*(polgrad+2)/2;
  Info(0, "Verbose switch is on.\nPolynomial degree ");
  Info(0, "%d, minimum %d control points\n ", polgrad, pollen);

  if (!cpfile) Message(0, "Expecting control points from stdin.\n");

  ascii2biff((IBAND *) &rband, cpfile, Ireal_typ);
  if (Ixsize((IBAND) rband) < 4) Error(6, "Error in cpfile\n");
  npunkt = Iysize((IBAND) rband);
  Info(0, "Read %d control points ... ",npunkt);
  nallocpunkt = npunkt;
  ijdata = (float *) malloc(pollen*nallocpunkt*sizeof(float));
  kdata = (float *) malloc(nallocpunkt*sizeof(float));
  ldata = (float *) malloc(nallocpunkt*sizeof(float));
  ind = 0; 

  for (i=0; i < npunkt; i++) {
    ir = rband[i+1][1]; 
    jr = rband[i+1][2]; 
    kr = rband[i+1][3]; 
    lr = rband[i+1][4]; 

    ijdata[ind++] = 1.0;
    ijdata[ind++] = ir;
    ijdata[ind++] = jr;
    if (polgrad > 1)
      {
	ijdata[ind++] = ir * ir;
        ijdata[ind++] = ir * jr;
        ijdata[ind++] = jr * jr;
      }
    kdata[i] = kr; 
    ldata[i] = lr;
  }
  Info(0,"done, %d points read\n",npunkt);
  if (npunkt < pollen) 
    Error(7, 
	    "%s: minimum %d control points for polynom degree %d required\n",
	    argv[0], pollen, polgrad);

  Info(0, "Create transformation model ... ");
  M = MData(npunkt,pollen,ijdata);
  K = MData(npunkt,1,kdata);
  L = MData(npunkt,1,ldata);
  Mp = MPseudoInv(M);
  Q = MMult(Mp,K);
  R = MMult(Mp,L);
  Info(0, "done\n");

  if (qr)
    {
      antelem = Q->linjer * Q->soyler;
      printf("# Q = \n");
      for (elem=0; elem<antelem; elem++)
	printf(" %10g", Q->elem[elem]);
      printf("\n");
      antelem = R->linjer * R->soyler;
      printf("# R = \n");
      for (elem=0; elem<antelem; elem++)
	printf(" %10g", R->elem[elem]);
      printf("\n");

    }

  if (rss)
    {
      printf("#         i          j     Q(i,j)     R(i,j)          k          l        rss\n");
      for (i=0; i< npunkt; i++)
	{
	  ir = rband[i+1][1]; 
	  jr = rband[i+1][2];
	  kr = rband[i+1][3]; 
	  lr = rband[i+1][4];
	  dk = kr - Q->elem[0] - ir * Q->elem[1] - jr * Q->elem[2];
	  dl = lr - R->elem[0] - ir * R->elem[1] - jr * R->elem[2];
	  if (polgrad == 2)
	    {
	      dk = dk - ir * ir * Q->elem[3] 
		      - ir * jr * Q->elem[4] 
                      - jr * jr * Q->elem[5];
	      dl = dl - ir * ir * R->elem[3] 
		      - ir * jr * R->elem[4] 
                      - jr * jr * R->elem[5];
	    }
	  rs = sqrt(dk*dk + dl *dl);
	  printf(" %10g %10g %10g %10g %10g %10g %10g\n", 
		 ir, jr, kr-dk, lr-dl, kr, lr, rs);
	}
    }

  if (argc == 1) exit(0);
  innfil = argv[1];
  utfil = argv[2];
  imgi = Iread_image(innfil);
  if (Ipixtyp(imgi[1]) != Iu_byte_typ)
    Error(2, "Input band must have pixel type unsigned byte.\n");
  xsize = Ixsize(imgi[1]);
  ysize = Iysize(imgi[1]);

  if (argc == oargc)
    {
      imgo = Icopy_init(imgi);
      if (cxstart || cystart)
	for(i=1; i<=Inbands(imgo); i++)
	  {
	    xstart = cxstart ? atoi(cxstart) : Ixstart(imgo[i]);
	    ystart = cystart ? atoi(cystart) : Iystart(imgo[i]);
	    Iset_start(imgo[i], xstart, ystart);
	  }
    } else {
      xsize = cxsize ? atoi(cxsize) : ( (cxstop && cxstart) ?
	      atoi(cxstop) - atoi(cxstart) + 1 : 
	      Error(2, "Bad xsize specification\n")); 
      ysize = cysize ? atoi(cysize) : ( (cystop && cystart) ?
	      atoi(cystop) - atoi(cystart) + 1 : 
	      Error(2, "Bad xsize specification\n")); 
      imgo = Imake_image(Inbands(imgi), Ititle(imgi), 3, xsize, ysize);
      for(i=1; i<=Inbands(imgo); i++)
	{
	  xstart = cxstart ? atoi(cxstart) : Ixstart(imgi[i]);
	  ystart = cystart ? atoi(cystart) : Iystart(imgi[i]);
	  Iset_start(imgo[i], xstart, ystart);
	}
      Icopy_text(imgi, imgo);
    }

  for (band = 1; band<=Inbands(imgi); band++) 
    { 

      Info(0, "Output-to-input map band %d ... \n", band);
      switch(polgrad)
	{
	case 1:
	  for(i=0; i<3; i++) xxa[i] = Q->elem[i];
	  for(i=0; i<3; i++) yya[i] = R->elem[i];
	  affine(imgi[band], imgo[band], 
		 xxa[0], xxa[1], xxa[2],
		 yya[0], yya[1], yya[2], ip, bg);
	  break;
	case 2:
	  for(i=0; i<6; i++) xxa[i] = Q->elem[i];
	  for(i=0; i<6; i++) yya[i] = R->elem[i];
	  quadratic(imgi[band], imgo[band], xxa, yya, ip, bg);
	  break;
	}

    Info(0, "done\n");
  }

  Ihistory(imgo, argv[0], args);
  Iwrite_image(imgo, utfil);
  
  return(0);
}



