#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include // only for unix?
#include
#include
#include
#include
#include
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 kalmanv;
int kalmanCounter;
Mat_ 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("detector_topic", 1);
image_sub_ = it_.subscribe("image", 1,&DetectorNode::imageCb, this);
// descriptor_sub = nh_.subscribe
// ("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_(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(0) = x_coord;
KF.statePre.at(1) = y_coord;
KF.statePre.at(2) = 0;
KF.statePre.at(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(0),prediction.at(1));
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at(0),estimated.at(1));
}
vector 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= 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(0),estimated.at(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(0),estimated.at(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(0),estimated.at(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(0),estimated.at(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;
}
↧