Classes | |
struct | Stats |
Public Member Functions | |
HandleDetector () | |
void | serviceThread () |
bool | spin () |
~HandleDetector () | |
Public Attributes | |
CvHaarClassifierCascade * | cascade |
sensor_msgs::PointCloudConstPtr | cloud_ |
ros::Subscriber | cloud_sub_ |
boost::condition_variable | data_cv_ |
boost::mutex | data_lock_ |
sensor_msgs::CvBridge | dbridge_ |
ros::ServiceServer | detect_service_ |
IplImage * | disp_ |
IplImage * | disp_clone_ |
stereo_msgs::DisparityImageConstPtr | dispimg_ |
ros::Subscriber | dispimg_sub_ |
bool | display_ |
int | frames_no_ |
bool | got_images_ |
sensor_msgs::CvBridge | lbridge_ |
IplImage * | left_ |
ros::Subscriber | left_image_sub_ |
sensor_msgs::ImageConstPtr | limage_ |
ros::Publisher | marker_pub_ |
double | max_height_ |
double | min_height_ |
ros::NodeHandle | nh_ |
ros::NodeHandle | nh_tilde_ |
ros::ServiceServer | preempt_service_ |
bool | preempted_ |
sensor_msgs::CvBridge | rbridge_ |
sensor_msgs::CameraInfoConstPtr | rcinfo_ |
IplImage * | right_ |
sensor_msgs::ImageConstPtr | rimage_ |
ros::CallbackQueue | service_queue_ |
boost::thread | service_thread_ |
ros::Time | start_image_wait_ |
CvMemStorage * | storage |
TopicSynchronizer | sync_ |
tf::TransformListener | tf_ |
double | timeout_ |
Private Member Functions | |
void | applyPositionPrior () |
Filters cloud point, retains only regions that could contain a handle. | |
bool | belongsToCluster (pair< float, float > center, pair< float, float > p) |
Checks if a point should belong to a cluster. | |
void | cloudCallback (const sensor_msgs::PointCloud::ConstPtr &point_cloud) |
bool | decideHandlePosition (vector< CvRect > &handle_rect, geometry_msgs::PointStamped &handle) |
Decide the handle position from several detections across multiple frames. | |
bool | detectHandleSrv (door_handle_detector::DoorsDetector::Request &req, door_handle_detector::DoorsDetector::Response &resp) |
Service call to detect doors. | |
double | disparitySTD (IplImage *Id, const CvRect &R, double &meanDisparity, double minDisparity=0.5) |
void | dispimgCallback (const stereo_msgs::DisparityImage::ConstPtr &dinfo) |
CvScalar | estimatePlaneLS (sensor_msgs::PointCloud points) |
Computes a least-squares estimate of a plane. | |
sensor_msgs::PointCloud | filterPointCloud (const sensor_msgs::PointCloud pc, const CvRect &rect) |
Filters a cloud point, retains only points coming from a specific region in the disparity image. | |
void | findHandleCascade (vector< CvRect > &handle_rect) |
Start handle detection. | |
CvPoint | getDisparityCenter (CvRect &r) |
bool | handlePossibleHere (CvRect &r) |
Determine if it's possible for handle to be in a specific ROI. | |
void | leftImageCallback (const sensor_msgs::Image::ConstPtr &image) |
void | loadClassifier (string cascade_classifier) |
void | pointCloudStatistics (const sensor_msgs::PointCloud &pc, Stats &x_stats, Stats &y_stats, Stats &z_stats) |
bool | preempt (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) |
pair< float, float > | rectCenter (const CvRect &r) |
bool | runHandleDetector (geometry_msgs::PointStamped &handle) |
Runs the handle detector. | |
void | showHandleMarker (geometry_msgs::PointStamped p) |
Publishes a visualization marker for a point. | |
void | subscribeStereoData () |
void | syncCallback () |
void | tryShrinkROI (CvRect &r) |
void | unsubscribeStereoData () |
Definition at line 105 of file handle_detector_vision.cpp.
HandleDetector::HandleDetector | ( | ) | [inline] |
Definition at line 173 of file handle_detector_vision.cpp.
HandleDetector::~HandleDetector | ( | ) | [inline] |
Definition at line 214 of file handle_detector_vision.cpp.
void HandleDetector::applyPositionPrior | ( | ) | [inline, private] |
Filters cloud point, retains only regions that could contain a handle.
Definition at line 999 of file handle_detector_vision.cpp.
bool HandleDetector::belongsToCluster | ( | pair< float, float > | center, | |
pair< float, float > | p | |||
) | [inline, private] |
Checks if a point should belong to a cluster.
center | ||
p |
Definition at line 708 of file handle_detector_vision.cpp.
void HandleDetector::cloudCallback | ( | const sensor_msgs::PointCloud::ConstPtr & | point_cloud | ) | [inline, private] |
Definition at line 307 of file handle_detector_vision.cpp.
bool HandleDetector::decideHandlePosition | ( | vector< CvRect > & | handle_rect, | |
geometry_msgs::PointStamped & | handle | |||
) | [inline, private] |
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.
handle_rect | The handle bounding boxes | |
handle | Point indicating the real-world handle position |
Definition at line 739 of file handle_detector_vision.cpp.
bool HandleDetector::detectHandleSrv | ( | door_handle_detector::DoorsDetector::Request & | req, | |
door_handle_detector::DoorsDetector::Response & | resp | |||
) | [inline, private] |
Service call to detect doors.
Definition at line 894 of file handle_detector_vision.cpp.
double HandleDetector::disparitySTD | ( | IplImage * | Id, | |
const CvRect & | R, | |||
double & | meanDisparity, | |||
double | minDisparity = 0.5 | |||
) | [inline, private] |
Definition at line 322 of file handle_detector_vision.cpp.
void HandleDetector::dispimgCallback | ( | const stereo_msgs::DisparityImage::ConstPtr & | dinfo | ) | [inline, private] |
Definition at line 298 of file handle_detector_vision.cpp.
CvScalar HandleDetector::estimatePlaneLS | ( | sensor_msgs::PointCloud | points | ) | [inline, private] |
Computes a least-squares estimate of a plane.
points | The point cloud |
Definition at line 957 of file handle_detector_vision.cpp.
sensor_msgs::PointCloud HandleDetector::filterPointCloud | ( | const sensor_msgs::PointCloud | pc, | |
const CvRect & | rect | |||
) | [inline, private] |
Filters a cloud point, retains only points coming from a specific region in the disparity image.
rect | Region in disparity image |
Definition at line 429 of file handle_detector_vision.cpp.
void HandleDetector::findHandleCascade | ( | vector< CvRect > & | handle_rect | ) | [inline, private] |
Start handle detection.
Definition at line 659 of file handle_detector_vision.cpp.
CvPoint HandleDetector::getDisparityCenter | ( | CvRect & | r | ) | [inline, private] |
Definition at line 494 of file handle_detector_vision.cpp.
bool HandleDetector::handlePossibleHere | ( | CvRect & | r | ) | [inline, private] |
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.
r |
Definition at line 568 of file handle_detector_vision.cpp.
void HandleDetector::leftImageCallback | ( | const sensor_msgs::Image::ConstPtr & | image | ) | [inline, private] |
Definition at line 286 of file handle_detector_vision.cpp.
void HandleDetector::loadClassifier | ( | string | cascade_classifier | ) | [inline, private] |
Definition at line 225 of file handle_detector_vision.cpp.
void HandleDetector::pointCloudStatistics | ( | const sensor_msgs::PointCloud & | pc, | |
Stats & | x_stats, | |||
Stats & | y_stats, | |||
Stats & | z_stats | |||
) | [inline, private] |
Definition at line 374 of file handle_detector_vision.cpp.
bool HandleDetector::preempt | ( | std_srvs::Empty::Request & | req, | |
std_srvs::Empty::Response & | resp | |||
) | [inline, private] |
Definition at line 942 of file handle_detector_vision.cpp.
pair<float,float> HandleDetector::rectCenter | ( | const CvRect & | r | ) | [inline, private] |
bool HandleDetector::runHandleDetector | ( | geometry_msgs::PointStamped & | handle | ) | [inline, private] |
Runs the handle detector.
handle | Position of detected handle |
Definition at line 842 of file handle_detector_vision.cpp.
void HandleDetector::serviceThread | ( | ) | [inline] |
Definition at line 1080 of file handle_detector_vision.cpp.
void HandleDetector::showHandleMarker | ( | geometry_msgs::PointStamped | p | ) | [inline, private] |
Publishes a visualization marker for a point.
p |
Definition at line 473 of file handle_detector_vision.cpp.
bool HandleDetector::spin | ( | ) | [inline] |
Needed for OpenCV event loop, to show images
Definition at line 1047 of file handle_detector_vision.cpp.
void HandleDetector::subscribeStereoData | ( | ) | [inline, private] |
Definition at line 237 of file handle_detector_vision.cpp.
void HandleDetector::syncCallback | ( | ) | [inline, private] |
Definition at line 261 of file handle_detector_vision.cpp.
void HandleDetector::tryShrinkROI | ( | CvRect & | r | ) | [inline, private] |
Definition at line 518 of file handle_detector_vision.cpp.
void HandleDetector::unsubscribeStereoData | ( | ) | [inline, private] |
Definition at line 248 of file handle_detector_vision.cpp.
CvHaarClassifierCascade* HandleDetector::cascade |
Definition at line 170 of file handle_detector_vision.cpp.
sensor_msgs::PointCloudConstPtr HandleDetector::cloud_ |
Definition at line 121 of file handle_detector_vision.cpp.
ros::Subscriber HandleDetector::cloud_sub_ |
Definition at line 134 of file handle_detector_vision.cpp.
boost::condition_variable HandleDetector::data_cv_ |
Definition at line 168 of file handle_detector_vision.cpp.
boost::mutex HandleDetector::data_lock_ |
Definition at line 167 of file handle_detector_vision.cpp.
sensor_msgs::CvBridge HandleDetector::dbridge_ |
Definition at line 118 of file handle_detector_vision.cpp.
ros::ServiceServer HandleDetector::detect_service_ |
Definition at line 138 of file handle_detector_vision.cpp.
IplImage* HandleDetector::disp_ |
Definition at line 125 of file handle_detector_vision.cpp.
IplImage* HandleDetector::disp_clone_ |
Definition at line 126 of file handle_detector_vision.cpp.
stereo_msgs::DisparityImageConstPtr HandleDetector::dispimg_ |
Definition at line 113 of file handle_detector_vision.cpp.
ros::Subscriber HandleDetector::dispimg_sub_ |
Definition at line 135 of file handle_detector_vision.cpp.
Definition at line 155 of file handle_detector_vision.cpp.
Definition at line 153 of file handle_detector_vision.cpp.
Definition at line 158 of file handle_detector_vision.cpp.
sensor_msgs::CvBridge HandleDetector::lbridge_ |
Definition at line 116 of file handle_detector_vision.cpp.
IplImage* HandleDetector::left_ |
Definition at line 123 of file handle_detector_vision.cpp.
ros::Subscriber HandleDetector::left_image_sub_ |
Definition at line 130 of file handle_detector_vision.cpp.
sensor_msgs::ImageConstPtr HandleDetector::limage_ |
Definition at line 111 of file handle_detector_vision.cpp.
ros::Publisher HandleDetector::marker_pub_ |
Definition at line 141 of file handle_detector_vision.cpp.
double HandleDetector::max_height_ |
Definition at line 151 of file handle_detector_vision.cpp.
double HandleDetector::min_height_ |
Definition at line 149 of file handle_detector_vision.cpp.
ros::NodeHandle HandleDetector::nh_ |
Definition at line 109 of file handle_detector_vision.cpp.
ros::NodeHandle HandleDetector::nh_tilde_ |
Definition at line 109 of file handle_detector_vision.cpp.
ros::ServiceServer HandleDetector::preempt_service_ |
Definition at line 139 of file handle_detector_vision.cpp.
Definition at line 157 of file handle_detector_vision.cpp.
sensor_msgs::CvBridge HandleDetector::rbridge_ |
Definition at line 117 of file handle_detector_vision.cpp.
sensor_msgs::CameraInfoConstPtr HandleDetector::rcinfo_ |
Definition at line 114 of file handle_detector_vision.cpp.
IplImage* HandleDetector::right_ |
Definition at line 124 of file handle_detector_vision.cpp.
sensor_msgs::ImageConstPtr HandleDetector::rimage_ |
Definition at line 112 of file handle_detector_vision.cpp.
ros::CallbackQueue HandleDetector::service_queue_ |
Definition at line 165 of file handle_detector_vision.cpp.
boost::thread HandleDetector::service_thread_ |
Definition at line 166 of file handle_detector_vision.cpp.
ros::Time HandleDetector::start_image_wait_ |
Definition at line 162 of file handle_detector_vision.cpp.
CvMemStorage* HandleDetector::storage |
Definition at line 171 of file handle_detector_vision.cpp.
Definition at line 143 of file handle_detector_vision.cpp.
tf::TransformListener HandleDetector::tf_ |
Definition at line 145 of file handle_detector_vision.cpp.
double HandleDetector::timeout_ |
Definition at line 161 of file handle_detector_vision.cpp.