#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 ...
(more)
Let me tell you something about headers, When you
#include < >
, you are including standard library header files. For example#include <sstream>
. It includes the system header files. When you#include " "
, you are including user-defined header files. For example#include "ros/ros.h"
. These are the libraries you downloaded.you're throwing a wall of code at us, but what is the question ?