The CliffDetector class detect cliff based on depth image. More...

#include <cliff_detector.h>

Public Member Functions
	CliffDetector ()
void	detectCliff (const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
	detectCliff detects descending stairs based on the information in a depth image
bool	getPublishDepthEnable () const
	getPublishDepthEnable
float	getSensorMountHeight ()
	getSensorMountHeight gets sensor mount height
float	getSensorTiltAngle ()
	getSensorTiltAngle gets sensor tilt angle in degrees
void	setBlockPointsThresh (const int thresh)
	setBlockPointsThresh sets threshold value of pixels in block to set block as stairs
void	setBlockSize (const int size)
	setBlockSize sets size of square block (subimage) used in cliff detector
void	setCamModelUpdate (const bool u)
	Sets the number of image rows to use in data processing.
void	setDepthImgStepCol (const int step)
	setDepthImgStepCol
void	setDepthImgStepRow (const int step)
	setDepthImgStepRow
void	setGroundMargin (const float margin)
	setGroundMargin sets the floor margin (in meters)
void	setParametersConfigurated (const bool u)
	setParametersConfigurated
void	setPublishDepthEnable (const bool enable)
	setPublishDepthEnable
void	setRangeLimits (const float rmin, const float rmax)
	setRangeLimits sets the minimum and maximum range of depth value from RGBD sensor.
void	setSensorMountHeight (const float height)
	setSensorMountHeight sets the height of sensor mount (in meters) from ground
void	setSensorTiltAngle (const float angle)
	setSensorTiltAngle sets the sensor tilt angle (in degrees)
void	setUsedDepthHeight (const unsigned int height)
	setUsedDepthHeight
	~CliffDetector ()
Public Attributes
sensor_msgs::ImageConstPtr	depth_msg_to_pub_
	Store points which contain stairs.
sensor_msgs::Image	new_depth_msg_
depth_nav_msgs::Point32List	stairs_points_msg_
Private Member Functions
double	angleBetweenRays (const cv::Point3d &ray1, const cv::Point3d &ray2) const
void	calcDeltaAngleForImgRows (double vertical_fov)
	calcDeltaAngleForImgRows
void	calcGroundDistancesForImgRows (double vertical_fov)
	Calculates distances to ground for every row of depth image.
void	calcTiltCompensationFactorsForImgRows (double vertical_fov)
	calcTiltCompensationFactorsForImgRows calculate factors used in tilt compensation
void	fieldOfView (double &min, double &max, double x1, double y1, double xc, double yc, double x2, double y2)
void	findCliffInDepthImage (const sensor_msgs::ImageConstPtr &depth_msg)
double	lengthOfVector (const cv::Point3d &vec) const
	lengthOfVector calculates length of 3D vector.
Private Attributes
unsigned int	block_points_thresh_
	Threshold value of points in block to admit stairs.
unsigned int	block_size_
	Square block (subimage) size (px).
bool	cam_model_update_
	Determines if continuously cam model update required.
image_geometry::PinholeCameraModel	camera_model_
	Class for managing sensor_msgs/CameraInfo messages.
std::vector< double >	delta_row_
unsigned int	depth_image_step_col_
	Columns step in depth processing (px).
unsigned int	depth_image_step_row_
	Rows step in depth processing (px).
bool	depth_sensor_params_update
std::vector< unsigned int >	dist_to_ground_
	Calculated distances to ground for every row of depth image in mm.
float	ground_margin_
	Margin for ground points feature detector (m)
std::string	outputFrameId_
	Output frame_id for laserscan.
bool	publish_depth_enable_
	Determines if depth should be republished.
float	range_max_
	Stores the current maximum range to use.
float	range_min_
	Stores the current minimum range to use.
float	sensor_mount_height_
	Height of sensor mount from ground.
float	sensor_tilt_angle_
	Sensor tilt angle (degrees)
std::vector< double >	tilt_compensation_factor_
	Calculated sensor tilt compensation factors.
unsigned int	used_depth_height_
	Used depth height from img bottom (px)

Detailed Description

The CliffDetector class detect cliff based on depth image.

Definition at line 66 of file cliff_detector.h.

Constructor & Destructor Documentation

CliffDetector::CliffDetector ( )

Definition at line 44 of file cliff_detector.cpp.

cliff_detector::CliffDetector::~CliffDetector ( ) [inline]

Definition at line 71 of file cliff_detector.h.

Member Function Documentation

double CliffDetector::angleBetweenRays	(	const cv::Point3d &	ray1,
		const cv::Point3d &	ray2
	)		const `[private]`

Computes the angle between two cv::Point3d

Computes the angle of two cv::Point3d assumed to be vectors starting at the origin (0,0,0). Uses the following equation: angle = arccos(a*b/(|a||b|)) where a = ray1 and b = ray2.

Parameters:

ray1	The first ray
ray2	The second ray

Returns:: The angle between the two rays (in radians)

Definition at line 222 of file cliff_detector.cpp.

void CliffDetector::calcDeltaAngleForImgRows ( double vertical_fov ) [private]

calcDeltaAngleForImgRows

Parameters:

vertical_fov

Definition at line 253 of file cliff_detector.cpp.

void CliffDetector::calcGroundDistancesForImgRows ( double vertical_fov ) [private]

Calculates distances to ground for every row of depth image.

Calculates distances to ground for rows of depth image based on known height of sensor mount and tilt angle. It assume that sensor height and tilt angle in relative to ground is constant.

Calculated values are placed in vector dist_to_ground_.

Definition at line 265 of file cliff_detector.cpp.

void CliffDetector::calcTiltCompensationFactorsForImgRows ( double vertical_fov ) [private]

calcTiltCompensationFactorsForImgRows calculate factors used in tilt compensation

Parameters:

vertical_fov

Definition at line 292 of file cliff_detector.cpp.

void CliffDetector::detectCliff	(	const sensor_msgs::ImageConstPtr &	depth_msg,
		const sensor_msgs::CameraInfoConstPtr &	info_msg
	)

detectCliff detects descending stairs based on the information in a depth image

This function detects descending stairs based on the information in the depth encoded image (UInt16 encoding). To do this, it requires the synchornized Image/CameraInfo pair associated with the image.

Parameters:

depth_msg	UInt16 encoded depth image.
info_msg	CameraInfo associated with depth_msg

Definition at line 47 of file cliff_detector.cpp.

void CliffDetector::fieldOfView	(	double &	min,
		double &	max,
		double	x1,
		double	y1,
		double	xc,
		double	yc,
		double	x2,
		double	y2
	)		`[private]`

Calculate vertical angle_min and angle_max by measuring angles between the top ray, bottom ray, and optical center ray

Parameters:

camModel	The image_geometry camera model for this image.
min_angle	The minimum vertical angle
max_angle	The maximum vertical angle

Definition at line 230 of file cliff_detector.cpp.

void CliffDetector::findCliffInDepthImage ( const sensor_msgs::ImageConstPtr & depth_msg ) [private]

Find 2D points relative to robots where stairs are detected

This uses a method

Parameters:

depth_msg The UInt16 encoded depth message.

Definition at line 312 of file cliff_detector.cpp.

bool cliff_detector::CliffDetector::getPublishDepthEnable ( ) const [inline]

getPublishDepthEnable

Returns:

Definition at line 123 of file cliff_detector.h.

float cliff_detector::CliffDetector::getSensorMountHeight ( ) [inline]

getSensorMountHeight gets sensor mount height

Returns:: Return sensor mount height in meters

Definition at line 102 of file cliff_detector.h.

float cliff_detector::CliffDetector::getSensorTiltAngle ( ) [inline]

getSensorTiltAngle gets sensor tilt angle in degrees

Returns:: Return sensor tilt angle in degrees

Definition at line 113 of file cliff_detector.h.

double CliffDetector::lengthOfVector ( const cv::Point3d & vec ) const [private]

lengthOfVector calculates length of 3D vector.

Method calculates the length of 3D vector assumed to be a vector with start at the (0,0,0).

Parameters:

vec	Vector 3D which lenght will be calculated.

Returns:: Returns the length of 3D vector.

Definition at line 216 of file cliff_detector.cpp.

void CliffDetector::setBlockPointsThresh ( const int thresh )

setBlockPointsThresh sets threshold value of pixels in block to set block as stairs

Parameters:

thresh Value of threshold in pixels.

Definition at line 166 of file cliff_detector.cpp.

void CliffDetector::setBlockSize ( const int size )

setBlockSize sets size of square block (subimage) used in cliff detector

Parameters:

size	Size of square block in pixels

Definition at line 154 of file cliff_detector.cpp.

void cliff_detector::CliffDetector::setCamModelUpdate ( const bool u ) [inline]

Sets the number of image rows to use in data processing.

scan_height is the number of rows (pixels) to use in the output.

Parameters:

scan_height	Number of pixels centered around the center of the image to data processing Set value which determine if sensor params needs continously update setCamModelUpdate
u

Definition at line 140 of file cliff_detector.h.

void CliffDetector::setDepthImgStepCol ( const int step )

setDepthImgStepCol

Parameters:

step

Definition at line 190 of file cliff_detector.cpp.

void CliffDetector::setDepthImgStepRow ( const int step )

setDepthImgStepRow

Parameters:

step

Definition at line 178 of file cliff_detector.cpp.

void CliffDetector::setGroundMargin ( const float margin )