nonmax.cc

用于计算矩阵的特征值,及矩阵的其他运算.可以用与稀疏矩阵

// Copyright (C) 2002 Charless C. Fowlkes <[e-mail]>
// Copyright (C) 2002 David R. Martin <[e-mail]>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
// 02111-1307, USA, or see http://www.gnu.org/copyleft/gpl.html.


#include <assert.h>
#include <limits.h>
#include <float.h>
#include <math.h>
#include <stdlib.h>
#include <iostream>
#include <stdio.h>
#include <ctype.h>
#include <memory.h>
#include <time.h>

#include "filterbank.hh"
#include "nonmax.hh"
#include "util.hh"
#include "image.hh"

namespace Group
{

  /////////////////////////////////////////////////////////////////////////////////////////////////////////
  //
  // data structure for a set of subpixel edgels
  //

  ParabolaSet::ParabolaSet (const int width, const int height)
  {
    resize(width,height);
  }

  ParabolaSet::ParabolaSet (const ParabolaSet& that)
  {
      maxo = that.maxo;
      maxe = that.maxe;
      edgemap = that.edgemap;
      X = that.X;
      Y = that.Y;
      orientation = that.orientation;
      energy = that.energy;
      fiterror = that.fiterror;
      curvature = that.curvature;
      dist = that.dist;
      smooth = that.smooth;
  }

  void ParabolaSet::resize(const int width, const int height)
  {
      maxo.resize(width,height);
      maxe.resize(width,height);
      edgemap.resize(width,height);
      X.resize(width,height);
      Y.resize(width,height);
      orientation.resize(width,height);
      energy.resize(width,height);
      fiterror.resize(width,height);
      curvature.resize(width,height);
      dist.resize(width,height);
      smooth.resize(width,height);
  }

