void callback(const sensor_msgs::ImageConstPtr &imgPtr){
cvImageFromROS =cv_bridge::toCvCopy(imgPtr);
diff =cv_bridge::toCvCopy(imgPtr);
cvImageFromROS->image.copyTo(image);
diff->image.copyTo(DiffImage);
out_msg.header = imgPtr->header;
out_msg.encoding = "mono16";
int cols=image.cols;
int rows=image.rows;
ushort v;
cv::Point p;
for(int i=0;i<cols;i++){
for(int j=0;j<rows;j++){
p.x=i;
p.y=j;
v=((float)image.at<ushort>(p));
v*=multiplier->cell(p.y,p.x,v);
image.at<ushort>(p)=(ushort)v;
}
}
out_msg.image = image;
pub.publish(out_msg.toImageMsg());
out_msg.image = image-DiffImage;
pub2.publish(out_msg.toImageMsg());
//std::cout<<"global diff: "<< cv::sum(out_msg.image)/(out_msg.image.rows*out_msg.image.cols)<<std::endl;
}
void faceDetect(){
//Detect faces
cv::cvtColor( frame, frame_gray, CV_BGR2GRAY );
cv::equalizeHist( frame_gray, frame_gray );
face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
//for each face draws an ellipse arround and look for the red color at a distance from
for( i = 0; i < faces.size(); i++ )
{
cv::Point center( faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5 );
cv::ellipse( frame, center, cv::Size( faces[i].width*0.5, faces[i].height*0.5), 0, 0, 360, cv::Scalar( 0, 255, 0 ), 2, 8, 0 );
faceX = (float) faces[i].x;
faceY = (float) faces[i].y;
if( ((faceX + faceColorThresh) > (posX ) | (faceX - faceColorThresh) < (posX )) ) {
face = true;
//publishing camera image
out_msg.image = frame; //frame
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
msg = out_msg.toImageMsg();
pub.publish(msg);
ROS_FATAL("PERSON DETECTED");
break;
}
}
}
inline void publish(Mat img, imgColorType t, ros::Time now =
ros::Time::now())
{
hasListener=(img_pub.getNumSubscribers() > 0);
if (!enable || !hasListener)
return;
Mat res;
switch (t)
{
case gray:
msg.encoding = sensor_msgs::image_encodings::MONO8;
res = img;
break;
case hsv:
msg.encoding = sensor_msgs::image_encodings::BGR8;
cvtColor(img, res, CV_HSV2BGR);
break;
case bgr:
default:
msg.encoding = sensor_msgs::image_encodings::BGR8;
res = img;
break;
}
msg.header.stamp = now;
msg.image = res;
img_pub.publish(msg.toImageMsg());
}
void callback(const sensor_msgs::ImageConstPtr &imgPtr){
cvImageFromROS =cv_bridge::toCvCopy(imgPtr);
cvImageFromROS->image.copyTo(image);
out_msg.header = imgPtr->header;
out_msg.encoding = "mono16";
out_msg.image = image;
pub.publish(out_msg.toImageMsg());
}
/***********************************************************
* @fn main(int argc, char **argv)
* @brief starts the Pot_Nav node and publishises twist when
* it gets a new image asuming 30 hz
***********************************************************/
int main(int argc, char **argv)
{
ros::init(argc, argv, "Pot_Nav");
ros::NodeHandle n;
image_transport::ImageTransport it(n);
dynamic_reconfigure::Server<MST_Potential_Navigation::Pot_Nav_ParamsConfig> srv;
dynamic_reconfigure::Server<MST_Potential_Navigation::Pot_Nav_ParamsConfig>::CallbackType f;
f = boost::bind(&setparamsCallback, _1, _2);
srv.setCallback(f);
//create subsctiptions
image_sub_edges = it.subscribe( n.resolveName("image_edges") , 1, edgesCallback );
//create publishers
twist_pub = n.advertise<geometry_msgs::Twist>( "nav_twist" , 5 );
map_pub = it.advertise( "map" , 5 );
//set rate to 30 hz
ros::Rate loop_rate(30);
//run main loop
while (ros::ok())
{
//check calbacks
ros::spinOnce();
//finds and publishes new twist
if(map_changed)
{
geometry_msgs::Twist twist;
twist = find_twist();
//publish twist
twist_pub.publish(twist);
//publish map
map_pub.publish(map_dis.toImageMsg());
map_changed = 0;
}
loop_rate.sleep();
}
return 0;
}
void HiwrCameraControllerNodelet::publishFrame(Mat frame ) {
out_msg_.encoding = sensor_msgs::image_encodings::MONO8;
out_msg_.image = frame;
out_msg_.header.seq = published_frame_;
char frame_id[20];
sprintf(frame_id , "%d" , published_frame_);
out_msg_.header.frame_id = frame_id;
out_msg_.header.stamp = ros::Time::now();
published_frame_++;
image_publisher_.publish(out_msg_.toImageMsg() );
}
void mapSubCallback(const nav_msgs::OccupancyGrid map) {
//================================================================MAP TO IMAGE
MAP_X = map.info.width;
MAP_Y = map.info.height;
MAP_RESOLUTION = map.info.resolution;
if ((MAP_X < 3) || (MAP_Y < 3) ) {
ROS_INFO("Map size is only x: %ld, y: %ld . Not running map to image conversion", MAP_X, MAP_Y);
return;
}
cv::Mat *map_mat = &cv_img.image;
// resize cv image if it doesn't have the same dimensions as the map
if ((map_mat->rows != MAP_Y) && (map_mat->cols != MAP_X)) {
*map_mat = cv::Mat(MAP_Y, MAP_X, CV_8U);
}
// ROS_INFO("Fault_1");
const std::vector<int8_t>&map_data(map.data);
unsigned char *map_mat_data_p = (unsigned char *)map_mat->data;
// ROS_INFO("Fault_2");
//We have to flip around the y axis, y for image starts at the top and y for map at the bottom
int MAP_Y_rev = MAP_Y - 1;
// ROS_INFO_STREAM("Fault for :" << MAP_Y_rev);
for (int y = MAP_Y_rev; y >= 0; --y) {
int idx_map_y = MAP_X * (MAP_Y - y);
int idx_img_y = MAP_X * y;
// ROS_INFO_STREAM("Fault_3 :" << y);
// if (y == 0) {
// ROS_INFO_STREAM("MAP_X :" << MAP_X);
// }
for (int x = 0; x < MAP_X; ++x) {
int idx = idx_img_y + x;
//
// if (y == 0 && x > 1000) {
// ROS_INFO_STREAM("x :" << x);
// try {
// //ROS_INFO_STREAM("valid:" << isValidPtr(map_data, idx_map_y + x));
// ROS_INFO_STREAM("Map data :" << map_data[idx_map_y + x]);
// } catch (std::exception& e) {
// std::cout << e.what() << std::endl;
// }
// }
switch (map_data[idx_map_y + x]) {
case -1:
map_mat_data_p[idx] = 255; //grey color:unknown default value of 127
break;
case 0:
map_mat_data_p[idx] = 255; // laser ray:default 255
break;
case 100:
map_mat_data_p[idx] = 0; // obstacle:default 0
break;
}
// if (y == 0 && x > 1000) {
// ROS_INFO_STREAM("x :" << x);
// }
}
// ROS_INFO_STREAM("Fault_3 :" << y);
}
// ROS_INFO("Fault_4");
//======================================================================== DETECT TREES
cv::Mat im, im_with_keypoints;
// Reduce the noise
if (GaussianBlur_kernelSize > 1) {
cv::GaussianBlur(cv_img.image, im, cv::Size(GaussianBlur_kernelSize, GaussianBlur_kernelSize), 0);
} else {
im = cv_img.image;
}
// Setup SimpleBlobDetector parameters.
cv::SimpleBlobDetector::Params params;
// Change thresholds
params.minThreshold = minThreshold;
params.maxThreshold = maxThreshold;
// Filter by Area.
if (minArea < maxArea) {
params.filterByArea = true;
params.minArea = minArea;
params.maxArea = maxArea;
} else {
params.filterByArea = false;
}
// Filter by Circularity
if (minCircularity < maxCircularity) {
params.filterByCircularity = true;
params.minCircularity = minCircularity;
params.maxCircularity = maxCircularity;
} else {
params.filterByCircularity = false;
}
// Filter by Convexity
if (minConvexity < maxConvexity) {
params.filterByConvexity = true;
params.minConvexity = minConvexity;
params.maxConvexity = maxConvexity;
} else {
params.filterByConvexity = false;
}
// Filter by Inertia
// params.filterByInertia = true;
// params.minInertiaRatio = 0.01;
// Storage for blobs
std::vector<cv::KeyPoint> keypoints;
// Set up detector with params
cv::Ptr<cv::SimpleBlobDetector> detector = cv::SimpleBlobDetector::create(params);
// Detect blobs
detector->detect(im, keypoints);
//================================================================ Publish Markers
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time();
marker.ns = "agri_mapper";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = -(MAP_X * MAP_RESOLUTION / 2) - 0.2;
marker.pose.position.y = -(MAP_Y * MAP_RESOLUTION / 2) - 0.2;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0;
marker.pose.orientation.y = 0;
marker.pose.orientation.z = 0;
marker.pose.orientation.w = 1;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.a = 1.0; // Don't forget to set the alpha!
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.points.resize(keypoints.size());
std::vector<cv::KeyPoint>::iterator it = keypoints.begin();
ROS_INFO_STREAM("making markers");
ROS_INFO_STREAM("\n Size : " << keypoints.size() << "\n");
for (int i = 0; it != keypoints.end(); ++it, i++) {
marker.points[i].x = (it->pt.x) * MAP_RESOLUTION;
marker.points[i].y = (MAP_Y - it->pt.y) * MAP_RESOLUTION;
marker.points[i].z = 0;
}
ROS_INFO_STREAM("made markers");
vis_pub.publish(marker);
//============================================================================PUBLISH IMAGE
// Draw detected blobs as red circles.
// DrawMatchesFlags::DRAW_RICH_KEYPOINTS flag ensures
// the size of the circle corresponds to the size of blob
cv::drawKeypoints(im, keypoints, im_with_keypoints, cv::Scalar(255, 0, 0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv_detectionImg.image = im_with_keypoints;
image_pub.publish(cv_img.toImageMsg());
detected_pub.publish(cv_detectionImg.toImageMsg());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "people_counting");
ros::NodeHandle node;
ros::param::param<std::string>(ros::this_node::getName() + "dataset", fileDataset_param, "haarcascade_upperbody.xml");
ros::param::param<double>(ros::this_node::getName() + "scaleFactor", scaleFactor_param, 0.1);
ros::param::param<int>(ros::this_node::getName() + "minNeighbors", minNeighbors_param, 0);
// ros::param::param<double>("minSize", minSize_param, 0);
ros::param::param<double>(ros::this_node::getName() + "maxSize", maxSize_param, 1.0);
// argc--; argv++;
// while( argc && *argv[0] == '-' )
// {
// if( !strcmp(*argv, "-fps") && ( argc > 1 ) )
// {
// fps = atof(*(argv+1));
// printf("With %ffps\n",fps);
// argc--; argv++;
// }
// if( !strcmp(*argv, "-crowd_dataset") && ( argc > 1 ) )
// {
// file_dataset = *(argv+1);
// printf("Using dataset form %s\n",file_dataset.c_str());
// argc--; argv++;
// }
// argc--; argv++;
// }
std::vector<cv::Rect> objects;
std::vector<cv::Scalar> colors;
cv::RNG rng(12345);
img_msg.encoding = "bgr8";
if ( ! classifier.load(fileDataset_param))
ROS_ERROR("Can't open %s", fileDataset_param.c_str());
ros::Publisher img_pub = node.advertise<sensor_msgs::Image>("crowd/count_img", 100);
ros::Publisher count_pub = node.advertise<std_msgs::UInt16>("crowd/people_count", 100);
image_transport::ImageTransport it(node);
image_transport::Subscriber img_sub = it.subscribe("image", 1, imageCallback);
ros::Rate loop_rate(fps);
while (ros::ok())
{
if (! img_msg.image.empty()) {
classifier.detectMultiScale(img_msg.image, objects, 1.0+scaleFactor_param, minNeighbors_param+1, 0, cv::Size(), maxSize);
for (int j = 0; j < objects.size(); ++j) {
if ( j > max_detected) {
colors.push_back(cv::Scalar(rng.uniform(0,255),rng.uniform(0, 255),rng.uniform(0, 255)));
max_detected = j;
}
cv::rectangle(img_msg.image, objects[j], colors[j], 2);
}
people_count.data = (u_int16_t)objects.size();
}
img_pub.publish(img_msg.toImageMsg());
count_pub.publish(people_count);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
void BallDetector::imageCb(const sensor_msgs::ImageConstPtr& msg){
static cv_bridge::CvImageConstPtr cv_const_ptr;
#ifdef BALLDETECTOR_DEBUG
static cv_bridge::CvImage cv_img;
cv_img.header.stamp = ros::Time::now();
if(debugLevel.sendDebugTimes){
debugTimeInit();
debugTimeStart();
}
#endif
try{
cv_const_ptr = cv_bridge::toCvShare(msg, enc::BGR8);
}catch (cv_bridge::Exception& e){
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("cvconvert");
}
#endif
#ifdef BALLDETECTOR_DEBUG
//Get a the image just as a Mat
static cv::Mat colorImg;
if(debugLevel.sendDebugImages){
colorImg = cv_const_ptr->image.clone();
//cv_const_ptr->image.copyTo(colorImg);
}else{
colorImg = cv_const_ptr->image;
}
#else
static cv::Mat colorImg;
colorImg = cv_const_ptr->image;
#endif
//Create the hsv image
static cv::Mat hsvImg;
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("allocate");
}
#endif
//colorImg is type CV_8U
cv::cvtColor(colorImg,hsvImg,CV_RGB2HSV);
//cv::cvtColor(colorImg,hsvImg,CV_RGB2HLS);
//cv::cvtColor(colorImg,hsvImg,CV_RGB2Lab); //faster but not on gumstix?
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("hsv");
}
#endif
//Perform thresholding
static cv::Mat threshImg;
cv::inRange(hsvImg,lowThresh,highThresh,threshImg);
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("threshold");
}
#endif
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugImages){
//Send some images out
cv_img.image = threshImg;
cv_img.encoding = enc::MONO8; // TYPE_8UC1;
debugImg2_pub_.publish(cv_img.toImageMsg());
cv_img.image = hsvImg;
cv_img.encoding = enc::BGR8;
debugImg3_pub_.publish(cv_img.toImageMsg());
}
#endif /** BALLDETECTOR_DEBUG **/
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeStart();
}
#endif
//Contours
static vector<vector<cv::Point> > contours;
contours.clear();
static vector<cv::Vec4i> hierarchy;
hierarchy.clear();
//Some quick testing indicates CV_RETR_* doesn't make too much
//difference in performance. The CV_CHAIN_APPROX_* also doesn't
//seem to make much difference, but the "NONE" draws the full
//outline with debug mode (taking more time), but is easier to look
//at the output.
cv::findContours( threshImg, contours, hierarchy,
CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
//cv::findContours( threshImg, contours, hierarchy,
//CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("findcontours");
}
#endif
// iterate through all the top-level contours,
// draw each connected component with its own random color
int idxMax = -1;
unsigned int idxMaxValue = 0;
int idx = 0;
if(hierarchy.size() > 0){
for( ; idx >= 0; idx = hierarchy[idx][0] ){
//If it is large enough, fit an ellipse to it
if(contours.at(idx).size() > 15){
//Slower than the below methods
//cv::RotatedRect r = cv::fitEllipse(cv::Mat(contours.at(idx)));
//cv::Rect rect = r.boundingRect();
//Find the min/max pixel locatiosn of x and y
int maxX = 0, maxY = 0;
unsigned int maxXidx = 0, maxYidx = 0;
//Just needs to be bigger than max image width/height
int minX = 999999999, minY = 999999999;
unsigned int minXidx = 0, minYidx = 0;
for(unsigned int i = 0; i < contours.at(idx).size(); i++){
cv::Point p = contours.at(idx).at(i);
if(p.x > maxX){
maxXidx = i;
maxX = p.x;
}
else if(p.x < minX){
minXidx = i;
minX = p.x;
}
if(p.y > maxY){
maxYidx = i;
maxY = p.y;
}
else if(p.y < minY){
minYidx = i;
minY = p.y;
}
}
//Keep the ellipse if it is mostly round
//if((rect.width/(1.0*rect.height) > 0.75) && (rect.height/(1.0*rect.width) > 0.75)){
if(((1.0*(contours.at(idx).at(maxXidx).x-contours.at(idx).at(minXidx).x)/
(contours.at(idx).at(maxYidx).y-contours.at(idx).at(minYidx).y))) > 0.75
&&
((1.0*(contours.at(idx).at(maxYidx).y-contours.at(idx).at(minYidx).y)/
(contours.at(idx).at(maxXidx).x-contours.at(idx).at(minXidx).x)) > 0.75)){
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugImages){
//cv::ellipse(colorImg,r,cv::Scalar(255,0,0,0),3);
//Color all the points white (it isn't all points, but a lot on the boarders)
for(unsigned int i = 0; i < contours.at(idx).size(); i++){
cv::Point p = contours.at(idx).at(i);
//Get a pointer to the row, then access the pixel elements
colorImg.ptr<uchar>(p.y)[3*p.x] = 255;
colorImg.ptr<uchar>(p.y)[3*p.x+1] = 255;
colorImg.ptr<uchar>(p.y)[3*p.x+2] = 255;
}
//draw max/min points
/*
cv::rectangle(colorImg,
cv::Point(contours.at(idx).at(maxXidx).x - 1,
contours.at(idx).at(maxXidx).y- 1),
cv::Point(contours.at(idx).at(maxXidx).x + 1,
contours.at(idx).at(maxXidx).y+ 1),
cv::Scalar(255,0,0,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idx).at(maxYidx).x - 1,
contours.at(idx).at(maxYidx).y- 1),
cv::Point(contours.at(idx).at(maxYidx).x + 1,
contours.at(idx).at(maxYidx).y+ 1),
cv::Scalar(255,0,0,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idx).at(minXidx).x - 1,
contours.at(idx).at(minXidx).y- 1),
cv::Point(contours.at(idx).at(minXidx).x + 1,
contours.at(idx).at(minXidx).y+ 1),
cv::Scalar(255,0,0,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idx).at(minYidx).x - 1,
contours.at(idx).at(minYidx).y- 1),
cv::Point(contours.at(idx).at(minYidx).x + 1,
contours.at(idx).at(minYidx).y+ 1),
cv::Scalar(255,0,0,0),2);
*/
}
#endif /** BALLDETECTOR_DEBUG **/
//If there are more points in it than our previous max, then record it
//N.B. contours.at(idx).size() isn't all points, rather a
//representation of them...but it seems to map fairly well.
if(idxMaxValue < contours.at(idx).size()){
idxMaxValue = contours.at(idx).size();
idxMax = idx;
}
}
}
}
}
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("iteratecontours");
}
#endif
//Make sure we found a good one
if(idxMax >= 0){
//Now publish and draw the best
cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(contours.at(idxMax)));
cv::Rect rect = ellipse.boundingRect();
//Publish the message for the best
ballDetector::ballLocation circ;
circ.header.stamp = ros::Time::now();
//Use the average of the height and width as the radius
double radius = (rect.height/2.0 + rect.width/2.0)/2.0;
circ.imageWidth = colorImg.cols;
circ.imageHeight = colorImg.rows;
//Make it relative to the center of the image
circ.x = rect.x + radius - threshImg.cols/2;
circ.y = -1*(rect.y + radius - threshImg.rows/2);
circ.radius = radius;
circle_pub_.publish(circ);
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugImages){
//Draw the best one bold
cv::ellipse(colorImg,ellipse,cv::Scalar(0,255,0,0),4);
//And the center point
cv::rectangle(colorImg,
cv::Point(rect.x + radius-2,rect.y + radius-2),
cv::Point(rect.x + radius+2,rect.y + radius+2),
cv::Scalar(0,255,0,0),4);
//Find min/max x,y pixel locations to draw them
//cout << "---------------------\n";
int maxX = 0, maxY = 0;
unsigned int maxXidx = 0, maxYidx = 0;
//Just needs to be bigger than max image width/height
int minX = 999999999, minY = 999999999;
unsigned int minXidx = 0, minYidx = 0;
for(unsigned int i = 0; i < contours.at(idxMax).size(); i++){
cv::Point p = contours.at(idxMax).at(i);
//printf("%d: [%d,%d] ",i,p.x,p.y);
if(p.x > maxX){
maxXidx = i;
maxX = p.x;
}
else if(p.x < minX){
minXidx = i;
minX = p.x;
}
if(p.y > maxY){
maxYidx = i;
maxY = p.y;
}
else if(p.y < minY){
minYidx = i;
minY = p.y;
}
}
/*
printf("\nminX: %d, %d, %d, %d\n",minX,minXidx,
contours.at(idxMax).at(minXidx).x,contours.at(idxMax).at(minXidx).y);
printf("maxX: %d, %d, %d, %d\n",maxX,maxXidx,
contours.at(idxMax).at(maxXidx).x,contours.at(idxMax).at(maxXidx).y);
printf("minY: %d, %d, %d, %d\n",minY,minYidx,
contours.at(idxMax).at(minYidx).x,contours.at(idxMax).at(minYidx).y);
printf("maxY: %d, %d, %d, %d\n",maxY,maxYidx,
contours.at(idxMax).at(maxYidx).x,contours.at(idxMax).at(maxYidx).y);
printf("diffX: %d\n",contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x);
printf("diffY: %d\n",contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y);
printf("ratio: %f %f\n",
1.0*(contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x)/
(contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y),
1.0*(contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y)/
(contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x));
*/
//draw max/min points
cv::rectangle(colorImg,
cv::Point(contours.at(idxMax).at(maxXidx).x - 1,
contours.at(idxMax).at(maxXidx).y - 1),
cv::Point(contours.at(idxMax).at(maxXidx).x + 1,
contours.at(idxMax).at(maxXidx).y + 1),
cv::Scalar(0,0,255,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idxMax).at(maxYidx).x - 1,
contours.at(idxMax).at(maxYidx).y - 1),
cv::Point(contours.at(idxMax).at(maxYidx).x + 1,
contours.at(idxMax).at(maxYidx).y + 1),
cv::Scalar(0,0,255,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idxMax).at(minXidx).x - 1,
contours.at(idxMax).at(minXidx).y - 1),
cv::Point(contours.at(idxMax).at(minXidx).x + 1,
contours.at(idxMax).at(minXidx).y + 1),
cv::Scalar(0,0,255,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idxMax).at(minYidx).x - 1,
contours.at(idxMax).at(minYidx).y - 1),
cv::Point(contours.at(idxMax).at(minYidx).x + 1,
contours.at(idxMax).at(minYidx).y + 1),
cv::Scalar(0,0,255,0),2);
}
#endif /** BALLDETECTOR_DEBUG **/
}
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("publish");
}
#endif
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugImages){
cv_img.image = colorImg;
cv_img.encoding = enc::BGR8;
debugImg1_pub_.publish(cv_img.toImageMsg());
}
if(debugLevel.sendDebugTimes){
debugTimeSend();
}
#endif /** BALLDETECTOR_DEBUG **/
}
void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData)
{
timeLast = timeCur;
timeCur = imageData->header.stamp.toSec() - 0.1163;
cv_bridge::CvImageConstPtr imageDataCv = cv_bridge::toCvShare(imageData, "mono8");
if (!systemInited) {
remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
oclImage0 = oclMat(image0);
systemInited = true;
return;
}
Mat imageLast, imageCur;
oclMat oclImageLast, oclImageCur;
if (isOddFrame) {
remap(imageDataCv->image, image1, mapx, mapy, CV_INTER_LINEAR);
oclImage1 = oclMat(image1);
imageLast = image0;
imageCur = image1;
oclImageLast = oclImage0;
oclImageCur = oclImage1;
} else {
remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
oclImage0 = oclMat(image0);
imageLast = image1;
imageCur = image0;
oclImageLast = oclImage1;
oclImageCur = oclImage0;
}
isOddFrame = !isOddFrame;
resize(oclImageLast, oclImageShow, showSize);
oclImageShow.download(imageShow);
cornerHarris(oclImageShow, oclHarrisLast, 2, 3, 0.04);
oclHarrisLast.download(harrisLast);
vector<Point2f> *featuresTemp = featuresLast;
featuresLast = featuresCur;
featuresCur = featuresTemp;
pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
imagePointsLast = imagePointsCur;
imagePointsCur = imagePointsTemp;
imagePointsCur->clear();
int recordFeatureNum = totalFeatureNum;
detectionCount = (detectionCount + 1) % (detectionSkipNum + 1);
if (detectionCount == detectionSkipNum) {
oclMat oclFeaturesSub;
for (int i = 0; i < ySubregionNum; i++) {
for (int j = 0; j < xSubregionNum; j++) {
int ind = xSubregionNum * i + j;
int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind];
if (numToFind > maxFeatureNumPerSubregion / 5.0) {
int subregionLeft = xBoundary + (int)(subregionWidth * j);
int subregionTop = yBoundary + (int)(subregionHeight * i);
Rect subregion = Rect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight);
oclFeatureDetector.maxCorners = numToFind;
oclFeatureDetector(oclImageLast(subregion), oclFeaturesSub);
if (oclFeaturesSub.cols > 0) {
oclFeatureDetector.downloadPoints(oclFeaturesSub, featuresSub);
numToFind = featuresSub.size();
} else {
numToFind = 0;
}
int numFound = 0;
for(int k = 0; k < numToFind; k++) {
featuresSub[k].x += subregionLeft;
featuresSub[k].y += subregionTop;
int xInd = (featuresSub[k].x + 0.5) / showDSRate;
int yInd = (featuresSub[k].y + 0.5) / showDSRate;
if (harrisLast.at<float>(yInd, xInd) > 1e-7) {
featuresLast->push_back(featuresSub[k]);
featuresInd.push_back(featuresIndFromStart);
numFound++;
featuresIndFromStart++;
}
}
totalFeatureNum += numFound;
subregionFeatureNum[ind] += numFound;
}
}
}
}
if (totalFeatureNum > 0) {
Mat featuresLastMat(1, totalFeatureNum, CV_32FC2, (void*)&(*featuresLast)[0]);
oclMat oclFeaturesLast(featuresLastMat);
oclMat oclFeaturesCur, oclFeaturesStatus;
oclOpticalFlow.sparse(oclImageLast, oclImageCur, oclFeaturesLast, oclFeaturesCur, oclFeaturesStatus);
if (oclFeaturesCur.cols > 0 && oclFeaturesCur.cols == oclFeaturesStatus.cols) {
download(oclFeaturesCur, *featuresCur);
download(oclFeaturesStatus, featuresStatus);
totalFeatureNum = featuresCur->size();
} else {
totalFeatureNum = 0;
}
}
for (int i = 0; i < totalSubregionNum; i++) {
subregionFeatureNum[i] = 0;
}
ImagePoint point;
int featureCount = 0;
for (int i = 0; i < totalFeatureNum; i++) {
double trackDis = sqrt(((*featuresLast)[i].x - (*featuresCur)[i].x)
* ((*featuresLast)[i].x - (*featuresCur)[i].x)
+ ((*featuresLast)[i].y - (*featuresCur)[i].y)
* ((*featuresLast)[i].y - (*featuresCur)[i].y));
if (!(trackDis > maxTrackDis || (*featuresCur)[i].x < xBoundary ||
(*featuresCur)[i].x > imageWidth - xBoundary || (*featuresCur)[i].y < yBoundary ||
(*featuresCur)[i].y > imageHeight - yBoundary) && featuresStatus[i]) {
int xInd = (int)(((*featuresLast)[i].x - xBoundary) / subregionWidth);
int yInd = (int)(((*featuresLast)[i].y - yBoundary) / subregionHeight);
int ind = xSubregionNum * yInd + xInd;
if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) {
(*featuresCur)[featureCount] = (*featuresCur)[i];
(*featuresLast)[featureCount] = (*featuresLast)[i];
featuresInd[featureCount] = featuresInd[i];
point.u = -((*featuresCur)[featureCount].x - kImage[2]) / kImage[0];
point.v = -((*featuresCur)[featureCount].y - kImage[5]) / kImage[4];
point.ind = featuresInd[featureCount];
imagePointsCur->push_back(point);
if (i >= recordFeatureNum) {
point.u = -((*featuresLast)[featureCount].x - kImage[2]) / kImage[0];
point.v = -((*featuresLast)[featureCount].y - kImage[5]) / kImage[4];
imagePointsLast->push_back(point);
}
featureCount++;
subregionFeatureNum[ind]++;
}
}
}
totalFeatureNum = featureCount;
featuresCur->resize(totalFeatureNum);
featuresLast->resize(totalFeatureNum);
featuresInd.resize(totalFeatureNum);
sensor_msgs::PointCloud2 imagePointsLast2;
pcl::toROSMsg(*imagePointsLast, imagePointsLast2);
imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast);
imagePointsLastPubPointer->publish(imagePointsLast2);
showCount = (showCount + 1) % (showSkipNum + 1);
if (showCount == showSkipNum) {
bridge.image = imageShow;
bridge.encoding = "mono8";
sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
imageShowPubPointer->publish(imageShowPointer);
}
}