Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13268] pkg/trunk
|
From: <mar...@us...> - 2009-04-02 00:15:10
|
Revision: 13268
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13268&view=rev
Author: mariusmuja
Date: 2009-04-02 00:15:08 +0000 (Thu, 02 Apr 2009)
Log Message:
-----------
Cleaned up vision door handle detection.
Modified Paths:
--------------
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
pkg/trunk/vision/door_handle_detect/CMakeLists.txt
pkg/trunk/vision/door_handle_detect/handles.launch
pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp
Added Paths:
-----------
pkg/trunk/vision/door_handle_detect/src/test.cpp
Modified: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-04-01 23:58:32 UTC (rev 13267)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-04-02 00:15:08 UTC (rev 13268)
@@ -313,6 +313,13 @@
{
ready_ = true;
}
+
+ void reset()
+ {
+ expected_count_ = 0;
+ count_ = 0;
+ done_ = false;
+ }
};
#endif
Modified: pkg/trunk/vision/door_handle_detect/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/door_handle_detect/CMakeLists.txt 2009-04-01 23:58:32 UTC (rev 13267)
+++ pkg/trunk/vision/door_handle_detect/CMakeLists.txt 2009-04-02 00:15:08 UTC (rev 13268)
@@ -6,3 +6,5 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
rospack_add_executable(door_handle_detect src/door_handle_detect.cpp)
+
+rospack_add_executable(test_service src/test.cpp)
Modified: pkg/trunk/vision/door_handle_detect/handles.launch
===================================================================
--- pkg/trunk/vision/door_handle_detect/handles.launch 2009-04-01 23:58:32 UTC (rev 13267)
+++ pkg/trunk/vision/door_handle_detect/handles.launch 2009-04-02 00:15:08 UTC (rev 13268)
@@ -24,6 +24,8 @@
<!-- <node pkg="dcam" type="stereodcam" respawn="false"/>-->
<node pkg="stereo_image_proc" type="stereoproc" respawn="false"/>
- <node pkg="door_handle_detect" name="door_handle_detect" type="door_handle_detect" respawn="false" output="screen"/>
+ <node pkg="door_handle_detect" name="door_handle_detect" type="door_handle_detect" respawn="false" output="screen">
+ <param name="display" type="bool" value="true" />
+ </node>
</launch>
Modified: pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp
===================================================================
--- pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp 2009-04-01 23:58:32 UTC (rev 13267)
+++ pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp 2009-04-02 00:15:08 UTC (rev 13268)
@@ -104,317 +104,280 @@
{
public:
+
image_msgs::Image limage;
image_msgs::Image rimage;
image_msgs::Image dimage;
image_msgs::StereoInfo stinfo;
image_msgs::DisparityInfo dispinfo;
image_msgs::CamInfo rcinfo;
-
image_msgs::CvBridge lbridge;
image_msgs::CvBridge rbridge;
image_msgs::CvBridge dbridge;
+ robot_msgs::PointCloud cloud_fetch;
robot_msgs::PointCloud cloud;
- robot_msgs::Door door;
- robot_msgs::Point32 door_handle;
IplImage* left;
IplImage* right;
IplImage* disp;
IplImage* disp_clone;
- CvScalar door_plane;
- bool have_door_plane;
- bool pause;
- bool display;
-
- string destination_frame;
-
TopicSynchronizer<HandleDetector> sync;
boost::mutex cv_mutex;
+ boost::condition_variable images_ready;
tf::TransformListener *tf_;
- CvHaarClassifierCascade* cascade;
- CvMemStorage* storage;
+ // minimum height to look at (in base_link frame)
+ double min_height;
+ // maximum height to look at (in base_link frame)
+ double max_height;
+ // no. of frames to detect handle in
+ int frames_no;
+ // display stereo images ?
+ bool display;
- HandleDetector() : ros::Node("stereo_view"),
- left(NULL), right(NULL), disp(NULL), disp_clone(NULL), pause(false),
- sync(this, &HandleDetector::image_cb_all, ros::Duration().fromSec(0.05), &HandleDetector::image_cb_timeout)
- {
- tf_ = new tf::TransformListener(*this);
- param("~display", display, false);
+ CvHaarClassifierCascade* cascade;
+ CvMemStorage* storage;
- if (display) {
- cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
- //cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
- // cvNamedWindow("contours", CV_WINDOW_AUTOSIZE);
- cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
- cvNamedWindow("disparity_original", CV_WINDOW_AUTOSIZE);
- }
+ HandleDetector()
+ :ros::Node("stereo_view"), left(NULL), right(NULL), disp(NULL), disp_clone(NULL), sync(this, &HandleDetector::image_cb_all, ros::Duration().fromSec(0.1), &HandleDetector::image_cb_timeout)
+ {
+ tf_ = new tf::TransformListener(*this);
+ // define node parameters
- std::list<std::string> left_list;
- left_list.push_back(std::string("stereo/left/image_rect_color"));
- left_list.push_back(std::string("stereo/left/image_rect"));
-// std::list<std::string> right_list;
-// right_list.push_back(std::string("stereo/right/image_rect_color"));
-// right_list.push_back(std::string("stereo/right/image_rect"));
+ param("~min_height", min_height, 0.7);
+ param("~max_height", max_height, 1.0);
+ param("~frames_no", frames_no, 7);
- sync.subscribe(left_list, limage, 1);
-// sync.subscribe(right_list, rimage, 1);
- sync.subscribe("stereo/disparity", dimage, 1);
- sync.subscribe("stereo/stereo_info", stinfo, 1);
- sync.subscribe("stereo/disparity_info", dispinfo, 1);
- sync.subscribe("stereo/right/cam_info", rcinfo, 1);
-
- sync.subscribe("stereo/cloud", cloud, 1);
- sync.ready();
-
- have_door_plane = false;
-
- subscribe("/door_detector/door_msg", door, &HandleDetector::door_detected,1);
-
- string handle_features_file;
- string background_features_file;
+ param("~display", display, false);
stringstream ss;
ss << getenv("ROS_ROOT") << "/../ros-pkg/vision/door_handle_detect/data/";
string path = ss.str();
-
string cascade_classifier;
- param<string>("cascade_classifier", cascade_classifier, path+"handles_data.xml");
- ROS_INFO("Loading cascade classifier\n");
- cascade = (CvHaarClassifierCascade*)cvLoad( cascade_classifier.c_str(), 0, 0, 0 );
+ param<string>("cascade_classifier", cascade_classifier, path + "handles_data.xml");
- if (!cascade) {
- ROS_ERROR("Cannot load cascade classifier\n");
+ if(display){
+ cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
+ //cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
+ cvNamedWindow("disparity_original", CV_WINDOW_AUTOSIZE);
}
- storage = cvCreateMemStorage(0);
+// subscribeStereoData();
+
+ // load a cascade classifier
+ loadClassifier(cascade_classifier);
// invalid location until we get a detection
- door_handle.x = -1;
- door_handle.y = -1;
- door_handle.z = -1;
- param<string>("destination_frame", destination_frame, "odom_combined");
- advertise<robot_msgs::PointStamped>("handle_detector/handle_location",1);
- advertise<robot_msgs::VisualizationMarker>("visualizationMarker",1);
- advertiseService("door_handle_vision_detector", &HandleDetector::detectDoorSrv, this);
- }
+// advertise<robot_msgs::PointStamped>("handle_detector/handle_location", 1);
+ advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 1);
+ advertiseService("door_handle_vision_detector", &HandleDetector::detectHandleSrv, this);
+ }
- ~HandleDetector()
- {
- if (left) {
- cvReleaseImage(&left);
- }
- if (right) {
- cvReleaseImage(&right);
- }
- if (disp) {
- cvReleaseImage(&disp);
- }
- if (storage) {
- cvReleaseMemStorage(&storage);
- }
- }
+ ~HandleDetector()
+ {
+ if(left){
+ cvReleaseImage(&left);
+ }
+ if(right){
+ cvReleaseImage(&right);
+ }
+ if(disp){
+ cvReleaseImage(&disp);
+ }
+ if(storage){
+ cvReleaseMemStorage(&storage);
+ }
+ }
private:
- /////////////////////////////////////////////////
- // Analyze the disparity image that values should not be too far off from one another
- // Id -- 8 bit, 1 channel disparity image
- // R -- rectangular region of interest
- // vertical -- This is a return that tells whether something is on a wall (descending disparities) or not.
- // minDisparity -- disregard disparities less than this
- //
- double disparitySTD(IplImage *Id, CvRect &R, double& meanDisparity, double minDisparity = 0.5 )
- {
- int ws = Id->widthStep;
- unsigned char *p = (unsigned char *)(Id->imageData);
- int rx = R.x;
- int ry = R.y;
- int rw = R.width;
- int rh = R.height;
- int nchan = Id->nChannels;
- p += ws*ry + rx*nchan; //Put at start of box
- double mean = 0.0,var = 0.0;
- double val;
- int cnt = 0;
- //For vertical objects, Disparities should decrease from top to bottom, measure that
- for(int Y=0; Y<rh; ++Y)
- {
- for(int X=0; X<rw; X++, p+=nchan)
- {
- val = (double)*p;
- if(val < minDisparity)
- continue;
- mean += val;
- var += val*val;
- cnt++;
- }
- p+=ws-(rw*nchan);
- }
- if(cnt == 0) //Error condition, no disparities, return impossible variance
- {
- return 10000000.0;
- }
- //DO THE VARIANCE MATH
- mean = mean/(double)cnt;
- var = (var/(double)cnt) - mean*mean;
- meanDisparity = mean;
- return(sqrt(var));
- }
+ void loadClassifier(string cascade_classifier)
+ {
+ // subscribe("/door_detector/door_msg", door, &HandleDetector::door_detected,1);
+ ROS_INFO("Loading cascade classifier");
+ cascade = (CvHaarClassifierCascade*)(cvLoad(cascade_classifier.c_str(), 0, 0, 0));
+ if(!cascade){
+ ROS_ERROR("Cannot load cascade classifier\n");
+ }
+ storage = cvCreateMemStorage(0);
+ }
+ void subscribeStereoData()
+ {
- robot_msgs::Point disparityTo3D(CvStereoCamModel& cam_model, int x, int y, double d)
- {
- CvMat* uvd = cvCreateMat(1,3,CV_32FC1);
- cvmSet(uvd,0,0,x);
- cvmSet(uvd,0,1,y);
- cvmSet(uvd,0,2,d);
- CvMat* xyz = cvCreateMat(1,3,CV_32FC1);
- cam_model.dispToCart(uvd,xyz);
- robot_msgs::Point result;
- result.x = cvmGet(xyz,0,0);
- result.y = cvmGet(xyz,0,1);
- result.z = cvmGet(xyz,0,2);
- return result;
- }
+ sync.reset();
+ std::list<std::string> left_list;
+ left_list.push_back(std::string("stereo/left/image_rect_color"));
+ left_list.push_back(std::string("stereo/left/image_rect"));
+ sync.subscribe(left_list, limage, 1);
+ // std::list<std::string> right_list;
+ // right_list.push_back(std::string("stereo/right/image_rect_color"));
+ // right_list.push_back(std::string("stereo/right/image_rect"));
+ // sync.subscribe(right_list, rimage, 1);
- /**
- *
- * @param a
- * @param b
- * @return
- */
- double distance3D(robot_msgs::Point a, robot_msgs::Point b)
- {
- return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y)+(a.z-b.z)*(a.z-b.z));
- }
+ sync.subscribe("stereo/disparity", dimage, 1);
+ sync.subscribe("stereo/stereo_info", stinfo, 1);
+ sync.subscribe("stereo/disparity_info", dispinfo, 1);
+ sync.subscribe("stereo/right/cam_info", rcinfo, 1);
+ sync.subscribe("stereo/cloud", cloud_fetch, 1);
+ sync.ready();
+// sleep(1);
+ }
- void get_bb_dimensions(IplImage *Id, CvRect &R, double meanDisparity, double& dx, double& dy, robot_msgs::Point& center)
- {
- // initialize stereo camera model
- double Fx = rcinfo.P[0]; double Fy = rcinfo.P[5];
- double Clx = rcinfo.P[2]; double Crx = Clx;
- double Cy = rcinfo.P[6];
- double Tx = -rcinfo.P[3]/Fx;
- CvStereoCamModel cam_model(Fx,Fy,Tx,Clx,Crx,Cy,4.0/(double)dispinfo.dpp);
+ void unsubscribeStereoData()
+ {
+// unsubscribe("stereo/left/image_rect_color");
+ unsubscribe("stereo/left/image_rect");
+ unsubscribe("stereo/disparity");
+ unsubscribe("stereo/stereo_info");
+ unsubscribe("stereo/disparity_info");
+ unsubscribe("stereo/right/cam_info");
+ unsubscribe("stereo/cloud");
+ }
+ /////////////////////////////////////////////////
+ // Analyze the disparity image that values should not be too far off from one another
+ // Id -- 8 bit, 1 channel disparity image
+ // R -- rectangular region of interest
+ // vertical -- This is a return that tells whether something is on a wall (descending disparities) or not.
+ // minDisparity -- disregard disparities less than this
+ //
+ double disparitySTD(IplImage *Id, const CvRect & R, double & meanDisparity, double minDisparity = 0.5)
+ {
+ int ws = Id->widthStep;
+ unsigned char *p = (unsigned char*)(Id->imageData);
+ int rx = R.x;
+ int ry = R.y;
+ int rw = R.width;
+ int rh = R.height;
+ int nchan = Id->nChannels;
+ p += ws * ry + rx * nchan; //Put at start of box
+ double mean = 0.0, var = 0.0;
+ double val;
+ int cnt = 0;
+ //For vertical objects, Disparities should decrease from top to bottom, measure that
+ for(int Y = 0;Y < rh;++Y){
+ for(int X = 0;X < rw;X++, p += nchan){
+ val = (double)*p;
+ if(val < minDisparity)
+ continue;
- int ws = Id->widthStep;
- unsigned char *p = (unsigned char *)(Id->imageData);
- int rx = R.x;
- int ry = R.y;
- int rw = R.width;
- int rh = R.height;
- int nchan = Id->nChannels;
- p += ws*ry + rx*nchan; //Put at start of box
+ mean += val;
+ var += val * val;
+ cnt++;
+ }
+ p += ws - (rw * nchan);
+ }
- robot_msgs::Point p1 = disparityTo3D(cam_model, rx, ry, meanDisparity);
- robot_msgs::Point p2 = disparityTo3D(cam_model, rx+rw, ry, meanDisparity);
- robot_msgs::Point p3 = disparityTo3D(cam_model, rx, ry+rh, meanDisparity);
+ if(cnt == 0){
+ return 10000000.0;
+ }
+ //DO THE VARIANCE MATH
+ mean = mean / (double)cnt;
+ var = (var / (double)cnt) - mean * mean;
+ meanDisparity = mean;
+ return (sqrt(var));
+ }
- center = disparityTo3D(cam_model, rx+rw/2, ry+rh/2, meanDisparity);
+ /**
+ * \brief Transforms a disparity image pixel to real-world point
+ *
+ * @param cam_model Camera model
+ * @param x coordinate in the disparity image
+ * @param y coordinate in the disparity image
+ * @param d disparity pixel value
+ * @return point in 3D space
+ */
+ robot_msgs::Point disparityTo3D(CvStereoCamModel & cam_model, int x, int y, double d)
+ {
+ CvMat *uvd = cvCreateMat(1, 3, CV_32FC1);
+ cvmSet(uvd, 0, 0, x);
+ cvmSet(uvd, 0, 1, y);
+ cvmSet(uvd, 0, 2, d);
+ CvMat *xyz = cvCreateMat(1, 3, CV_32FC1);
+ cam_model.dispToCart(uvd, xyz);
+ robot_msgs::Point result;
+ result.x = cvmGet(xyz, 0, 0);
+ result.y = cvmGet(xyz, 0, 1);
+ result.z = cvmGet(xyz, 0, 2);
+ return result;
+ }
- dx = distance3D(p1,p2);
- dy = distance3D(p1,p3);
+ /**
+ * \brief Computes distance between two 3D points
+ *
+ * @param a
+ * @param b
+ * @return
+ */
+ double distance3D(robot_msgs::Point a, robot_msgs::Point b)
+ {
+ return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
+ }
- }
+ /**
+ * \brief Computes size and center of ROI in real-world
+ *
+ * Given a disparity images and a ROI in the image this function computes the approximate real-world size
+ * and center of the ROI.
+ *
+ * This function is just an approximation, it uses the mean value of the disparity in the ROI and assumes
+ * the ROI is flat region perpendicular on the camera z axis. It could be improved by finding a dominant plane
+ * in the and using only those disparity values.
+ *
+ * @param R
+ * @param meanDisparity
+ * @param dx
+ * @param dy
+ * @param center
+ */
+ void getROIDimensions(const CvRect& r, double & dx, double & dy, robot_msgs::Point & center)
+ {
+ // initialize stereo camera model
+ double Fx = rcinfo.P[0];
+ double Fy = rcinfo.P[5];
+ double Clx = rcinfo.P[2];
+ double Crx = Clx;
+ double Cy = rcinfo.P[6];
+ double Tx = -rcinfo.P[3] / Fx;
+ CvStereoCamModel cam_model(Fx, Fy, Tx, Clx, Crx, Cy, 4.0 / (double)dispinfo.dpp);
+ double mean = 0;
+ disparitySTD(disp, r, mean);
- bool handlePossibleHere(CvRect& r)
- {
- const float nz_fraction = 0.1;
+ robot_msgs::Point p1 = disparityTo3D(cam_model, r.x, r.y, mean);
+ robot_msgs::Point p2 = disparityTo3D(cam_model, r.x + r.width, r.y, mean);
+ robot_msgs::Point p3 = disparityTo3D(cam_model, r.x, r.y + r.height, mean);
+ center = disparityTo3D(cam_model, r.x + r.width / 2, r.y + r.height / 2, mean);
+ dx = distance3D(p1, p2);
+ dy = distance3D(p1, p3);
+ }
- cvSetImageROI(disp,r);
- cvSetImageCOI(disp, 1);
- int cnt = cvCountNonZero(disp);
- cvResetImageROI(disp);
- cvSetImageCOI(disp,0);
- double mean = 0;
- disparitySTD(disp, r, mean);
-
- if (cnt<nz_fraction*r.width*r.height) {
-// ROS_INFO("Too few pixels in the disparity, discarding");
- return false;
- }
-
- robot_msgs::PointCloud pc = filterPointCloud(r);
- CvScalar plane = estimatePlaneLS(pc);
- cnt = 0;
- double sum = 0;
- double max_dist = 0;
- for (size_t i = 0; i<pc.pts.size();++i) {
- robot_msgs::Point32 p = pc.pts[i];
- double dist = fabs(plane.val[0]*p.x+plane.val[1]*p.y+plane.val[2]*p.z+plane.val[3]);
- max_dist = max(max_dist,dist);
- sum += dist;
- cnt++;
- }
- sum /= cnt;
-
- printf("Average distance to plane: %f, max: %f\n", sum, max_dist);
-
- if (max_dist>0.1 || sum<0.005) {
-// cvRectangle(left, cvPoint(r.x,r.y), cvPoint(r.x+r.width, r.y+r.height), CV_RGB(0,0,255));
- return false;
- }
-
-
- double dx, dy;
- robot_msgs::Point p;
- get_bb_dimensions(disp, r, mean, dx, dy,p);
-
- if (dx>0.25 || dy>0.15) {
- ROS_INFO("Too big, discarding");
- return false;
- }
-
- robot_msgs::PointStamped pin, pout;
- pin.header.frame_id = cloud.header.frame_id;
- pin.header.stamp = cloud.header.stamp;
- pin.point.x = p.z;
- pin.point.y = -p.x;
- pin.point.z = -p.y;
-
-
- tf_->transformPoint("base_link", pin, pout);
-
-// printf("r: (%d, %d, %d, %d), sdv: %f, dx: %f, dy: %f, x: %f, y: %f, z: %f\n", r.x, r.y, r.width, r.height, sdv, dx, dy,
-// pout.point.x, pout.point.y, pout.point.z);
-
- if (pout.point.z>0.9 || pout.point.z<0.7) {
- ROS_INFO("Too high, discarding");
- return false;
-
- }
-
-
- // publish handle location
- tf_->transformPoint(destination_frame, pin, pout);
- door_handle.x = pout.point.x;
- door_handle.y = pout.point.y;
- door_handle.z = pout.point.z;
- publish("handle_detector/handle_location", pout);
-
-
- robot_msgs::VisualizationMarker marker;
- marker.header.frame_id = destination_frame;
- marker.header.stamp = ros::Time((uint64_t)0ULL);
+ /**
+ * \brief Publishes a visualization marker for a point.
+ * @param p
+ */
+ void showHandleMarker(robot_msgs::PointStamped p)
+ {
+ robot_msgs::VisualizationMarker marker;
+ marker.header.frame_id = p.header.frame_id;
+ marker.header.stamp = ros::Time((uint64_t)(0ULL));
marker.id = 0;
marker.type = robot_msgs::VisualizationMarker::SPHERE;
marker.action = robot_msgs::VisualizationMarker::ADD;
- marker.x = pout.point.x;
- marker.y = pout.point.y;
- marker.z = pout.point.z;
+ marker.x = p.point.x;
+ marker.y = p.point.y;
+ marker.z = p.point.z;
marker.yaw = 0.0;
marker.pitch = 0.0;
marker.roll = 0.0;
@@ -425,327 +388,574 @@
marker.r = 0;
marker.g = 255;
marker.b = 0;
+ publish("visualizationMarker", marker);
+ }
- publish( "visualizationMarker", marker );
- return true;
- }
+// void showPlaneMarker(CvScalar plane, robot_msgs::PointCloud pc)
+// {
+//
+// double min_d = 10000;
+// int min_i = -1;
+// for (size_t i=0;i<pc.pts.size();++i) {
+// float dist = fabs(plane.val[0]*pc.pts[i].x+plane.val[1]*pc.pts[i].y+plane.val[2]*pc.pts[i].z+plane.val[3]);
+// if (dist<min_d) {
+// min_d = dist;
+// min_i = i;
+// }
+// }
+//
+//
+// robot_msgs::VisualizationMarker marker;
+// marker.header.frame_id = pc.header.frame_id;
+// marker.header.stamp = pc.header.stamp;
+// marker.id = 1211;
+// marker.type = robot_msgs::VisualizationMarker::SPHERE;
+// marker.action = robot_msgs::VisualizationMarker::ADD;
+// marker.x = pc.pts[min_i].x;
+// marker.y = pc.pts[min_i].y;
+// marker.z = pc.pts[min_i].z;
+// marker.yaw = 0.0;
+// marker.pitch = 0.0;
+// marker.roll = 0.0;
+// marker.xScale = 0.1;
+// marker.yScale = 0.1;
+// marker.zScale = 0.1;
+// marker.alpha = 255;
+// marker.r = 255;
+// marker.g = 0;
+// marker.b = 0;
+// publish("visualizationMarker", marker);
+//
+// printf("Show marker at: (%f,%f,%f)", marker.x, marker.y, marker.z);
+// }
- void findHandleCascade( )
- {
+ /**
+ * \brief Determine if it's possible for handle to be in a specific ROI
+ *
+ * It looks at things like, height, approximate size of ROI, how flat it is.
+ *
+ * @param r
+ * @return
+ */
+ bool handlePossibleHere(const CvRect &r)
+ {
+ const float nz_fraction = 0.1;
- IplImage *gray = cvCreateImage( cvSize(left->width,left->height), 8, 1 );
+ cvSetImageROI(disp, r);
+ cvSetImageCOI(disp, 1);
+ int cnt = cvCountNonZero(disp);
+ cvResetImageROI(disp);
+ cvSetImageCOI(disp, 0);
- cvCvtColor( left, gray, CV_BGR2GRAY );
- cvEqualizeHist( gray, gray );
- cvClearMemStorage( storage );
+ if(cnt < nz_fraction * r.width * r.height){
+ return false;
+ }
- if( cascade )
- {
- CvSeq* handles = cvHaarDetectObjects( gray, cascade, storage,
- 1.1, 2, 0
- //|CV_HAAR_FIND_BIGGEST_OBJECT
- //|CV_HAAR_DO_ROUGH_SEARCH
- //|CV_HAAR_DO_CANNY_PRUNING
- //|CV_HAAR_SCALE_IMAGE
- ,
- cvSize(10, 10) );
- for(int i = 0; i < (handles ? handles->total : 0); i++ )
- {
- CvRect* r = (CvRect*)cvGetSeqElem( handles, i );
- if (handlePossibleHere(*r)) {
- cvRectangle(left, cvPoint(r->x,r->y), cvPoint(r->x+r->width, r->y+r->height), CV_RGB(0,255,0));
- }
- else {
-// cvRectangle(left, cvPoint(r->x,r->y), cvPoint(r->x+r->width, r->y+r->height), CV_RGB(255,0,0));
- }
- }
- }
+ // compute least-squares handle plane
+ robot_msgs::PointCloud pc = filterPointCloud(r);
+ CvScalar plane = estimatePlaneLS(pc);
- cvReleaseImage( &gray );
- }
+ cnt = 0;
+ double sum = 0;
+ double max_dist = 0;
+ for(size_t i = 0;i < pc.pts.size();++i){
+ robot_msgs::Point32 p = pc.pts[i];
+ double dist = fabs(plane.val[0] * p.x + plane.val[1] * p.y + plane.val[2] * p.z + plane.val[3]);
+ max_dist = max(max_dist, dist);
+ sum += dist;
+ cnt++;
+ }
+ sum /= cnt;
+ if(max_dist > 0.1 || sum < 0.005){
+ return false;
+ }
+ double dx, dy;
+ robot_msgs::Point p;
+ getROIDimensions(r, dx, dy, p);
+ if(dx > 0.25 || dy > 0.15){
+ ROS_INFO("Too big, discarding");
+ return false;
+ }
- void image_cb_all(ros::Time t)
- {
- if (pause) return;
- cv_mutex.lock();
+ robot_msgs::PointStamped pin, pout;
+ pin.header.frame_id = cloud.header.frame_id;
+ pin.header.stamp = cloud.header.stamp;
+ pin.point.x = p.z;
+ pin.point.y = -p.x;
+ pin.point.z = -p.y;
+ try {
+ tf_->transformPoint("base_link", pin, pout);
+ }
+ catch(tf::LookupException & ex){
+ ROS_ERROR("Lookup exception: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException & ex){
+ ROS_DEBUG("Extrapolation exception: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException & ex){
+ ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ }
- if (lbridge.fromImage(limage, "bgr"))
- {
- if(left != NULL)
- cvReleaseImage(&left);
- left = cvCloneImage(lbridge.toIpl());
- }
+ if(pout.point.z > max_height || pout.point.z < min_height){
+ return false;
+ }
- if (rbridge.fromImage(rimage, "bgr"))
- {
- if(right != NULL)
- cvReleaseImage(&right);
+ printf("Handle at: (%d,%d,%d,%d)\n", r.x,r.y,r.width, r.height);
- right = cvCloneImage(rbridge.toIpl());
- }
- if (dbridge.fromImage(dimage))
- {
- if(disp != NULL)
- cvReleaseImage(&disp);
+ return true;
+ }
- disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
- cvCvtScale(dbridge.toIpl(), disp, 4.0/dispinfo.dpp);
- }
+ /**
+ * \brief Start handle detection
+ */
+ void findHandleCascade(vector<CvRect> & handle_rect)
+ {
+ IplImage *gray = cvCreateImage(cvSize(left->width, left->height), 8, 1);
+ cvCvtColor(left, gray, CV_BGR2GRAY);
+ cvEqualizeHist(gray, gray);
+ cvClearMemStorage(storage);
+ if(cascade){
+ CvSeq *handles = cvHaarDetectObjects(gray, cascade, storage, 1.1, 2, 0, //|CV_HAAR_FIND_BIGGEST_OBJECT
+ //|CV_HAAR_DO_ROUGH_SEARCH
+ //|CV_HAAR_DO_CANNY_PRUNING
+ //|CV_HAAR_SCALE_IMAGE
+ cvSize(10, 10));
+ for(int i = 0;i < (handles ? handles->total : 0);i++){
+ CvRect *r = (CvRect*)cvGetSeqElem(handles, i);
- if (display) {
- cvShowImage("left", left);
- //cvShowImage("right", right);
- // cvShowImage("disparity", disp_clone);
+ if(handlePossibleHere(*r)){
+ handle_rect.push_back(*r);
+// if(display){
+// cvRectangle(left, cvPoint(r->x, r->y), cvPoint(r->x + r->width, r->y + r->height), CV_RGB(0, 255, 0));
+// }
+ }
+ else{
+ // cvRectangle(left, cvPoint(r->x, r->y), cvPoint(r->x + r->width, r->y + r->height), CV_RGB(255, 0, 0));
+ }
+ }
- cvShowImage("disparity_original", disp);
+ }
+ else {
+ ROS_ERROR("Cannot look for handle, no detector loaded");
+ }
- }
+ cvReleaseImage(&gray);
+ }
- applyPositionPrior(disp);
- if (display) {
- cvShowImage("disparity", disp);
- }
-// findEdges(left);
- findHandleCascade();
+ /**
+ * \brief Finds edges in an image
+ * @param img
+ */
+ void findEdges(IplImage *img)
+ {
+ IplImage *gray = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
+ cvCvtColor(img, gray, CV_RGB2GRAY);
+ cvCanny(gray, gray, 20, 40);
+ CvMemStorage *storage = cvCreateMemStorage(0);
+ CvSeq *lines = 0;
+ lines = cvHoughLines2(gray, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI / 360, 100, 200, 100);
+ for(int i = 0;i < lines->total;i++){
+ CvPoint *line = (CvPoint*)cvGetSeqElem(lines, i);
+ CvPoint p1 = line[0];
+ CvPoint p2 = line[1];
+ if(abs(p1.x - p2.x) > 0){
+ float min_angle = 80;
+ float slope = float(abs(p1.y - p2.y)) / abs(p1.x - p2.x);
+ float min_slope = tan(CV_PI / 2 - min_angle * CV_PI / 180);
+ if(slope < min_slope)
+ continue;
+ printf("slope: %f, min_slope: %f\n", slope, min_slope);
+ }
+ cvLine(left, p1, p2, CV_RGB(255, 0, 0), 2, CV_AA, 0);
+ }
- if (display) {
- // cvShowImage("disparity", disp);
- cvShowImage("left", left);
- }
- if (disp_clone!=NULL)
- cvReleaseImage(&disp_clone);
+ cvReleaseImage(&gray);
+ }
- cv_mutex.unlock();
+ /**
+ * \brief Checks if a point should belong to a cluster
+ *
+ * @param center
+ * @param p
+ * @return
+ */
+ bool belongsToCluster(pair<float,float> center, pair<float,float> p)
+ {
+ return fabs(center.first-p.first)<3 && fabs(center.second-p.second)<3;
+ }
-// pause = true;
- }
+ /**
+ * Helper function.
+ *
+ * @param r
+ * @return
+ */
+ pair<float,float> rectCenter(const CvRect& r)
+ {
+ return make_pair(r.x+(float)r.width/2,r.y+(float)r.height/2);
+ }
+ /**
+ * \brief Decide the handle position from several detections across multiple frames
+ *
+ * This method does some simple clustering of all the bounding boxes and picks the cluster with
+ * the most elements.
+ *
+ * @param handle_rect The handle bounding boxes
+ * @param handle Point indicating the real-world handle position
+ * @return
+ */
+ bool decideHandlePosition(vector<CvRect>& handle_rect, robot_msgs::PointStamped & handle)
+ {
+ if (handle_rect.size()==0) {
+ return false;
+ }
- void door_detected()
- {
- door_plane.val[0] = door.normal.x;
- door_plane.val[1] = door.normal.y;
- door_plane.val[2] = door.normal.z;
- door_plane.val[3] = -(door.normal.x*door.door_p1.x+door.normal.y*door.door_p1.y+door.normal.z*door.door_p1.z);
+ vector<int> cluster_size;
+ vector<pair<float,float> > centers;
+ vector<pair<float,float> > sizes;
- printf("Door plane: %f, %f, %f, %f\n", door_plane.val[0],door_plane.val[1],door_plane.val[2],door_plane.val[3]);
+ for (size_t i=0;i<handle_rect.size();++i) {
+ CvRect r = handle_rect[i];
+ pair<float,float> crt = rectCenter(r);
+ bool found_cluster = false;
+ for (size_t j=0;j<centers.size();++j) {
+ if (belongsToCluster(centers[j],crt)) {
+ int n = cluster_size[j];
+ centers[j].first = (centers[j].first*n+crt.first)/(n+1);
+ centers[j].second = (centers[j].second*n+crt.second)/(n+1);
+ sizes[j].first = (sizes[j].first*n+r.width)/(n+1);
+ sizes[j].second = (sizes[j].second*n+r.height)/(n+1);
+ cluster_size[j]++;
+ found_cluster = true;
+ break;
+ }
+ }
+ if (!found_cluster) {
+ centers.push_back(crt);
+ sizes.push_back(make_pair(r.width,r.height));
+ cluster_size.push_back(1);
+ }
+ }
- have_door_plane = true;
- }
+ int max_ind = 0;
+ int max = cluster_size[0];
- void findEdges(IplImage* img)
- {
- IplImage* gray = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
- cvCvtColor(img, gray, CV_RGB2GRAY);
- cvCanny(gray,gray,20,40);
+ for (size_t i=0;i<cluster_size.size();++i) {
+ if (cluster_size[i]>max) {
+ max = cluster_size[i];
+ max_ind = i;
+ }
+ }
- CvMemStorage* storage = cvCreateMemStorage(0);
- CvSeq* lines = 0;
- lines = cvHoughLines2( gray, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/360, 100, 200, 100 );
- for( int i = 0; i < lines->total; i++ )
- {
- CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i);
- CvPoint p1 = line[0];
- CvPoint p2 = line[1];
+ CvRect bbox;
+ bbox.x = (int) centers[max_ind].first-(int)(sizes[max_ind].first/2);
+ bbox.y = (int) centers[max_ind].second-(int)(sizes[max_ind].second/2);
+ bbox.width = (int) sizes[max_ind].first;
+ bbox.height = (int) sizes[max_ind].second;
+ double dx, dy;
+ robot_msgs::Point p;
+ getROIDimensions(bbox, dx, dy, p);
- if (abs(p1.x-p2.x)>0) {
- float min_angle = 80;
- float slope = float(abs(p1.y-p2.y))/abs(p1.x-p2.x);
- float min_slope = tan(CV_PI/2-min_angle*CV_PI/180);
- if (slope<min_slope) continue;
- printf("slope: %f, min_slope: %f\n",slope, min_slope);
- }
+ handle.header.frame_id = cloud.header.frame_id;
+ handle.header.stamp = cloud.header.stamp;
+ handle.point.x = p.z;
+ handle.point.y = -p.x;
+ handle.point.z = -p.y;
- cvLine( left, p1, p2, CV_RGB(255,0,0), 2, CV_AA, 0 );
- }
+ printf("Clustered Handle at: (%d,%d,%d,%d)\n", bbox.x,bbox.y,bbox.width, bbox.height);
- if (display) {
- cvShowImage("contours", gray);
- }
- cvReleaseImage(&gray);
- }
+ if(display){
+ cvRectangle(left, cvPoint(bbox.x, bbox.y), cvPoint(bbox.x + bbox.width, bbox.y + bbox.height), CV_RGB(0, 255, 0));
+ cvShowImage("left", left);
+ }
+// showHandleMarker(handle);
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- /** \brief Service call to detect doors*/
- //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- bool detectDoorSrv (door_handle_detector::DoorsDetector::Request &req,
- door_handle_detector::DoorsDetector::Response &resp)
- {
- resp.doors.resize(1);
+ return true;
+ }
- resp.doors[0] = req.door;
- if (door_handle.z>=0) { // valid door handle
- resp.doors[0].handle = door_handle;
- resp.doors[0].weight = 1;
- }
- else {
- resp.doors[0].weight = -1;
- return false;
- }
- return true;
- }
+ /**
+ * \brief Runs the handle detector
+ *
+ * @param handle Position of detected handle
+ * @return True if handle was found, false otherwise.
+ */
+ bool runHandleDetector(robot_msgs::PointStamped & handle)
+ {
+ vector<CvRect> handle_rect;
+ // acquire cv_mutex lock
+ boost::unique_lock<boost::mutex> images_lock(cv_mutex);
+ for (int i=0;i<frames_no;++i) {
- void image_cb_timeout(ros::Time t)
- {
- if (limage.header.stamp != t)
- printf("Timed out waiting for left image\n");
+ printf("Waiting for images\n");
+ // block until images are available to process
+ images_ready.wait(images_lock);
-// if (rimage.header.stamp != t)
-// printf("Timed out waiting for right image\n");
+ printf("Woke up, processing images\n");
- if (dimage.header.stamp != t)
- printf("Timed out waiting for disparity image\n");
+ if(display){
+ // show original disparity
+ cvShowImage("disparity_original", disp);
+ }
+ // eliminate from disparity locations that cannot contain a handle
+ applyPositionPrior();
+ // run cascade classifier
+ findHandleCascade(handle_rect);
+ if(display){
+ // show filtered disparity
+ cvShowImage("disparity", disp);
+ // show left image
+ cvShowImage("left", left);
+ }
+ }
- if (stinfo.header.stamp != t)
- printf("Timed out waiting for stereo info\n");
-//
- if (cloud.header.stamp != t)
- printf("Timed out waiting for point cloud\n");
+ bool found = decideHandlePosition(handle_rect, handle);
+ return found;
+ }
- }
+ /**
+ * \brief Service call to detect doors
+ */
+ bool detectHandleSrv(door_handle_detector::DoorsDetector::Request & req, door_handle_detector::DoorsDetector::Response & resp)
+ {
+ robot_msgs::PointStamped handle;
+ subscribeStereoData();
+ bool found = runHandleDetector(handle);
+ unsubscribeStereoData();
- robot_msgs::PointCloud filterPointCloud(const CvRect& rect)
- {
- robot_msgs::PointCloud result;
+ if(!found){
+ return false;
+ }
+ robot_msgs::PointStamped handle_transformed;
+ // transform the point in the expected frame
+ try {
+ tf_->transformPoint(req.door.header.frame_id, handle, handle_transformed);
+ }
+ catch(tf::LookupException & ex){
+ ROS_ERROR("Lookup exception: %s\n", ex.what());
+ }
+ catch(tf::ExtrapolationException & ex){
+ ROS_DEBUG("Extrapolation exception: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException & ex){
+ ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ }
+ if(found){
+ resp.doors.resize(1);
+ resp.doors[0] = req.door;
+ resp.doors[0].handle.x = handle.point.x;
+ resp.doors[0].handle.y = handle.point.y;
+ resp.doors[0].handle.z = handle.point.z;
+ resp.doors[0].weight = 1;
+ }else{
+ resp.doors[0].weight = -1;
+ }
+ return true;
+ }
- int xchan = -1;
- int ychan = -1;
- for (size_t i=0;i<cloud.chan.size();++i) {
- if (cloud.chan[i].name == "x") {
- xchan = i;
- }
- if (cloud.chan[i].name == "y") {
- ychan = i;
- }
- }
+ /**
+ * \brief Filters a cloud point, retains only points coming from a specific region in the disparity image
+ *
+ * @param rect Region in disparity image
+ * @return Filtered point cloud
+ */
+ robot_msgs::PointCloud filterPointCloud(const CvRect & rect)
+ {
+ robot_msgs::PointCloud result;
+ result.header.frame_id = cloud.header.frame_id;
+ result.header.stamp = cloud.header.stamp;
+ int xchan = -1;
+ int ychan = -1;
+ for(size_t i = 0;i < cloud.chan.size();++i){
+ if(cloud.chan[i].name == "x"){
+ xchan = i;
+ }
+ if(cloud.chan[i].name == "y"){
+ ychan = i;
+ }
+ }
- if (xchan!=-1 && ychan!=-1) {
- for (size_t i=0;i<cloud.pts.size();++i) {
- int x = (int)cloud.chan[xchan].vals[i];
- int y = (int)cloud.chan[ychan].vals[i];
- if (x>=rect.x && x<rect.x+rect.width && y>=rect.y && y<rect.y+rect.height) {
- result.pts.push_back(cloud.pts[i]);
- }
- }
- }
+ if(xchan != -1 && ychan != -1){
+ for(size_t i = 0;i < cloud.pts.size();++i){
+ int x = (int)(cloud.chan[xchan].vals[i]);
+ int y = (int)(cloud.chan[ychan].vals[i]);
+ if(x >= rect.x && x < rect.x + rect.width && y >= rect.y && y < rect.y + rect.height){
+ result.pts.push_back(cloud.pts[i]);
+ }
+ }
- return result;
- }
+ }
-//
-// // returns the plane in Hessian normal form
- CvScalar estimatePlaneLS(robot_msgs::PointCloud points)
- {
- int cnt = points.pts.size();
- CvMat* A = cvCreateMat(cnt, 3, CV_32FC1);
+ return result;
+ }
- for (int i=0;i<cnt;++i) {
- robot_msgs::Point32 p = points.pts[i];
- cvmSet(A,i,0,p.x);
- cvmSet(A,i,1,p.y);
- cvmSet(A,i,2,p.z);
- }
+ /**
+ * \brief Computes a least-squares estimate of a plane.
+ *
+ * @param points The point cloud
+ * @return Plane in Hessian normal form
+ */
+ CvScalar estimatePlaneLS(robot_msgs::PointCloud points)
+ {
+ int cnt = points.pts.size();
+ CvMat *A = cvCreateMat(cnt, 3, CV_32FC1);
+ for(int i = 0;i < cnt;++i){
+ robot_msgs::Point32 p = points.pts[i];
+ cvmSet(A, i, 0, p.x);
+ cvmSet(A, i, 1, p.y);
+ cvmSet(A, i, 2, p.z);
+ }
+ vector<float> ones(cnt, 1);
+ CvMat B;
+ cvInitMatHeader(&B, cnt, 1, CV_32FC1, &ones[0]);
+ CvMat *X = cvCreateMat(3, 1, CV_32FC1);
+ int ok = cvSolve(A, &B, X, CV_SVD);
+ CvScalar plane;
+ if(ok){
+ float *xp = X->data.fl;
+ float d = sqrt(xp[0] * xp[0] + xp[1] * xp[1] + xp[2] * xp[2]);
+ plane.val[0] = xp[0] / d;
+ plane.val[1] = xp[1] / d;
+ plane.val[2] = xp[2] / d;
+ plane.val[3] = -1 / d;
+ }else{
+ plane.val[0] = plane.val[1] = plane.val[2] = plane.val[3] = -1;
+ }
+ cvReleaseMat(&A);
+ return plane;
+ }
- vector<float> ones(cnt,1);
- CvMat B;
- cvInitMatHeader(&B,cnt,1,CV_32FC1,&ones[0]);
- CvMat* X = cvCreateMat(3,1,CV_32FC1);
+ /**
+ * \brief Filters cloud point, retains only regions that could contain a handle
+ */
+ void applyPositionPrior()
+ {
+ robot_msgs::PointCloud base_cloud;
+ tf_->setExtrapolationLimit(ros::Duration(2.0));
+ try {
+ tf_->transformPointCloud("base_link", cloud, base_cloud);
+ }
+ catch(tf::ExtrapolationException & ex){
+ tf_->clear();
+ ROS_WARN("TF exception: %s", ex.what());
+ }
+ catch(tf::LookupException & ex){
+ ROS_ERROR("Lookup exception: %s\n", ex.what());
+ }
+ catch(tf::ConnectivityException & ex){
+ ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ }
+ int xchan = -1;
+ int ychan = -1;
+ for(size_t i = 0;i < base_cloud.chan.size();++i){
+ if(base_cloud.chan[i].name == "x"){
+ xchan = i;
+ }
+ if(base_cloud.chan[i].name == "y"){
+ ychan = i;
+ }
+ }
- int ok = cvSolve( A, &B, X,CV_SVD );
+ if(xchan != -1 && ychan != -1){
+ unsigned char *pd = (unsigned char*)(disp->imageData);
+ int ws = disp->widthStep;
+ for(size_t i = 0;i < base_cloud.get_pts_size();++i){
+ robot_msgs::Point32 crt_point = base_cloud.pts[i];
+ int x = (int)(base_cloud.chan[xchan].vals[i]);
+ int y = (int)(base_cloud.chan[ychan].vals[i]);
- CvScalar plane;
-
- if (ok) {
- float* xp = X->data.fl;
-
- float d = sqrt(xp[0]*xp[0]+xp[1]*xp[1]+xp[2]*xp[2]);
- plane.val[0] = xp[0]/d;
- plane.val[1] = xp[1]/d;
- plane.val[2] = xp[2]/d;
- plane.val[3] = -1/d;
+ // pointer to the current pixel
+ unsigned char* crt_pd = pd+y*ws+x;
+ if (crt_point.z>max_height || crt_point.z<min_height) {
+ *crt_pd = 0;
+ }
+ }
}
else {
- plane.val[0] = plane.val[1] = plane.val[2] = plane.val[3] = -1;
+ ROS_WARN("I can't find image coordinates in the point cloud, no filtering done.");
}
+ }
- cvReleaseMat(&A);
- return plane;
- }
+ /**
+ * Callback from topic synchronizer, timeout
+ * @param t
+ */
+ void image_cb_timeout(ros::Time t)
+ {
+ if(limage.header.stamp != t) {
+ printf("Timed out waiting for left image\n");
+ }
- void applyPositionPrior(IplImage* disp)
- {
- robot_msgs::PointCloud base_cloud;
+ if(dimage.header.stamp != t) {
+ printf("Timed out waiting for disparity image\n");
+ }
- tf_->setExtrapolationLimit(ros::Duration(2.0));
- try {
- tf_->transformPointCloud("base_link", cloud, base_cloud);
- }
- catch(tf::ExtrapolationException& ex) {
- tf_->clear();
- ROS_WARN("TF exception: %s", ex.what());
- }
+ if(stinfo.header.stamp != t) {
+ printf("Timed out waiting for stereo info\n");
+ }
+ if(cloud_fetch.header.stamp != t) {
+ printf("Timed out waiting for point cloud\n");
+ }
+ }
- int xchan = -1;
- int ychan = -1;
- for (size_t i=0;i<base_cloud.chan.size();++i) {
- if (base_cloud.chan[i].name == "x") {
- xchan = i;
- }
- if (base_cloud.chan[i].name == "y") {
- ychan = i;
- }
- }
+ /**
+ * Callback from topic synchronizer, images ready to be consumed
+ * @param t
+ */
+ void image_cb_all(ros::Time t)
+ {
+ // obtain lock on vision data
+ boost::lock_guard<boost::mutex> lock(cv_mutex);
- if (xchan!=-1 && ychan!=-1) {
+ if(lbridge.fromImage(limage, "bgr")){
+ if(left != NULL)
+ cvReleaseImage(&left);
- unsigned char *pd = (unsigned char *)(disp->imageData);
- int ws = disp->widthStep;
- for (size_t i=0;i<base_cloud.get_pts_size();++i) {
- robot_msgs::Point32 crt_point = base_cloud.pts[i];
+ left = cvCloneImage(lbridge.toIpl());
+ }
+ if(rbridge.fromImage(rimage, "bgr")){
+ if(right != NULL)
+ cvReleaseImage(&right);
- int x = (int)base_cloud.chan[xchan].vals[i];
- int y = (int)base_cloud.chan[ychan].vals[i];
+ right = cvCloneImage(rbridge.toIpl());
+ }
+ if(dbridge.fromImage(dimage)){
+ if(disp != NULL)
+ cvReleaseImage(&disp);
- // pointer to the current pixel
- unsigned char* crt_pd = pd+y*ws+x;
- if (crt_point.z>1.00 || crt_point.z<0.70) {
- *crt_pd = 0;
- }
+ disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
+ cvCvtScale(dbridge.toIpl(), disp, 4.0 / dispinfo.dpp);
+ }
- if (*crt_pd>0 && have_door_plane) {
- // distance from the door plane
- double distance = crt_point.x*door_plane.val[0]+crt_point.y*door_plane.val[1]+crt_point.z*door_plane.val[2]+door_plane.val[3];
+ cloud = cloud_fetch;
- if (distance<0.01 || distance>0.1) {
- *crt_pd = 0;
- }
- }
+ images_ready.notify_all();
+ }
- }
- }
- else {
- ROS_WARN("I can't find image coordinates in the point cloud, no filtering done.");
- }
- }
+
public:
+ /**
+ * Needed for OpenCV event loop, to show images
+ * @return
+ */
bool spin()
{
while (ok())
@@ -755,9 +965,6 @@
if(key == 27) //ESC
break;
- if (key=='p')
- pause = false;
-
cv_mutex.unlock();
usleep(10000);
}
Added: pkg/trunk/vision/door_handle_detect/src/test.cpp
===================================================================
--- pkg/trunk/vision/door_handle_detect/src/test.cpp (rev 0)
+++ pkg/trunk/vision/door_handle_detect/src/test.cpp 2009-04-02 00:15:08 UTC (rev 13268)
@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////
+// The roscpp_tutorial package shows off the features of the c++ client library
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#include "ros/node.h"
+#include "door_handle_detector/DoorsDetector.h"
+#include <cstdlib>
+
+
+
+
+bool callDetector()
+{
+
+ door_handle_detector::DoorsDetector::Request req;
+ door_handle_detector::DoorsDetector::Response resp;
+
+ req.door.header.frame_id = "base_link";
+
+ if (ros::service::call("door_handle_vision_detector", req, resp))
+ {
+ printf("Handle found\n");
+ return true;
+ }
+ else
+ {
+ printf("Handle not found\n");
+ return false;
+ }
+}
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ ros::Node n("handle_detect_test");
+ callDetector();
+
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