  void ParabolaSet::init()
  {
      maxo.init(0);
      maxe.init(0);
      edgemap.init(0);
      X.init(0);
      Y.init(0);
      orientation.init(0);
      energy.init(0);
      fiterror.init(0);
      curvature.init(0);
      dist.init(0);
      smooth.init(0);
  }

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////
  //
  // fit cylindrical parabola to a circular patch of radius r centered
  // at (x,y) at angle theta.  return parabola coefficients and
  // the normalized fit error.  z = ax^2 + bx + c
  //
  void fitParabolaLeastSqr (
      const int x, const int y,
      const float theta, const float r,
      const Util::Image& z,
      float& a, float& b, float& c, float& error)
  {
      a = b = c = 0;
      error = 0;

      const int width = z.size(0);
      const int height = z.size(1);
      const int rad = (int) ceil (r);
      const float sint = sin(theta);
      const float cost = cos(theta);

      // d := projection of the 3x3 grid onto line perpendicular to theta
      // di := Sum over u,v of d[u,v]^i
      float d0 = 0, d1 = 0, d2 = 0, d3 = 0, d4 = 0;
      float v0 = 0, v1 = 0, v2 = 0;
      for (int v = -rad; v <= rad; v++) {
          for (int u = -rad; u <= rad; u++) {
              const int xi = x + u;
              const int yi = y + v;
              if (xi < 0 || xi >= width) { continue; }
              if (yi < 0 || yi >= height) { continue; }
              if (u*u + v*v > r*r) { continue; }
              const float zi = z(xi,yi);
              const float di = -u*sint + v*cost; // distance to diameter
              d0 += 1;
              d1 += di;
              d2 += di * di;
              d3 += di * di * di;
              d4 += di * di * di * di;
              v0 += zi;
              v1 += di * zi;
              v2 += di * di * zi;
          }
      }

      // solve the linear system A [c b a] = v
      // A[3][3] = {{d0, d1, d2},
      //            {d1, d2, d3},
      //            {d2, d3, d4}}
      const float detA =
          -d2*d2*d2 + 2*d1*d2*d3 - d0*d3*d3 - d1*d1*d4 + d0*d2*d4;
      const float invA[3][3] = {
          {-d3*d3 + d2*d4,  d2*d3 - d1*d4, -d2*d2 + d1*d3},
          { d2*d3 - d1*d4, -d2*d2 + d0*d4,  d1*d2 - d0*d3},
          {-d2*d2 + d1*d3,  d1*d2 - d0*d3, -d1*d1 + d0*d2}
      };

      if (detA != 0)
      {
        c = (invA[0][0] * v0 + invA[0][1] * v1 + invA[0][2] * v2) / detA;
        b = (invA[1][0] * v0 + invA[1][1] * v1 + invA[1][2] * v2) / detA;
        a = (invA[2][0] * v0 + invA[2][1] * v1 + invA[2][2] * v2) / detA;
      }
      else
      {
        //parabola fit failed due to invertability issue.
        //this can happen for very small fit radius when
        //we are clipped by an image boundary.  set some
        //default values so that we never choose this as
        //a boundary location
        c = 0.0f;
        b = 10.0f;
        a = 1.0f;
      }

      //compute residual error
      float znorm = 0;
      float dznorm = 0;
      for (int v = -rad; v <= rad; v++) {
          for (int u = -rad; u <= rad; u++) {
              const int xi = x + u;
              const int yi = y + v;
              if (xi < 0 || xi >= width) { continue; }
              if (yi < 0 || yi >= height) { continue; }
              if (u*u + v*v > r*r) { continue; }
              const float zi = z(xi,yi);
              const float di = -u*sint + v*cost;
              znorm += zi * zi;
              const float dzi = a*di*di + b*di + c - zi;
              dznorm += dzi * dzi;
          }
      }
      if (znorm != 0) {
          error = sqrt (dznorm / znorm); // normalized RMS error
      }
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //
  // simple nonmax suppression based on linear interpolation
  // for a 3x3 neighborhood.
  //
  // single orientation 
  //
  void nonmaxSuppress(const Util::Image& mag, const float theta, Util::Image& suppressed)
  {
    const int width = mag.size(0);
    const int height = mag.size(1);
    suppressed.resize(width,height);

    float tval = Util::mod(theta + (M_PI/2),M_PI);

    for (int y = 1; y < height-1; y++)
    {
      for (int x = 1; x < width-1; x++)
      {
         float mval = mag(x,y);
         suppressed(x,y) = mval;

         if ((tval >= 0) && (tval < (M_PI / 4)))
         {
           float d = tan(tval);
           float v1 = (1-d)*mag(x+1,y) + d*mag(x+1,y+1);
           float v2 = (1-d)*mag(x-1,y) + d*mag(x-1,y-1);
           if ((v2 > mval) || (v1 > mval))
           {
             suppressed(x,y) = 0;
           }
         }
         else if ((tval >= (M_PI/4)) && (tval < (M_PI / 2)))
         {
           float d = tan((M_PI/2) - tval);
           float v1 = (1-d)*mag(x,y+1) + d*mag(x+1,y+1);
           float v2 = (1-d)*mag(x,y-1) + d*mag(x-1,y-1);
           if ((v2 > mval) || (v1 > mval))
           {
             suppressed(x,y) = 0;
           }
         }
         else if ((tval >= (M_PI/2)) && (tval < (3*M_PI / 4)))
         {
           float d = tan(tval - (M_PI/2));
           float v1 = (1-d)*mag(x,y+1) + d*mag(x-1,y+1);
           float v2 = (1-d)*mag(x,y-1) + d*mag(x+1,y-1);
           if ((v2 > mval) || (v1 > mval))
           {
             suppressed(x,y) = 0;
           }
         }
         else if ((tval >= (3*M_PI/4)) && (tval < M_PI))
         {
           float d = tan(M_PI - tval);
           float v1 = (1-d)*mag(x-1,y) + d*mag(x-1,y+1);
           float v2 = (1-d)*mag(x+1,y) + d*mag(x+1,y-1);
           if ((v2 > mval) || (v1 > mval))
           {
             suppressed(x,y) = 0;
           }
         }
      }
    }
  }

  //
  // simple nonmax suppression based on linear interpolation
  // for a 3x3 neighborhood.
  //
  // oreintation per pixel
  //
  void nonmaxSuppress(const Util::Image& mag, const Util::Image& theta, Util::Image& suppressed)
  {
    const int width = mag.size(0);
    const int height = mag.size(1);
    suppressed.resize(width,height);

    for (int y = 1; y < height-1; y++)
    {
      for (int x = 1; x < width-1; x++)
      {
         float tval = Util::mod(theta(x,y) + (M_PI/2),M_PI);
         float mval = mag(x,y);
         suppressed(x,y) = mval;

         if ((tval >= 0) && (tval < (M_PI / 4)))
         {
           float d = tan(tval);
           float v1 = (1-d)*mag(x+1,y) + d*mag(x+1,y+1);
           float v2 = (1-d)*mag(x-1,y) + d*mag(x-1,y-1);
           if ((v2 > mval) || (v1 > mval))
           {
             suppressed(x,y) = 0;
           }
         }
         else if ((tval >= (M_PI/4)) && (tval < (M_PI / 2)))
         {
           float d = tan((M_PI/2) - tval);
           float v1 = (1-d)*mag(x,y+1) + d*mag(x+1,y+1);
           float v2 = (1-d)*mag(x,y-1) + d*mag(x-1,y-1);
           if ((v2 > mval) || (v1 > mval))
           {
             suppressed(x,y) = 0;
           }
         }
         else if ((tval >= (M_PI/2)) && (tval < (3*M_PI / 4)))
         {
           float d = tan(tval - (M_PI/2));
           float v1 = (1-d)*mag(x,y+1) + d*mag(x-1,y+1);
           float v2 = (1-d)*mag(x,y-1) + d*mag(x+1,y-1);
           if ((v2 > mval) || (v1 > mval))
           {
             suppressed(x,y) = 0;
           }
         }
         else if ((tval >= (3*M_PI/4)) && (tval < M_PI))
         {
           float d = tan(M_PI - tval);
           float v1 = (1-d)*mag(x-1,y) + d*mag(x-1,y+1);
           float v2 = (1-d)*mag(x+1,y) + d*mag(x+1,y-1);
           if ((v2 > mval) || (v1 > mval))
           {
             suppressed(x,y) = 0;
           }
         }
      }
    }
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //
  // interpolate max using 3 point technique
  //
  static void
  interpolateMaxLagrange (const float x1, const float y1,
                          const float x2, const float y2,
                          const float x3, const float y3,
                          float &x, float &y)
  {
      assert (x1 < x2);
      assert (x2 < x3);

      assert (y2 >= y1);
      assert (y2 >= y3);

      // Location of max.
      x = (x3 * x3 * (y1 - y2) + x1 * x1 * (y2 - y3) + x2 * x2 * (y3 - y1))
          / (2. * (x3 * (y1 - y2) + x1 * (y2 - y3) + x2 * (y3 - y1)));

      // Lagrange interpolation to find max value.
      y = y1 * ((x - x2) * (x - x3)) / ((x1 - x2) * (x1 - x3))
          + y2 * ((x - x1) * (x - x3)) / ((x2 - x1) * (x2 - x3))
          + y3 * ((x - x1) * (x - x2)) / ((x3 - x1) * (x3 - x2));

      if (!finite (x) || !finite (y))
        {
            x = x2;
            y = y2;
        }
  }


  //
  // fit clyndrical parabolas at given max orientation and return the
  // resulting subpixel info
  // r = radius of disc on which we fit the cylindrical parabola
  // theta = orientation of fits
  //
  void fitSubpixelParabolas (const Util::Image& mag, const float theta, 
                             float r, ParabolaSet& parabs)
  {
      const int width = mag.size(0);
      const int height = mag.size(1);
      parabs.resize(width,height);
      parabs.init();

      r = Util::max(r,1.5f); 
      //compute the best paraboloid fit at each point (x,y)
      for (int x = 0; x < width; x++)
      {
        for (int y = 0; y < height; y++)
        {
              float maxe = mag(x,y);
              assert (theta >= 0 && theta < M_PI);

              // save the maximal orientation and energy at all pixels
              parabs.maxo(x,y) = theta;
              parabs.maxe(x,y) = maxe;

              // these are the least squares paraboloid coeff. estimates
              // for z = a*d^2 + b*d + c, where d = -x*sint + y*cost
              // error is the normalized fit error
              float a = 0, b = 0, c = 0;
              float error = 0;

              fitParabolaLeastSqr ( x, y, theta, r, mag, a, b, c, error);

              // compute various parameters of fitted parabola
              // curvature is normalized to the window radius
              const float curvature = a * r * r;
              const float dist = -b / (2*a);
              const float energy = a*dist*dist + b*dist + c;
              const float smooth = Util::max(c,0.0f);
              const float ycoord = dist*cos(theta);
              const float xcoord = -dist*sin(theta);

              // save fit info for all putative needles
              parabs.X(x,y) = xcoord;
              parabs.Y(x,y) = ycoord;
              parabs.energy(x,y) = energy;
              parabs.orientation(x,y) = theta;
              parabs.curvature(x,y) = curvature;
              parabs.fiterror(x,y) = error;
              parabs.dist(x,y) = dist;
              parabs.smooth(x,y) = smooth;

              // if any of the values are infinite or NaN, then skip
              if (!finite(xcoord)) { continue; }
              if (!finite(ycoord)) { continue; }
              if (!finite(energy)) { continue; }
              if (!finite(theta)) { continue; }
              if (!finite(curvature)) { continue; }
              if (!finite(error)) { continue; }

              // if curvature is positive, then this is not a max; skip
              if (curvature > -1e-10) { continue; }

              // if the center is out of this pixel's neighborhood, then skip
              if (fabs(dist) > 1) { continue; }

              // only mark valid those needles that are maxima in the
              // pixel's immediate neighborhood
              parabs.edgemap(x,y) = 1;
          }
      }
  }


  //
  // fit clyndrical parabolas at given max orientation and return the
  // resulting subpixel info
  // r = radius of square to which we fit the cylindrical parabola
  //
  void fitSubpixelParabolas (const Util::Image& mag, const Util::Image& theta, 
                             float r, ParabolaSet& parabs)
  {
      const int width = mag.size(0);
      const int height = mag.size(1);
      parabs.resize(width,height);
      parabs.init();