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

#include <string>
#include <vector>

#include "cloud.h"
#include "options.h"
#include "math_types.h"

#include "cvBumblebee.h"
#include <boost/filesystem.hpp>

#ifdef HAVE_TRICLOPS

class triclops_cloud : public cloud3
{
  public:
    triclops_cloud (const options &opts, const calibration &_calib)
      : cloud3 (opts, _calib),
        cvb (opts.bumblebee_configuration ()),
        left_image_template (opts.left_image_template ()),
        right_image_template (opts.right_image_template ())
      {
        const CvSize depth_size = cvb.get_depth_size ();
        bg_disp = cvCreateImage (depth_size, IPL_DEPTH_16U, 1);
        mask = cvCreateImage (depth_size, IPL_DEPTH_8U, 1);

        init ();
      }
    
    virtual bool read_background (const std::string &depth_background)
      {
        // Initialise accumulator array
        const CvSize depth_size = cvb.get_depth_size ();
        IplImage *accum = cvCreateImage (depth_size, IPL_DEPTH_32S, 1);
        IplImage *count = cvCreateImage (depth_size, IPL_DEPTH_8U, 1);
        cvZero (accum);
        cvZero (count);
        
        // Read data
        const char *input_template = "%s/%s%.6d.png";
        point_vector P;
        const int num_frames = 5;
        for (int bg_frame = 15; bg_frame < 15+num_frames; bg_frame++)
          {
            char left_file [100], right_file [100];
            sprintf (left_file, input_template, depth_background.c_str (), "left", bg_frame);
            sprintf (right_file, input_template, depth_background.c_str (), "right", bg_frame);
                
            IplImage *left = cvLoadImage (left_file);
            IplImage *right = cvLoadImage (right_file);

            if (left && right)
              {
                // Compute disparity
                cvSmooth (left, left, CV_GAUSSIAN, 3);
                cvSmooth (right, right, CV_GAUSSIAN, 3);

                cvb.set_images (left, right);
                IplImage *disp = cvb.get_disparity ();

                // Update accumulator array
                for (int x = 0; x < depth_size.width; x++)
                  for (int y = 0; y < depth_size.height; y++)
                    if (!invalid_disparity (GRAY16 (disp, x, y)))
                      {
                        GRAY32 (accum, x, y) += (int) GRAY16 (disp, x, y);
                        GRAY (count, x, y) ++;
                      }
                
                cvReleaseImage (&left);
                cvReleaseImage (&right);
                cvReleaseImage (&disp);
              }
          }
        
        P.reserve (depth_size.width * depth_size.height);
        for (int x = 0; x < depth_size.width; x++)
          for (int y = 0; y < depth_size.height; y++)
            if (GRAY (count, x, y) > 0)
              {
                GRAY16 (bg_disp, x, y) = (double)GRAY32 (accum, x, y) / (double)GRAY (count, x, y);
                bool valid;
                const vector3 xyz = cvb.get_xyz (GRAY16 (bg_disp, x, y), x, y, valid);
                P.push_back (calib.camera_to_world (xyz));                
              }
            else
              GRAY16 (bg_disp, x, y) = NO_DISPARITY;
        
        cvReleaseImage (&accum);
        cvReleaseImage (&count);
        
        if (P.size () > 0)
          bg_nearest_cloud.refresh (P);
        
        return (P.size () > 0);
      }

