/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/calibration.h

Go to the documentation of this file.

// Copyright (C) 2011 Soren Hauberg
//
// 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 3
// 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, see <http://www.gnu.org/licenses/>.
 
#ifndef CALIBRATION_H
#define CALIBRATION_H

#include "options.h"
#include "cxcore.h"
#include <string>

class calibration
{
public:
  calibration (const options &opts)
    : intrinsic (opts.intrinsic_matrix ())
    {
      load_world (opts.external_view ());
    }
  
  const matrix3& get_intrinsic_matrix () const
    {
      return intrinsic;
    }
  
  CvPoint camera_to_pixel (const vector3 &v, double scale = 1.0) const
    {
      CvPoint retval;
      
      const vector3 p = intrinsic * v;
      // We add 0.5 to ensure pixel coordinates are rounded rather than floored (retval contains integers).
      retval.x = scale * p (0) / p (2) + 0.5;
      retval.y = scale * p (1) / p (2) + 0.5;
      
      return retval;
    }
  
   CvPoint world_to_pixel (const vector3 &v, double scale = 1.0) const
    {
      const vector3 c = world_to_camera (v);
      return camera_to_pixel (c, scale);
    }
  
  vector3 pixel_to_camera (const CvPoint p, double scale = 1.0) const
    {
      vector3 retval;
      retval [0] = (((double)p.x) / scale - intrinsic (0, 2)) / intrinsic (0, 0);
      retval [1] = (((double)p.y) / scale - intrinsic (1, 2)) / intrinsic (1, 1);
      retval [2] = 1.0;
      return retval;
    }
  
  vector3 pixel_to_world (const CvPoint p, double scale = 1.0) const
    {
      const vector3 c = pixel_to_camera (p, scale);
      return camera_to_world (c);
    }
  
  vector3 world_to_camera (const vector3 &v) const
    {
      // XXX: 100 is a hack!!
      const vector3 retval = 100.0 * (inv_world_R * v - world_translation);
      return retval;
    }
  
  vector3 camera_to_world (const vector3 &v) const
    {
      // XXX: 100 is a hack!!
      const vector3 retval = world_R * (v / 100.0 + world_translation);
      return retval;
    }
  
 vector3 camera_centre () const
   {
     return camera_to_world (vector3 (0, 0, 0));
   }
    
  void set_world_translation (const vector3 &t)
    {
      world_translation = t;
    };
      
  vector3 get_world_translation () const
    {
      return world_translation;
    };
      
  void set_world_rotation (const vector3 &R)
    {
      world_rot_vec = R;
  
      const matrix3 rxm = OpenTissue::math::Rx (R [0]);
      const matrix3 rym = OpenTissue::math::Ry (R [1]);
      const matrix3 rzm = OpenTissue::math::Rz (R [2]);
  
      world_R = rxm * rym * rzm;

      const matrix3 irxm = OpenTissue::math::Rx (-R [0]);
      const matrix3 irym = OpenTissue::math::Ry (-R [1]);
      const matrix3 irzm = OpenTissue::math::Rz (-R [2]);
  
      inv_world_R = irzm * irym * irxm;
    };

  vector3 get_world_rotation () const
    {
      return world_rot_vec;
    };

  bool save_world (const std::string filename) const
    {
      bool retval = false;
      std::ofstream rtfile (filename.c_str ());
      if (rtfile.is_open ())
        {
          rtfile << world_translation [0] << " " << world_translation [1] << " "
                 << world_translation [2] << std::endl;
          rtfile << world_rot_vec [0] << " " << world_rot_vec [1] << " "
                 << world_rot_vec [2] << std::endl;
          rtfile.close ();
          retval = true;
        }
      else
        std::cerr << "Could not open '" << filename << "' for writing" << std::endl;
    
      return retval;
    };

  bool load_world (const std::string filename)
    {
      bool retval = false;
      std::ifstream rtfile (filename.c_str ());
      if (rtfile.is_open ())
        {
          for (size_t k = 0; k < 3; k++)
            rtfile >> world_translation [k];
          for (size_t k = 0; k < 3; k++)
            rtfile >> world_rot_vec [k];
          rtfile.close ();
          set_world_rotation (world_rot_vec);
          retval = true;
        }
      else
        std::cerr << "Could not open '" << filename << "' for reading" << std::endl;
    
      return retval;
    };

private:
  matrix3 intrinsic, world_R, inv_world_R;
  vector3 world_translation, world_rot_vec;
};

#endif // CALIBRATION_H