automatic commit
[folded-ctf.git] / pi_referential.cc
diff --git a/pi_referential.cc b/pi_referential.cc
new file mode 100644 (file)
index 0000000..6e7eb3e
--- /dev/null
@@ -0,0 +1,655 @@
+
+///////////////////////////////////////////////////////////////////////////
+// This program is free software: you can redistribute it and/or modify  //
+// it under the terms of the version 3 of the GNU General Public License //
+// as published by the Free Software Foundation.                         //
+//                                                                       //
+// 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, see <http://www.gnu.org/licenses/>.  //
+//                                                                       //
+// Written by Francois Fleuret, (C) IDIAP                                //
+// Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
+///////////////////////////////////////////////////////////////////////////
+
+#include "pi_referential.h"
+#include "global.h"
+#include "rich_image.h"
+
+void PiReferential::draw_frame(RGBImage *image,
+                               int registration_mode,
+                               int x1, int y1,
+                               int x2, int y2,
+                               int x3, int y3,
+                               int x4, int y4) {
+
+  int r, g, b;
+
+  switch(registration_mode) {
+
+  case PiReferential::RM_HEAD:
+    r = 0; g = 255; b = 0;
+    break;
+
+  case PiReferential::RM_HEAD_NO_POLARITY:
+    r = 128; g = 255; b = 128;
+    break;
+
+  case PiReferential::RM_BELLY:
+    r = 64; g = 0; b = 255;
+    break;
+
+  case PiReferential::RM_BELLY_NO_POLARITY:
+    r = 192; g = 128; b = 255;
+    break;
+
+  case PiReferential::RM_HEAD_BELLY:
+  case PiReferential::RM_HEAD_BELLY_EDGES:
+    r = 255; g = 0; b = 0;
+    break;
+
+  case PiReferential::RM_BODY:
+  case PiReferential::RM_BODY_EDGES:
+    r = 0; g = 128; b = 255;
+    break;
+
+  default:
+    cerr << "INCONSISTENCY" << endl;
+    abort();
+  }
+
+  if(global.pictures_for_article) {
+    r = 255; g = 255; b = 255;
+    image->draw_line(6, r, g, b, x1, y1, x2, y2);
+    image->draw_line(6, r, g, b, x2, y2, x3, y3);
+    image->draw_line(6, r, g, b, x3, y3, x4, y4);
+    image->draw_line(6, r, g, b, x4, y4, x1, y1);
+
+    r =   0; g =   0; b =   0;
+    image->draw_line(2, r, g, b, x1, y1, x2, y2);
+    image->draw_line(2, r, g, b, x2, y2, x3, y3);
+    image->draw_line(2, r, g, b, x3, y3, x4, y4);
+    image->draw_line(2, r, g, b, x4, y4, x1, y1);
+  } else {
+    //   int xc = (x1 + x2 + x3 + x4)/4, yc = (y1 + y2 + y3 + y4)/4;
+    //     image->draw_line(1, r, g, b, xc - delta, yc, xc + delta, yc);
+    //     image->draw_line(1, r, g, b, xc, yc - delta, xc, yc + delta);
+    image->draw_line(2, r, g, b, x1, y1, x2, y2);
+    image->draw_line(2, r, g, b, x2, y2, x3, y3);
+    image->draw_line(2, r, g, b, x3, y3, x4, y4);
+    image->draw_line(2, r, g, b, x4, y4, x1, y1);
+    //     image->draw_line(2, r, g, b,
+    //                      (2*xc + 5 * x1 + 5 * x2)/12, (2 * yc + 5 * y1 + 5 * y2)/12,
+    //                      (x1 + x2)/2, (y1 + y2)/2);
+    //     image->draw_line(6, r, g, b,
+    //                      (2*xc + 3 * x2 + 3 * x3)/8, (2 * yc + 3 * y2 + 3 * y3)/8,
+    //                      (x2 + x3)/2, (y2 + y3)/2
+    //                      );
+  }
+}
+
+void PiReferential::draw_window(RGBImage *image,
+                                int registration_mode, Rectangle *window,
+                                int filled) {
+  int r, g, b;
+
+  switch(registration_mode) {
+
+  case PiReferential::RM_HEAD:
+    r = 0; g = 255; b = 0;
+    break;
+
+  case PiReferential::RM_HEAD_NO_POLARITY:
+    r = 128; g = 255; b = 128;
+    break;
+
+  case PiReferential::RM_BELLY:
+    r = 64; g = 0; b = 255;
+    break;
+
+  case PiReferential::RM_BELLY_NO_POLARITY:
+    r = 192; g = 128; b = 255;
+    break;
+
+  case PiReferential::RM_HEAD_BELLY:
+  case PiReferential::RM_HEAD_BELLY_EDGES:
+    r = 255; g = 0; b = 0;
+    break;
+
+  case PiReferential::RM_BODY:
+  case PiReferential::RM_BODY_EDGES:
+    r = 0; g = 128; b = 255;
+    break;
+
+  default:
+    cerr << "INCONSISTENCY" << endl;
+    abort();
+  }
+
+  int xmin = int(window->xmin);
+  int ymin = int(window->ymin);
+  int xmax = int(window->xmax);
+  int ymax = int(window->ymax);
+
+  if(global.pictures_for_article) {
+    r = 255; g = 255; b = 255;
+    image->draw_line(6, r, g, b, xmin, ymin, xmax, ymin);
+    image->draw_line(6, r, g, b, xmax, ymin, xmax, ymax);
+    image->draw_line(6, r, g, b, xmax, ymax, xmin, ymax);
+    image->draw_line(6, r, g, b, xmin, ymax, xmin, ymin);
+
+//     if(filled) {
+//       int delta = 6;
+//       for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
+//         int x1 = xmin + d;
+//         int y1 = ymin;
+//         int x2 = xmin + d + ymax - ymin;
+//         int y2 = ymax;
+//         if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
+//         if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
+//         image->draw_line(3, r, g, b, x1, y1, x2, y2);
+//       }
+//     }
+
+    r =   0; g =   0; b =   0;
+    image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
+    image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
+    image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
+    image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
+
+//     if(filled) {
+//       int delta = 6;
+//       for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
+//         int x1 = xmin + d;
+//         int y1 = ymin;
+//         int x2 = xmin + d + ymax - ymin;
+//         int y2 = ymax;
+//         if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
+//         if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
+//         image->draw_line(1, r, g, b, x1, y1, x2, y2);
+//       }
+//     }
+  } else {
+    image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
+    image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
+    image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
+    image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
+    if(filled) {
+      int delta = 4;
+      for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
+        int x1 = xmin + d;
+        int y1 = ymin;
+        int x2 = xmin + d + ymax - ymin;
+        int y2 = ymax;
+        if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
+        if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
+        image->draw_line(1, r, g, b, x1, y1, x2, y2);
+      }
+    }
+  }
+
+}
+
+void PiReferential::draw_edge_and_scale(RGBImage *image,
+                                        int registration_mode, Rectangle *window,
+                                        int _tag, int _edge_scale) {
+  const int ref_radius = 10;
+  int r, g, b;
+  int edges = 0;
+
+  switch(registration_mode) {
+
+  case PiReferential::RM_HEAD:
+    r = 0; g = 255; b = 0;
+    break;
+
+  case PiReferential::RM_HEAD_NO_POLARITY:
+    r = 128; g = 255; b = 128;
+    break;
+
+  case PiReferential::RM_BELLY:
+    r = 64; g = 0; b = 255;
+    break;
+
+  case PiReferential::RM_BELLY_NO_POLARITY:
+    r = 192; g = 128; b = 255;
+    break;
+
+  case PiReferential::RM_HEAD_BELLY_EDGES:
+    edges = 1;
+  case PiReferential::RM_HEAD_BELLY:
+    r = 255; g = 0; b = 0;
+    break;
+
+  case PiReferential::RM_BODY_EDGES:
+    edges = 1;
+  case PiReferential::RM_BODY:
+    r = 0; g = 128; b = 255;
+    break;
+
+  default:
+    cerr << "INCONSISTENCY" << endl;
+    abort();
+  }
+
+  scalar_t xc = (window->xmin + window->xmax)/2;
+  scalar_t yc = (window->ymin + window->ymax)/2;
+  int radius = ref_radius * (1 << _edge_scale);
+
+  image->draw_ellipse(1, r, g, b, xc, yc, radius, radius, 0);
+
+  if(_tag >= RichImage::first_edge_tag && _tag < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
+
+    scalar_t dx, dy;
+
+    switch(_tag - RichImage::first_edge_tag) {
+    case 0:
+      dx =  0; dy = -1;
+      break;
+
+    case 1:
+      dx =  1; dy = -1;
+      break;
+
+    case 2:
+      dx =  1; dy =  0;
+      break;
+
+    case 3:
+      dx =  1; dy =  1;
+      break;
+
+    case 4:
+      dx =  0; dy =  1;
+      break;
+
+    case 5:
+      dx = -1; dy =  1;
+      break;
+
+    case 6:
+      dx = -1; dy =  0;
+      break;
+
+    case 7:
+      dx = -1; dy = -1;
+      break;
+
+    default:
+      abort();
+    }
+
+    scalar_t l = sqrt(dx * dx + dy * dy);
+
+//     dx = dx / l;
+//     dy = dy / l;
+
+    if(edges) {
+      int delta = 3;
+      image->draw_ellipse(1, r, g, b, xc, yc, radius + delta, radius + delta, 0);
+    }
+
+    for(scalar_t u = 0; u <= radius; u += 0.1) {
+      scalar_t s = sqrt(radius * radius - (u * u * l * l))/l;
+      image->draw_line(2, r, g, b,
+                       int(xc + u * dx - s * dy), int(yc + u * dy + s * dx),
+                       int(xc + u * dx + s * dy), int(yc + u * dy - s * dx));
+    }
+
+//     for(int y = yc - radius; y <= yc + radius; y++) {
+//       for(int x = xc - radius; x <= xc + radius; x++) {
+//         if(x >= 0 && x < image->width() && y >= 0 && y < image->height() &&
+//            (x - xc) * dx + (y - yc) * dy >= 0) {
+//           image->draw_point(r, g, b, x, y);
+//         }
+//       }
+//     }
+
+  }
+
+  else if(_tag == RichImage::variance_tag) {
+    image->draw_ellipse(1, r, g, b, xc, yc, 8, 8, 0);
+  }
+
+  //   else if(_tag >= RichImage::first_gray_tag && _tag < RichImage::first_gray_tag + RichImage::nb_gray_tags) {
+  //   }
+}
+
+PiReferential::PiReferential(PoseCell *cell) {
+  scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
+
+  _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
+
+  scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
+
+  //////////////////////////////////////////////////////////////////////
+  // Locations and scales
+
+  // Head location
+
+  _head_xc = cell->_head_xc.middle() * discrete_scale_ratio;
+  _head_yc = cell->_head_yc.middle() * discrete_scale_ratio;
+  _head_radius = cell->_head_radius.middle() * discrete_scale_ratio;
+  _head_window_scaling = _head_radius * 2.0;
+
+  // Body location
+
+  _body_xc = cell->_body_xc.middle() * discrete_scale_ratio;
+  _body_yc = cell->_body_yc.middle() * discrete_scale_ratio;
+  _body_window_scaling = sqrt(_body_radius_1 * _body_radius_2);
+
+  if((_head_xc - _body_xc) * cos(_body_tilt) + (_head_yc - _body_yc) * sin(_body_tilt) > 0) {
+    _body_tilt += M_PI;
+  }
+
+  // Belly location
+
+  const scalar_t belly_frame_factor = 2.0;
+
+  _belly_xc = _body_xc;
+  _belly_yc = _body_yc;
+  _belly_window_scaling = _head_window_scaling * belly_frame_factor;
+
+  // Head-belly location
+
+  _head_belly_xc = (_head_xc + _body_xc) * 0.5;
+  _head_belly_yc = (_head_yc + _body_yc) * 0.5;
+
+  //////////////////////////////////////////////////////////////////////
+  // Frames
+
+  if(_body_xc >= _head_xc) {
+    _horizontal_polarity = 1;
+  } else {
+    _horizontal_polarity = -1;
+  }
+
+  // Head frame
+
+  if(_horizontal_polarity < 0) {
+    _head_ux = _head_radius * 2.0;
+    _head_uy = 0;
+  } else {
+    _head_ux = - _head_radius * 2.0;
+    _head_uy = 0;
+  }
+
+  _head_vx = 0;
+  _head_vy = - _head_radius * 2.0;
+
+  _head_ux_nopolarity = _head_radius * 2.0;
+  _head_uy_nopolarity = 0;
+  _head_vx_nopolarity = 0;
+  _head_vy_nopolarity = - _head_radius * 2.0;
+
+  // Belly frame
+
+  _belly_ux = _head_ux * belly_frame_factor;
+  _belly_uy = _head_uy * belly_frame_factor;
+  _belly_vx = _head_vx * belly_frame_factor;
+  _belly_vy = _head_vy * belly_frame_factor;
+
+  _belly_ux_nopolarity = _head_ux_nopolarity * belly_frame_factor;
+  _belly_uy_nopolarity = _head_uy_nopolarity * belly_frame_factor;
+  _belly_vx_nopolarity = _head_vx_nopolarity * belly_frame_factor;
+  _belly_vy_nopolarity = _head_vy_nopolarity * belly_frame_factor;
+
+  // Head-belly frame
+
+  _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
+  _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
+
+  if(_horizontal_polarity < 0) {
+    _head_belly_vx =   _head_belly_uy;
+    _head_belly_vy = - _head_belly_ux;
+  } else {
+    _head_belly_vx = - _head_belly_uy;
+    _head_belly_vy =   _head_belly_ux;
+  }
+
+  scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
+
+  _head_belly_vx = _head_belly_vx/l * _head_radius * 2;
+  _head_belly_vy = _head_belly_vy/l * _head_radius * 2;
+  _head_belly_edge_shift = int(floor(- RichImage::nb_edge_tags * atan2(_head_belly_ux, _head_belly_uy) / (2 * M_PI) + 0.5));
+  _head_belly_edge_shift = (RichImage::nb_edge_tags + _head_belly_edge_shift) % RichImage::nb_edge_tags;
+
+  // Body frame
+
+  _body_ux =   cos(_body_tilt) * _body_radius_1 * 2.0;
+  _body_uy =   sin(_body_tilt) * _body_radius_1 * 2.0;
+  _body_vx = - sin(_body_tilt) * _body_radius_2 * 2.0;
+  _body_vy =   cos(_body_tilt) * _body_radius_2 * 2.0;
+
+  _body_edge_shift = int(floor(RichImage::nb_edge_tags * _body_tilt / (2 * M_PI) + 0.5));
+  _body_edge_shift = (RichImage::nb_edge_tags + _body_edge_shift) % RichImage::nb_edge_tags;
+}
+
+int PiReferential::common_scale() {
+  return _common_scale;
+}
+
+void PiReferential::register_rectangle(int registration_mode,
+                                       Rectangle *original,
+                                       Rectangle *result) {
+  scalar_t alpha, beta , xc, yc, w, h;
+
+  alpha = (original->xmin + original->xmax) * 0.5;
+  beta  = (original->ymin + original->ymax) * 0.5;
+
+  switch(registration_mode) {
+
+  case RM_HEAD:
+    {
+      xc = _head_xc + alpha * _head_ux + beta * _head_vx;
+      yc = _head_yc + alpha * _head_uy + beta * _head_vy;
+      w = (original->xmax - original->xmin) * _head_window_scaling;
+      h = (original->ymax - original->ymin) * _head_window_scaling;
+    }
+    break;
+
+  case RM_HEAD_NO_POLARITY:
+    {
+      xc = _head_xc + alpha * _head_ux_nopolarity + beta * _head_vx_nopolarity;
+      yc = _head_yc + alpha * _head_uy_nopolarity + beta * _head_vy_nopolarity;
+      w = (original->xmax - original->xmin) * _head_window_scaling;
+      h = (original->ymax - original->ymin) * _head_window_scaling;
+    }
+    break;
+
+  case RM_BELLY:
+    {
+      xc = _belly_xc + alpha * _belly_ux + beta * _belly_vx;
+      yc = _belly_yc + alpha * _belly_uy + beta * _belly_vy;
+      w = (original->xmax - original->xmin) * _belly_window_scaling;
+      h = (original->ymax - original->ymin) * _belly_window_scaling;
+    }
+    break;
+
+  case RM_BELLY_NO_POLARITY:
+    {
+      xc = _belly_xc + alpha * _belly_ux_nopolarity + beta * _belly_vx_nopolarity;
+      yc = _belly_yc + alpha * _belly_uy_nopolarity + beta * _belly_vy_nopolarity;
+      w = (original->xmax - original->xmin) * _belly_window_scaling;
+      h = (original->ymax - original->ymin) * _belly_window_scaling;
+    }
+    break;
+
+  case RM_HEAD_BELLY:
+  case RM_HEAD_BELLY_EDGES:
+    {
+      xc = _head_belly_xc + alpha * _head_belly_ux + beta * _head_belly_vx;
+      yc = _head_belly_yc + alpha * _head_belly_uy + beta * _head_belly_vy;
+      w = (original->xmax - original->xmin) * _head_window_scaling;
+      h = (original->ymax - original->ymin) * _head_window_scaling;
+    }
+    break;
+
+  case RM_BODY:
+  case RM_BODY_EDGES:
+    {
+      xc = _body_xc + alpha * _body_ux + beta * _body_vx;
+      yc = _body_yc + alpha * _body_uy + beta * _body_vy;
+      w = (original->xmax - original->xmin) * _body_window_scaling;
+      h = (original->ymax - original->ymin) * _body_window_scaling;
+    }
+    break;
+
+  default:
+    cerr << "Undefined registration mode." << endl;
+    abort();
+  }
+
+  result->xmin = xc - 0.5 * w;
+  result->ymin = yc - 0.5 * h;
+  result->xmax = xc + 0.5 * w;
+  result->ymax = yc + 0.5 * h;
+
+  ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
+}
+
+int PiReferential::register_edge(int registration_mode, int edge_type) {
+
+  if(edge_type >= RichImage::first_edge_tag &&
+     edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
+
+    int e = edge_type - RichImage::first_edge_tag;
+
+    switch(registration_mode) {
+    case PiReferential::RM_HEAD_NO_POLARITY:
+    case PiReferential::RM_BELLY_NO_POLARITY:
+      break;
+
+    case PiReferential::RM_HEAD:
+    case PiReferential::RM_BELLY:
+    case PiReferential::RM_HEAD_BELLY:
+    case PiReferential::RM_BODY:
+      if(_horizontal_polarity < 0) {
+        e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
+      }
+      break;
+
+    case PiReferential::RM_HEAD_BELLY_EDGES:
+      if(_horizontal_polarity < 0) {
+        e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
+      }
+      e += _head_belly_edge_shift;
+      break;
+
+    case PiReferential::RM_BODY_EDGES:
+      if(_horizontal_polarity < 0) {
+        e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
+      }
+      e += _body_edge_shift;
+      break;
+
+    default:
+      cerr << "INCONSISTENCY" << endl;
+      abort();
+    }
+
+    e = e % RichImage::nb_edge_tags;
+
+    return RichImage::first_edge_tag + e;
+
+  }
+
+  else return edge_type;
+}
+
+void PiReferential::draw(RGBImage *image, int level) {
+  int x1, y1, x2, y2, x3, y3, x4, y4;
+
+  if(level >= 2) {
+
+    // Draw the RM_BODY reference frame
+
+    x1 = int(_body_xc + _body_ux + _body_vx);
+    y1 = int(_body_yc + _body_uy + _body_vy);
+    x2 = int(_body_xc - _body_ux + _body_vx);
+    y2 = int(_body_yc - _body_uy + _body_vy);
+    x3 = int(_body_xc - _body_ux - _body_vx);
+    y3 = int(_body_yc - _body_uy - _body_vy);
+    x4 = int(_body_xc + _body_ux - _body_vx);
+    y4 = int(_body_yc + _body_uy - _body_vy);
+
+    draw_frame(image, RM_BODY, x1, y1, x2, y2, x3, y3, x4, y4);
+  }
+
+  if(level >= 1) {
+
+    // Draw the RM_BELLY reference frame
+
+    x1 = int(_belly_xc + _belly_ux + _belly_vx);
+    y1 = int(_belly_yc + _belly_uy + _belly_vy);
+    x2 = int(_belly_xc - _belly_ux + _belly_vx);
+    y2 = int(_belly_yc - _belly_uy + _belly_vy);
+    x3 = int(_belly_xc - _belly_ux - _belly_vx);
+    y3 = int(_belly_yc - _belly_uy - _belly_vy);
+    x4 = int(_belly_xc + _belly_ux - _belly_vx);
+    y4 = int(_belly_yc + _belly_uy - _belly_vy);
+
+    draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
+
+    // Draw the RM_HEAD_BELLY reference frame
+
+    x1 = int(_head_belly_xc + _head_belly_ux + _head_belly_vx);
+    y1 = int(_head_belly_yc + _head_belly_uy + _head_belly_vy);
+    x2 = int(_head_belly_xc - _head_belly_ux + _head_belly_vx);
+    y2 = int(_head_belly_yc - _head_belly_uy + _head_belly_vy);
+    x3 = int(_head_belly_xc - _head_belly_ux - _head_belly_vx);
+    y3 = int(_head_belly_yc - _head_belly_uy - _head_belly_vy);
+    x4 = int(_head_belly_xc + _head_belly_ux - _head_belly_vx);
+    y4 = int(_head_belly_yc + _head_belly_uy - _head_belly_vy);
+
+    draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
+  }
+
+  // Draw the RM_HEAD reference frame
+
+  x1 = int(_head_xc + _head_ux + _head_vx);
+  y1 = int(_head_yc + _head_uy + _head_vy);
+  x2 = int(_head_xc - _head_ux + _head_vx);
+  y2 = int(_head_yc - _head_uy + _head_vy);
+  x3 = int(_head_xc - _head_ux - _head_vx);
+  y3 = int(_head_yc - _head_uy - _head_vy);
+  x4 = int(_head_xc + _head_ux - _head_vx);
+  y4 = int(_head_yc + _head_uy - _head_vy);
+
+  draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
+}
+
+void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
+  switch(registration_mode) {
+  case RM_HEAD:
+    (*out) << "RM_HEAD";
+    break;
+  case RM_HEAD_NO_POLARITY:
+    (*out) << "RM_HEAD_NO_POLARITY";
+    break;
+  case RM_BELLY:
+    (*out) << "RM_BELLY";
+    break;
+  case RM_BELLY_NO_POLARITY:
+    (*out) << "RM_BELLY_NO_POLARITY";
+    break;
+  case RM_HEAD_BELLY:
+    (*out) << "RM_HEAD_BELLY";
+    break;
+  case RM_HEAD_BELLY_EDGES:
+    (*out) << "RM_HEAD_BELLY_EDGES";
+    break;
+  case RM_BODY:
+    (*out) << "RM_BODY";
+    break;
+  case RM_BODY_EDGES:
+    (*out) << "RM_BODY_EDGES";
+    break;
+  default:
+    abort();
+  }
+}