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

#include "skeleton_state.h"
#include "auxil.h"
#include "options.h"
#include "math_types.h"
#include "kinect_cloud.h"
#include "point_cloud.h"
#include "triclops_cloud.h"

inline
bool nearest_bone_point (const vector3 &p, const_bone_iter iter,
                                vector3 &result, const vector3 &root)
{
  bool is_end_point = true;
  const vector3 a = iter->absolute ().T () + root;
  const vector3 b = iter->parent ()->absolute ().T () + root;

  const double len2 = iter->get_length2 ();
  const vector3 bma = (b - a);
  const double t = -((a - p) * bma) / len2;
  
  if (t < 0)
    {
      result = a;
    }
  else if (t > 1)
    {
      result = b;
    }
  else
    {
      result = a + t * bma;
      is_end_point = false;
    }
  
  return is_end_point;
}

inline
double point_bone_distance2 (const vector3 &p, const_bone_iter iter, const vector3 &root)
{
  vector3 nearest;
  nearest_bone_point (p, iter, nearest, root);
  const double retval = norm2 (nearest - p);
  
  return retval;
}

template <class state_type, class observation_type>
class measure
{
public:
  measure (const options &opts, const calibration &_calib)
    : threshold2 (square (opts.measure_threshold ())),
      variance (opts.measure_variance ()),
      calib (_calib),
      cam (calib.camera_centre ())
    {
    };
  
  //#define POINT_CAPSULE_DISTANCE
  #define POINT_HALF_CAPSULE_DISTANCE
  //#define ELLIPSOIDAL_DISTANCE_1
  //#define ELLIPSOIDAL_DISTANCE_2
  
  double log_measure (state_type &state, const observation_type &observation) const
    {
      // Mean-min-distance measure
      state.compute_pose ();
      const vector3 root = state.get_root ();

      double mean = 0;
      const size_t num_points = observation.size ();
      for (size_t idx = 0; idx < num_points; idx++)
        {
          #ifdef ELLIPSOIDAL_DISTANCE_2
          double sum = 0;
          #else
          double min = threshold2;
          #endif // ELLIPSOIDAL_DISTANCE_2
    
          const vector3 p = observation [idx];
          for (const_bone_iter iter = state.bone_begin (); iter != state.bone_end (); iter++)
            {
              if (!iter->is_root ())
                {
                  #ifdef POINT_CAPSULE_DISTANCE
                  // Point-to-capsule distance
                  const double d2 = point_bone_distance2 (p, iter, root);
                  const double r = iter->get_width ();
                  const double dist = square (sqrt (d2) - r);

                  if (dist < min) min = dist;
                  #endif // POINT_CAPSULE_DISTANCE

                  // Point-to-half-capsule distance
                  #ifdef POINT_HALF_CAPSULE_DISTANCE
                  const double dist = point_half_cylinder_distance2 (p, iter, root, min);
                  if (dist < min) min = dist;
                  #endif // POINT_HALF_CAPSULE_DISTANCE

                  // Ellipsoidal distance 1
                  #ifdef ELLIPSOIDAL_DISTANCE_1
                  ellipsoid E;
                  double dist = threshold2;
                  if (state.bone_to_ellipsoid (iter, E))
                    {
                      vector3 on_ellipsoid;
                      E.project_point (p, on_ellipsoid);
                      dist = std::min (E.euc_mahalanobis2 (p, on_ellipsoid), threshold2);
                    }

                  if (dist < min) min = dist;
                  #endif // ELLIPSOIDAL_DISTANCE_1

                  // Ellipsoidal distance 2
                  #ifdef ELLIPSOIDAL_DISTANCE_2
                  ellipsoid E;
                  if (state.bone_to_ellipsoid (iter, E))
                    {
                      vector3 on_ellipsoid;
                      E.project_point (p, on_ellipsoid);
                      const double dist2 = std::min (E.euc_mahalanobis2 (p, on_ellipsoid), threshold2);
                      
                      sum += exp (-dist2);
                    }
                  #endif // ELLIPSOIDAL_DISTANCE_2
                }
            }
          
          #ifdef ELLIPSOIDAL_DISTANCE_2
          mean += log (sum); // ellip2
          #else
          mean += min; // Cylinder & ellip1
          #endif // ELLIPSOIDAL_DISTANCE_2
        }
  
      mean /= (double)num_points;
  
      #ifdef ELLIPSOIDAL_DISTANCE_2
      const double retval = mean / variance; // ellip2
      #else
      const double retval = -0.5 * mean / variance;
      #endif // ELLIPSOIDAL_DISTANCE_2

      //std::cout << num_points << " " << mean << " " << retval << std::endl;
      return (retval);
    }
  
private:
  double point_half_cylinder_distance2 (const vector3 &p, const_bone_iter iter,
                                        const vector3 &root, const double thresh) const
    {
      vector3 on_bone;
      const bool is_end_point = nearest_bone_point (p, iter, on_bone, root);
  
      // Compute distance to the cylinder. Note: this distance provides a lower
      // bound for the distance to the half-cylinder
      const double r = iter->get_width ();
      const double d = sqrt (norm2 (p - on_bone));
      double retval = square (d - r);
      
      // We only do all the fancy half-cylinder stuff if the nearest point was
      // actually on the bone and not to far away to be worth considering.
      if (!is_end_point && retval < thresh /*threshold2*/)
        {
          const vector3 v1 = unit (cam - on_bone);
          const vector3 v2 = unit (p - on_bone);
          const double dot = (v1 * v2);
  
          // Do we need to move the closest point to the visible part of the cylinder?
          if (dot < 0)
            {
              // We're on the wrong side, so we go to the border of the half-cylinder
              // XXX: Can we figure out which border to look at just by looking at the sign
              // of the angle?
              const vector3 a = iter->absolute ().T () + root;
              const vector3 b = iter->parent ()->absolute ().T () + root;

              const vector3 bone = unit (b - a);
              const vector3 v3 = r * unit (v1 % bone); // Orthogonal to v1 and bone
              const vector3 p1 = on_bone + v3;
              const vector3 p2 = on_bone - v3;
              const double dp1 = norm2 (p - p1);
              const double dp2 = norm2 (p - p2);
              retval = std::min (dp1, dp2);
            }
        }
      return retval;
    }

protected:
  const double threshold2;
  const double variance;
  const calibration &calib;
  const vector3 cam;
};

#endif // SKELETON_CLOUD_MEASURE_H