    bool refresh (bool goto_next = true)
      {
        // Read data from disk
        char left_file [100], right_file [100];
        sprintf (left_file, left_image_template.c_str (), frame);
        sprintf (right_file, right_image_template.c_str (), frame);
        if (goto_next) frame ++;
        
        IplImage *left = cvLoadImage (left_file);
        IplImage *right = cvLoadImage (right_file);

        const bool found = (left && right);
        if (!found)
          {
            std::cerr << "triclops_cloud::refresh: could not open '"
                      << left_file << "' / '" << right_file << "' for reading" << std::endl;
          }
        else
          {
            // Compute disparity
            cvSmooth (left, left, CV_GAUSSIAN, 3);
            cvSmooth (right, right, CV_GAUSSIAN, 3);

            cvb.set_images (left, right);
            IplImage *disp = cvb.get_disparity ();
            
            depth_bgsub (disp, bg_disp, mask, 250);

            // Allocate storage
            const CvSize size = cvGetSize (disp);
            const CvSize imsize = cvGetSize (left);
            const size_t num_points = size.width * size.height;
            points.clear ();
            points.reserve (num_points / downsample);
            colours.clear ();
            colours.reserve (num_points / downsample);

            // Move (x,y,z) to internal data structure
            const double sample_scale = (double)imsize.width / (double)size.width;
            int count = 0;
            for (int x = 0; x < size.width; x++)
              {
                for (int y = 0; y < size.height; y++)
                  {
                    if (GRAY (mask, x, y) == 0 || count++ % downsample != 0)
                      continue;
                  
                    bool valid;
                    const vector3 xyz = cvb.get_xyz (x, y, valid);
                    
                    if (valid)
                      {
                        const vector3 p = calib.camera_to_world (xyz);

                        const int xx = sample_scale*x, yy = sample_scale*y;
                        const float red   = ((float)RED (right, xx, yy)) / 255.0f;
                        const float green = ((float)GREEN (right, xx, yy)) / 255.0f;
                        const float blue  = ((float)BLUE (right, xx, yy)) / 255.0f;
                        const vector3 rgb (red, green, blue);
                        
                        points.push_back (p);
                        colours.push_back (rgb);
                      }
                  }
              }
      
            // Render background
//            render_background_image (background, PROJ2D_SCALE);
//            proj2d.update_segment (background);
            
            // Update kNN structure
            nearest_cloud.refresh (points);
            
            // Clean up
            cvReleaseImage (&left);
            cvReleaseImage (&right);
            cvReleaseImage (&disp);
          }

        return found;
      }

    vector3 bg_normal (const size_t i) const
      {
        return vector3 (0, 0, 0);
      }

    void depth_bgsub (IplImage *im, IplImage *bg, IplImage *dest, int threshold)
      {
        cvZero (dest);
        
        const CvSize size = cvGetSize (im);
        for (int x = 0; x < size.width; x++)
          {
            for (int y = 0; y < size.height; y++)
              {
                unsigned short vim = GRAY16 (im, x, y);
                unsigned short vbg = GRAY16 (bg, x, y);
              
                unsigned char result;
                if (invalid_disparity (vbg) && !invalid_disparity (vim))
                  result = 255;
                else if (invalid_disparity (vbg))
                  result = 0;
                else if (!invalid_disparity (vbg) && !invalid_disparity (vim) &&  abs (vbg - vim) > threshold)
                  result = 255;
                else
                  result = 0;

                GRAY (dest, x, y) = result;
              }
          }
        
        cvErode (dest, dest, NULL, 3);
        cvDilate (dest, dest, NULL, 3);
        
        //cvNamedWindow ("test", CV_WINDOW_AUTOSIZE);
        //cvShowImage ("test", dest);
        //cvWaitKey (10);
      }

  protected:
    cvBumblebee cvb;
    const std::string left_image_template, right_image_template;
    IplImage *bg_disp, *mask;
};

#else // HAVE_TRICLOPS

// A dummy implementation of the triclops_cloud for cases where Triclops is not available
class triclops_cloud : public cloud3
{
  public:
    triclops_cloud (const options &opts, const calibration &_calib)
      : cloud3 (opts, _calib)
      {
        std::cerr << "Program was not linked with TriClops" << std::endl;
        exit (-1);      
        init (); // this will never be called; included to avoid copy-paste errors :-)
      }
    
    virtual bool read_background (const std::string &depth_background)
      {
        return false;
      }

    bool refresh (bool goto_next = true)
      {
        std::cerr << "Program was not linked with TriClops" << std::endl;
        exit (-1);
        return false;
      }

    vector3 bg_normal (const size_t i) const
      {
        return vector3 (0, 0, 0);
      }
};

#endif // HAVE_TRICLOPS
#endif // TRICLOPS_CLOUD_H