Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16117] pkg/trunk
|
From: <mar...@us...> - 2009-05-27 01:01:51
|
Revision: 16117
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16117&view=rev
Author: mariusmuja
Date: 2009-05-27 00:23:28 +0000 (Wed, 27 May 2009)
Log Message:
-----------
Changes to door_handle_detection
Modified Paths:
--------------
pkg/trunk/mapping/door_handle_detector/CMakeLists.txt
pkg/trunk/mapping/door_handle_detector/launch/door_handle_detector_bag.xml
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/mapping/door_handle_detector/test/detect_handle_from_bag.py
pkg/trunk/mapping/door_handle_detector/test/test_setup.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
pkg/trunk/vision/stereo_view/stereo_bag.launch
Modified: pkg/trunk/mapping/door_handle_detector/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/door_handle_detector/CMakeLists.txt 2009-05-27 00:12:25 UTC (rev 16116)
+++ pkg/trunk/mapping/door_handle_detector/CMakeLists.txt 2009-05-27 00:23:28 UTC (rev 16117)
@@ -46,21 +46,21 @@
rostest_door_handle(door_closed_conference_1)
rostest_door_handle(door_closed_josh)
rostest_door_handle(door_closed_kurt)
-rostest_door_handle(door_closed_sachin)
+#rostest_door_handle(door_closed_sachin)
rostest_door_handle(door_closed_tully)
# Currently failing tests below
#rostest_door_handle(door_closed_melonee)
#rostest_door_handle(door_closed_40)
-#rostest_door_handle(door_closed_alex)
+rostest_door_handle(door_closed_alex)
#rostest_door_handle(door_closed_blaise)
#rostest_door_handle(door_closed_conor)
#rostest_door_handle(door_closed_curt)
#rostest_door_handle(door_closed_daniel)
-#rostest_door_handle(door_closed_ethan)
-#rostest_door_handle(door_closed_john)
+rostest_door_handle(door_closed_ethan)
+rostest_door_handle(door_closed_john)
#rostest_door_handle(door_closed_melonee_2)
-#rostest_door_handle(door_closed_melonee_3)
+rostest_door_handle(door_closed_melonee_3)
#rostest_door_handle(door_closed_patrick)
-#rostest_door_handle(door_closed_victoria)
-#rostest_door_handle(door_closed_vijay)
+rostest_door_handle(door_closed_victoria)
+rostest_door_handle(door_closed_vijay)
Modified: pkg/trunk/mapping/door_handle_detector/launch/door_handle_detector_bag.xml
===================================================================
--- pkg/trunk/mapping/door_handle_detector/launch/door_handle_detector_bag.xml 2009-05-27 00:12:25 UTC (rev 16116)
+++ pkg/trunk/mapping/door_handle_detector/launch/door_handle_detector_bag.xml 2009-05-27 00:23:28 UTC (rev 16117)
@@ -5,6 +5,7 @@
<!-- Handle detector Camera -->
<node pkg="door_handle_detector" name="handle_detector_vision" type="handle_detector_vision" respawn="true" output="screen">
+ <param name="display" value="True" />
</node>
<!-- Handle detector Laser -->
Modified: pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-05-27 00:12:25 UTC (rev 16116)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-05-27 00:23:28 UTC (rev 16117)
@@ -75,6 +75,9 @@
using namespace std;
+#define ROSCONSOLE_MIN_SEVERITY ROSCONSOLE_SEVERITY_DEBUG
+
+
template <typename T>
class IndexedIplImage
{
@@ -521,84 +524,84 @@
bool handlePossibleHere(CvRect &r)
{
- tryShrinkROI(r);
+ tryShrinkROI(r);
- if (r.width<10 || r.height<10) {
- return false;
- }
+ if (r.width<10 || r.height<10) {
+ return false;
+ }
- cvSetImageROI(disp, r);
- cvSetImageCOI(disp, 1);
- int cnt;
- const float nz_fraction = 0.1;
- cnt = cvCountNonZero(disp);
- if (cnt < nz_fraction * r.width * r.height){
- cvResetImageROI(disp);
- cvSetImageCOI(disp, 0);
- return false;
- }
- cvResetImageROI(disp);
- cvSetImageCOI(disp, 0);
+ cvSetImageROI(disp, r);
+ cvSetImageCOI(disp, 1);
+ int cnt;
+ const float nz_fraction = 0.1;
+ cnt = cvCountNonZero(disp);
+ if (cnt < nz_fraction * r.width * r.height){
+ cvResetImageROI(disp);
+ cvSetImageCOI(disp, 0);
+ return false;
+ }
+ cvResetImageROI(disp);
+ cvSetImageCOI(disp, 0);
- // compute least-squares handle plane
- robot_msgs::PointCloud pc = filterPointCloud(r);
- CvScalar plane = estimatePlaneLS(pc);
+ // compute least-squares handle plane
+ 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;
- if(max_dist > 0.1 || sum < 0.002){
- ROS_DEBUG("Not enough depth variation for handle candidate: %f, %f\n", max_dist, sum);
- return false;
- }
+ cnt = 0;
+ double avg = 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);
+ avg += dist;
+ cnt++;
+ }
+ avg /= cnt;
+ if(max_dist > 0.1 || avg < 0.001){
+ ROS_DEBUG("Not enough depth variation for handle candidate: %f, %f\n", max_dist, avg);
+ return false;
+ }
- double dx, dy;
- robot_msgs::Point p;
- getROIDimensions(r, dx, dy, p);
- if(dx > 0.25 || dy > 0.15){
- ROS_DEBUG("Too big, discarding");
- return false;
- }
+ double dx, dy;
+ robot_msgs::Point p;
+ getROIDimensions(r, dx, dy, p);
+ if(dx > 0.25 || dy > 0.15){
+ ROS_DEBUG("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.x;
- pin.point.y = p.y;
- pin.point.z = p.z;
- try {
- tf_->transformPoint("base_footprint", 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());
- }
+ robot_msgs::PointStamped pin, pout;
+ pin.header.frame_id = cloud.header.frame_id;
+ pin.header.stamp = cloud.header.stamp;
+ pin.point.x = p.x;
+ pin.point.y = p.y;
+ pin.point.z = p.z;
+ try {
+ tf_->transformPoint("base_footprint", 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(pout.point.z > max_height || pout.point.z < min_height){
- printf("Height not within admissable range: %f\n", pout.point.z);
- return false;
- }
+ if(pout.point.z > max_height || pout.point.z < min_height){
+ printf("Height not within admissable range: %f\n", pout.point.z);
+ return false;
+ }
- ROS_DEBUG("Handle at: (%d,%d,%d,%d)", r.x,r.y,r.width, r.height);
+ ROS_DEBUG("Handle at: (%d,%d,%d,%d)", r.x,r.y,r.width, r.height);
- return true;
+ return true;
}
/**
@@ -623,7 +626,7 @@
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));
+ cvRectangle(left, cvPoint(r->x, r->y), cvPoint(r->x + r->width, r->y + r->height), CV_RGB(255, 255, 0));
cvRectangle(disp, cvPoint(r->x, r->y), cvPoint(r->x + r->width, r->y + r->height), CV_RGB(255, 255, 255));
}
}
Modified: pkg/trunk/mapping/door_handle_detector/test/detect_handle_from_bag.py
===================================================================
--- pkg/trunk/mapping/door_handle_detector/test/detect_handle_from_bag.py 2009-05-27 00:12:25 UTC (rev 16116)
+++ pkg/trunk/mapping/door_handle_detector/test/detect_handle_from_bag.py 2009-05-27 00:23:28 UTC (rev 16117)
@@ -60,6 +60,14 @@
# Threshold for agreement between laser and camera detection
self.epsilon = 0.1
+
+ def handle_dist(self, handle_laser, handle_camera):
+ dx = handle_laser.doors[0].handle.x - handle_camera.doors[0].handle.x
+ dy = handle_laser.doors[0].handle.y - handle_camera.doors[0].handle.y
+ dz = handle_laser.doors[0].handle.z - handle_camera.doors[0].handle.z
+ return math.sqrt(dx*dx + dy*dy + dz*dz)
+
+
def test_door_handle_detector(self):
rospy.sleep(2.0)
d = Door()
@@ -74,14 +82,17 @@
print "time ",d.header.stamp
door = self.detect_door_laser(d)
- handle_laser = self.detect_handle_laser(door.doors[0])
- handle_camera = self.detect_handle_camera(door.doors[0])
+ handle_camera = self.detect_handle_camera(door.doors[0])
+ cnt = 0
+
+ dist = self.epsilon + 1
+
+ while cnt<5 and dist>self.epsilon:
+ handle_laser = self.detect_handle_laser(door.doors[0])
+ dist = self.handle_dist(handle_laser,handle_camera)
+ cnt += 1
# Check handle positions against each other
- dx = handle_laser.doors[0].handle.x - handle_camera.doors[0].handle.x
- dy = handle_laser.doors[0].handle.y - handle_camera.doors[0].handle.y
- dz = handle_laser.doors[0].handle.z - handle_camera.doors[0].handle.z
- dist = math.sqrt(dx*dx + dy*dy + dz*dz)
print 'Difference between camera and laser: %9.6f'%(dist)
@@ -107,16 +118,21 @@
print "Waiting for service...", rospy.resolve_name('handle_detector')
rospy.wait_for_service('doors_detector')
print "Service is available"
- try:
- print "Getting service proxy"
- find_handle_laser = rospy.ServiceProxy('handle_detector', DoorsDetector)
- print "Calling service"
- door_reply = find_handle_laser(door_request)
- print "Request finished"
- print "Handle detected by laser at (%f, %f, %f)"%(door_reply.doors[0].handle.x, door_reply.doors[0].handle.y, door_reply.doors[0].handle.z)
- print "Laser frame", door_reply.doors[0].header.frame_id
- return door_reply
- except:
+ detected = True
+ for tries in xrange(2):
+ print "Laser detection try: %d"%(tries+1)
+ try:
+ print "Getting service proxy"
+ find_handle_laser = rospy.ServiceProxy('handle_detector', DoorsDetector)
+ print "Calling service"
+ door_reply = find_handle_laser(door_request)
+ print "Request finished"
+ print "Handle detected by laser at (%f, %f, %f)"%(door_reply.doors[0].handle.x, door_reply.doors[0].handle.y, door_reply.doors[0].handle.z)
+ print "Laser frame", door_reply.doors[0].header.frame_id
+ return door_reply
+ except:
+ detected = False
+ if not detected:
self.fail("handle_detector service call failed")
def detect_handle_camera(self, door_request):
Modified: pkg/trunk/mapping/door_handle_detector/test/test_setup.xml
===================================================================
--- pkg/trunk/mapping/door_handle_detector/test/test_setup.xml 2009-05-27 00:12:25 UTC (rev 16116)
+++ pkg/trunk/mapping/door_handle_detector/test/test_setup.xml 2009-05-27 00:23:28 UTC (rev 16117)
@@ -12,6 +12,7 @@
<param name="do_stereo" type="bool" value="True"/>
<param name="do_calc_points" type="bool" value="True"/>
<param name="do_keep_coords" type="bool" value="True"/>
+ <param name="num_disp" value="128"/>
</group>
<node pkg="stereo_image_proc" type="stereoproc" respawn="false" />
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-05-27 00:12:25 UTC (rev 16116)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-05-27 00:23:28 UTC (rev 16117)
@@ -53,6 +53,7 @@
<param name="exposure_auto" type="bool" value="true"/>
<param name="brightness_auto" type="bool" value="true"/>
<param name="gain_auto" type="bool" value="true"/>
+ <param name="num_disp" value="128"/>
</group>
<node machine="three" pkg="dcam" type="stereodcam" respawn="false"/>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-05-27 00:12:25 UTC (rev 16116)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-05-27 00:23:28 UTC (rev 16117)
@@ -65,6 +65,7 @@
<param name="brightness_auto" type="bool" value="True" />
<param name="fps" type="double" value="15.0"/>
<param name="frame_id" type="string" value="stereo_optical_frame"/>
+ <param name="num_disp" value="128"/>
</group>
<node machine="four" pkg="dcam" type="stereodcam" respawn="false"/>
Modified: pkg/trunk/vision/stereo_view/stereo_bag.launch
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_bag.launch 2009-05-27 00:12:25 UTC (rev 16116)
+++ pkg/trunk/vision/stereo_view/stereo_bag.launch 2009-05-27 00:23:28 UTC (rev 16117)
@@ -9,5 +9,6 @@
<node name="stereoproc" pkg="stereo_image_proc" type="stereoproc" respawn="false" />
<node name="stereo_view" pkg="stereo_view" type="stereo_view_pixel_info" respawn="false" output="screen"/>
+ <node name="stereodcam_params" pkg="dcam" type="stereodcam_params.py" respawn="false" output="screen"/>
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
