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

#include "cloud.h"
#include "options.h"
#include <list>
#include <vector>
#include <limits>

typedef std::vector<vector3> point_vector;

template <class observation_type>
class mocap_data : public cloud<point_vector>
{
  public:
    mocap_data (const options &opts, const calibration &_calib)
      : cloud<point_vector> (opts, _calib)
      {
        try
          {
            const std::string mocap_filename = opts.mocap_filename ();
            read_data (mocap_filename);
            curr_frame_iter = all_points.begin ();
            
            // Fast-forward to the starting frame
            for (int f = 0; f < frame; f++)
              curr_frame_iter ++;
          }
        catch (missing_option)
          {
            curr_frame_iter = all_points.end ();
          }
      };
    
    ~mocap_data ()
      {
        for (std::list<point_vector*>::iterator iter = all_points.begin ();
             iter != all_points.end (); iter ++)
          {
            if (*iter != NULL)
              {
                delete *iter;
                *iter = NULL;
              }
          }
      };
    
    bool refresh (bool goto_next = true)
      {
        bool retval = true;
        if (goto_next && curr_frame_iter != all_points.end ())
          {
            frame ++;
            curr_frame_iter ++;
            if (curr_frame_iter == all_points.end ())
              retval = false;
          }
          
        return retval;
      };
    
    size_t size () const
      {
        return all_points.size ();
      };
      
    void show () const
      {
        if (curr_frame_iter != all_points.end ())
          {
            const point_vector *points = *curr_frame_iter;
            if (points != NULL)
              {
                OpenTissue::gl::ColorPicker (1.0, 0.3, 0.0);
                for (size_t k = 0; k < points->size (); k++)
                  OpenTissue::gl::DrawPoint ((*points) [k], 0.15);
              }
          }
      };
    
    double average_pose_dist (const skeleton_state<observation_type> &pose)
      {
        double dist = 0;
        if (curr_frame_iter != all_points.end ())
          {
            const skeleton_type& skeleton = pose.get_skeleton ();
            const point_vector *points = *curr_frame_iter;
            if (points != NULL)
              {
                for (size_t k = 0; k < points->size (); k++)
                  dist += point_skin_dist (skeleton, (*points) [k]);
                dist /= (double)points->size ();
              }
          }

        return dist;
      };
  
  protected:
    // Data
    std::list< point_vector* > all_points;
    std::list< point_vector* >::iterator curr_frame_iter;
  
    // Auxilary functions
    void read_data (const std::string filename)
      {
        FILE *mocap_fid = fopen (filename.c_str (), "r");
        if (mocap_fid != NULL)
          {
            // XXX: This is not robust! The input line could exceed 1000 char's
            char line [1000];
            char *status = fgets (line, sizeof (line), mocap_fid);
            while (status == line)
              {
                // Count number of words in the string
                int num_words = 1;
                char *lp = line;
                while (*lp != '\n')
                  {
                    if (*lp == ' ' && *(lp-1) != ' ')
                      num_words ++;
                    lp++;
                  }
                   
                point_vector *points = NULL;
                if ((num_words/3)*3 != num_words)
                  {
                    std::cerr << "warning: number of word (" << num_words
                              << ") in mocap validation file is not divisble by 3"
                              << std::endl;
                  }
                else
                  {
                    lp = line;
                    const int num_points = num_words / 3;
                    points = new point_vector (num_points);
                    int idx = 0;
                    for (int v = 0; v < num_points; v++)
                      {
                        double x, y, z;
                        const int num_read = sscanf (lp, "%lf %lf %lf", &x, &y, &z);
                        if (num_read == 3)
                          {
                            #define IS_MINUS_ONE(X) (fabs (x + 1.0) < 0.00001)
                            if (!(IS_MINUS_ONE (x) && IS_MINUS_ONE (y) && IS_MINUS_ONE (z)))
                              (*points) [idx++] = calib.camera_to_world (vector3 (x, y, z));
                            #undef IS_MINUS_ONE
                                 
                            for (int w = 0; w < 3; w++)
                              {
                                while (*lp != ' ' && *lp != '\n')
                                  lp++;
                                while (*lp == ' ' || *lp == '\n')
                                  lp++;
                              }
                          }
                        else
                          {
                            std::cerr << "warning: did not read a 3D point from mocap validation file"
                                      << std::endl;
                            delete points;
                            points = NULL;
                            break;
                          }
                      }
                    points->resize (idx);
                  }
                all_points.push_back (points);
                
                status = fgets (line, sizeof (line), mocap_fid);
              }
            fclose (mocap_fid);
          }
      };
    
    double point_skin_dist (const skeleton_type &skeleton, const vector3 &x0)
      {
        double min_dist = std::numeric_limits<double>::infinity();
        
        for (const_bone_iter iter = skeleton.begin (); iter != skeleton.end (); iter++)
          {
            if (!iter->is_root ())
              {
                const vector3 x1 = iter->absolute ().T ();
                const vector3 x2 = iter->parent ()->absolute ().T ();
                if (x1 != x2) // XXX: Why can this ever happen? (it does...)
                  {
                    const vector3 d01 = x0 - x1;
                    const vector3 d21 = x2 - x1;
                    const double alpha = force_in_range ((d01 * d21) / (d21 * d21), 0.0, 1.0);

                    const vector3 on_stick = alpha * x2 + (1.0 - alpha) * x1;
                    const vector3 d = on_stick - x0;
                    const double dist_to_bone = sqrt (d * d);

                    const double r = iter->get_width ();
                    const double dist_to_skin = fabs (dist_to_bone - r);
                
                    if (dist_to_skin < min_dist)
                      min_dist = dist_to_skin;
                  }
              }
          }
        return min_dist;
      };

};

#endif // MOCAP_DATA_H