Revision history [back]

#include <ros/ros.h>
#include <ros/package.h>
#include <geometry_msgs/Point.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <cv.h>

#include <unistd.h> // only for unix?
#include <boost/filesystem.hpp>
#include <vector>

#include <iostream>
#include <string>
#include <sstream>

namespace fs = ::boost::filesystem;
using namespace std;
using namespace cv;

static const std::string OPENCV_WINDOW = "Image window";

#define drawCross( center, color, d )                   \
  line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
  line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )

#define CLIP_CHAR(c) ((c)>255?255:(c)<0?0:(c))

class DetectorNode
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
  ros::Publisher coord_pub;



  // Kalman-related variables
  bool kalmanHasBeenSetup;
  KalmanFilter KF;
  vector<Point> kalmanv;
  int kalmanCounter;
  Mat_<float> measurement;
  HOGDescriptor hog;

public:

  DetectorNode(ros::NodeHandle n)
    : nh_(n),
      it_(n)
  {
    ROS_DEBUG("Detector node constructor started");
    // Subscrive to input video feed and publish output video feed
    coord_pub = n.advertise<geometry_msgs::Point>("detector_topic", 1);
    image_sub_ = it_.subscribe("image", 1,
                   &DetectorNode::imageCb, this);
    //          descriptor_sub = nh_.subscribe<cdio_messages::descriptor>
    //  ("descriptors", 1, &DetectorNode::descriptorCb, this);

    //cv::namedWindow(OPENCV_WINDOW);
    ROS_DEBUG("Detector Node started");

    hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector());
    kalmanHasBeenSetup = false;
  }

  ~DetectorNode()
  {
    cv::destroyWindow(OPENCV_WINDOW);
  }

  bool kalmanIsSetup()
  {
    return kalmanHasBeenSetup;
  }

  void setupKalman(float x_coord, float y_coord)
  {
    ROS_DEBUG("Setup Kalman filter");

    KF.init(4,2,0);
    KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
    measurement.create(2,1); 
    measurement.setTo(Scalar(0));

    KF.statePre.at<float>(0) = x_coord;
    KF.statePre.at<float>(1) = y_coord;
    KF.statePre.at<float>(2) = 0;
    KF.statePre.at<float>(3) = 0;
    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(1e-2)); // Bigger number => faster update
    setIdentity(KF.measurementNoiseCov, Scalar::all(10));
    setIdentity(KF.errorCovPost, Scalar::all(.1));

    // Reset the kalman result vector and counter
    kalmanv.clear();
    kalmanCounter = 0;
    kalmanHasBeenSetup = true;
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    // Convert image message to OpenCV format
    cv_bridge::CvImagePtr cv_ptr;

    try
      {
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
      }
    catch (cv_bridge::Exception& e)
      {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
      }


    //bool found_object = false;
    Point object_coordinates;
    Mat imgBig, img;

    namedWindow("HOG - Detect people", CV_WINDOW_AUTOSIZE);
    img = cv_ptr->image;


    // Get image sizes
    float imgWidth = (img.size()).width;
    float imgHeight = (img.size()).height;

    bool usingKalman = true;
    int kalmanCounterMax = 5;

    if(kalmanIsSetup() && usingKalman)
    {
      ROS_DEBUG("Predict and update Kalman filter");
      // Predict and update Kalman filter
      Mat prediction = KF.predict();
      Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
      Mat estimated = KF.correct(measurement);
      Point statePt(estimated.at<float>(0),estimated.at<float>(1));
    }

    vector<Rect> found, found_filtered;
    hog.detectMultiScale(img, found, 0, Size(8,8), Size(32,32), 1.05, 2);


    size_t i, j;
    size_t biggest = 0;
    Rect biggestRect;
    for (i=0; i<found.size(); i++)
      {
    Rect r = found[i];
    for (j=0; j<found.size(); j++)
      if (j!=i && (r & found[j])==r)
        break;
    if (j==found.size())
      {
        found_filtered.push_back(r);
        ROS_DEBUG("Found object");

        // Check and save if biggest rect found
        if(j >= biggest)
          {
        biggest = j;
        biggestRect = r;
          } 
      }
      }

    // Display biggest detection only
    if(found_filtered.size() > 0)
      {
    // Draw green box around detected object
    Rect r = biggestRect;
    r.x += cvRound(r.width*0.1);
    r.width = cvRound(r.width*0.8);
    r.y += cvRound(r.height*0.06);
    r.height = cvRound(r.height*0.9);
    rectangle(img, r.tl(), r.br(), cv::Scalar(0,255,0), 2);

    //Publish stuff
    ROS_DEBUG_STREAM("Found an object at (" << r.x+r.width/2 << "," << r.y+r.height/2 << ")");
    object_coordinates = cv::Point(r.x+r.width/2,r.y+r.height/2);
    geometry_msgs::Point coord;
    coord.x = object_coordinates.x/imgWidth;
    coord.y = object_coordinates.y/imgHeight;
    coord.z = r.height/imgHeight; // Relative vertical size
    coord_pub.publish(coord);

    if(kalmanIsSetup() && usingKalman)
      {
        ROS_DEBUG("Update the kalman filter and draw cross");
        // Update the kalman filter and draw cross
        measurement(0) = r.x+r.width/2;
        measurement(1) = r.y+r.height/2;
        Mat estimated = KF.correct(measurement);
        Point statePt(estimated.at<float>(0),estimated.at<float>(1));
        kalmanv.push_back(statePt);
        drawCross( statePt, Scalar(255,255,255), 5 );
        usingKalman = false;
        kalmanCounter = 0;
      }
    else if(usingKalman)
      {
        ROS_DEBUG("Setting up Kalman");
        setupKalman(coord.x,coord.y);
      }
      }

    if(kalmanIsSetup() && usingKalman && (kalmanCounter < kalmanCounterMax))
      {
    ROS_DEBUG("Predicting position using Kalman");
    kalmanCounter++;
    Mat estimated = KF.predict();
    Point statePt(estimated.at<float>(0),estimated.at<float>(1));
    kalmanv.push_back(statePt);
    drawCross( statePt, Scalar(255,255,255), 5 );
      }
    else if(kalmanIsSetup() && usingKalman && (kalmanCounter == kalmanCounterMax))
      {
    ROS_DEBUG("Kalman filter timed out after %d frames without object.",kalmanCounterMax);
      }

    /*
    // Display multiple objects
    for (i=0; i<found_filtered.size(); i++)
      {
    Rect r = found_filtered[i];
    r.x += cvRound(r.width*0.1);
    r.width = cvRound(r.width*0.8);
    r.y += cvRound(r.height*0.06);
    r.height = cvRound(r.height*0.9);
    rectangle(img, r.tl(), r.br(), cv::Scalar(0,255,0), 2);
    printf("%d, %d",r.x+r.width/2, r.y+r.height/2);


    //Publish stuff
    ROS_DEBUG_STREAM("Found an object at (" << r.x+r.width/2 << "," << r.y+r.height/2 << ")");
    object_coordinates = cv::Point(r.x+r.width/2,r.y+r.height/2);
    geometry_msgs::Point coord;
    coord.x = object_coordinates.x/imgWidth;
    coord.y = object_coordinates.y/imgHeight;
    coord.z = r.height/imgHeight; // Relative vertical size
    //coord_pub.publish(coord);

    if(kalmanIsSetup() && usingKalman)
      {
        ROS_DEBUG("Update the kalman filter and draw cross");
        // Update the kalman filter and draw cross
        measurement(0) = r.x+r.width/2;
        measurement(1) = r.y+r.height/2;
        Mat estimated = KF.correct(measurement);
        Point statePt(estimated.at<float>(0),estimated.at<float>(1));
        kalmanv.push_back(statePt);
        drawCross( statePt, Scalar(255,255,255), 5 );
        usingKalman = false;
        kalmanCounter = 0;
      }
    else if(usingKalman)
      {
        ROS_DEBUG("Setting up Kalman");
        setupKalman(coord.x,coord.y);
      }

      }

      if(usingKalman && kalmanCounter < 5)
    {
      ROS_DEBUG("Predicting position using Kalman");
      kalmanCounter++;
      Mat estimated = KF.predict();
      Point statePt(estimated.at<float>(0),estimated.at<float>(1));
      kalmanv.push_back(statePt);
      drawCross( statePt, Scalar(255,255,255), 5 );
    }

    */




    imshow("HOG - Detect people", img);
    waitKey(1);

  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "people_detector_node");
  ros::NodeHandle n("~");
  DetectorNode ic(n);
  ros::spin();
  return 0;
}