/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/cloud_and_stick.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 CLOUD_AND_STICK_H
#define CLOUD_AND_STICK_H

#include "point_cloud.h"
#include "auxil.h"
#include <utility>
#include <vector>

typedef std::pair<vector3, vector3> stick_type;

// Auxilary function for computing distance between a stick and a point
inline
vector3 nearest_point_on_stick (const stick_type &stick, const double alpha)
{
  const vector3 p = alpha * stick.second + (1.0 - alpha) * stick.first;
  return p;
}

inline
double dist2 (const stick_type &stick, const vector3 &x0, double &alpha)
{
  const vector3 x1 = stick.first;
  const vector3 x2 = stick.second;
  const vector3 d01 = x0 - x1;
  const vector3 d21 = x2 - x1;
  alpha = (d01 * d21) / (d21 * d21);
  alpha = force_in_range (alpha, 0.0, 1.0);
  
  const vector3 p = nearest_point_on_stick (stick, alpha);
  const vector3 d = p - x0;
  const double retval = d * d;
  
  return retval;
}

inline double dist2 (const stick_type &stick, const vector3 &x0)
{
  double alpha;
  return dist2 (stick, x0, alpha);
}

template <class cloud_type>
class cloud_and_stick : public cloud_type
{
  public:
    cloud_and_stick (const options &opts, const calibration &_calib)
      : cloud_type (opts, _calib),
//        sticks_index (0)
//        sticks_index (opts.data_start_idx () + 21) // XXX: How should this be initialised?
        sticks_index (opts.data_start_idx ()) // XXX: How should this be initialised?
      {
        FILE *fid = fopen (opts.stick_filename ().c_str (), "r");
        const bool found = (fid != NULL);
        if (!found)
          {
            std::cerr << "cloud_and_stick::cloud_and_stick: could not open '"
                      << opts.stick_filename () << "' for reading." << std::endl;
            exit (-1);
          }
        else
          {
            // Count number of lines in file
            // XXX: This is currently not stable as it depends on 'buf_size'
            const int buf_size = 1000;
            char line [buf_size];
            int num_sticks = 0;
            while (fgets (line, buf_size, fid) != NULL)
              num_sticks ++;

            // Allocate data storage
            sticks.resize (num_sticks);
            points_on_sticks.resize (num_sticks);
            
            // Read the data
            rewind (fid);
            const int num_points = 2;
            for (int k = 0; k < num_sticks; k++)
              {
                points_on_sticks [k] = new std::vector<vector3> (num_points);
                for (int n = 0; n < num_points; n++)
                  {
                    double x, y, z;
                    const int num_read = fscanf (fid, "%lf %lf %lf", &x, &y, &z);
                    if (num_read != 3)
                      {
                        std::cerr << "cloud_and_stick: invalid stick data" << std::endl;
                        break;
                      }
                    const vector3 v = this->calib.camera_to_world (vector3 (x, y, z));

                    (*(points_on_sticks [k])) [n] = v;
                  }
                const int num_read = fscanf (fid, "\n");
                if (num_read != 0)
                  {
                    std::cerr << "cloud_and_stick: invalid line ending in stick data" << std::endl;
                  }
                sticks [k].first  = (*(points_on_sticks [k])) [0];
                sticks [k].second = (*(points_on_sticks [k])) [num_points-1];
              }
            fclose (fid);
          }
      }
    
    ~cloud_and_stick ()
      {
        for (size_t k = 0; k < points_on_sticks.size (); k++)
          delete points_on_sticks [k];
      }
    
    bool refresh (bool goto_next = true)
      {
        // Read cloud data
        bool success = cloud_type::refresh (goto_next);
        
        if (success)
          {
            // Update stick index
            if (goto_next)
              {
                sticks_index ++;
                if (sticks_index >= sticks.size ())
                  success = false;
              }
            
            // Remove points that are likely to be generated by the stick
            remove_stick_data ();
          }
        return success;
      }

    void show () const
      {
        cloud_type::show ();
  
        // Draw stick
        const stick_type stick = get_stick ();

        OpenTissue::gl::ColorPicker (0.9, 0.1, 0.9, 1.0);
        OpenTissue::gl::DrawVector (stick.second, stick.first - stick.second, 1.0, false);
        OpenTissue::gl::DrawPoint (stick.first, 0.2);
        OpenTissue::gl::ColorPicker (0.9, 0.1, 0.1, 1.0);
        OpenTissue::gl::DrawPoint (stick.second, 0.2);

        // Draw lines from optical centre to the stick
        //const vector3 origin = this->calib.camera_centre ();
        //OpenTissue::gl::DrawVector (origin, stick.first - origin, 1.0, false);
        //OpenTissue::gl::DrawVector (origin, stick.second - origin, 1.0, false);
        
        // Draw points
        //for (size_t n = 0; n < points_on_sticks [sticks_index]->size (); n++)
        //  OpenTissue::gl::DrawPoint ((*(points_on_sticks [std::max (sticks_index-1, 0u)]))[n], 0.2);
      }
    
    stick_type get_stick () const
      {
        return sticks [sticks_index];
      }
  
  protected:
    // Data
    std::vector<stick_type> sticks;
    std::vector< std::vector<vector3>* > points_on_sticks;
    unsigned int sticks_index;
    
    virtual void remove_stick_data () = 0;
};

class point_cloud_and_stick : public cloud_and_stick<point_cloud>
{
  public:
    point_cloud_and_stick (const options &opts, const calibration &_calib)
      : cloud_and_stick<point_cloud> (opts, _calib)
      {
      }
      
  protected:
    void remove_stick_data ()
      {
        const size_t num_points = points.size ();
        size_t num_deletes = 0;
        for (size_t k = 0; k < num_points; k++)
          {
            if (sqrt (dist2 (sticks [sticks_index], points [k])) < 1.0)
              num_deletes ++;
            else
              {
                points [k - num_deletes]  = points [k];
                colours [k - num_deletes] = colours [k];
              }
          }
        points.resize (num_points - num_deletes);
        colours.resize (num_points - num_deletes);
      }
};

#endif // CLOUD_AND_STICK_H