#include <depth_sensor_pose.h>

Public Member Functions
	DepthSensorPose ()
void	estimateParams (const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
bool	getPublishDepthEnable () const
	getPublishDepthEnable
float	getSensorMountHeight () const
float	getSensorTiltAngle () const
void	setCamModelUpdate (const bool u)
	setCamModelUpdate
void	setDepthImgStepCol (const int step)
	setDepthImgStepCol
void	setDepthImgStepRow (const int step)
	setDepthImgStepRow
void	setGroundMaxPoints (const unsigned int u)
void	setPublishDepthEnable (const bool enable)
	setPublishDepthEnable
void	setRangeLimits (const float rmin, const float rmax)
void	setRansacDistanceThresh (const float u)
void	setRansacMaxIter (const unsigned int u)
void	setReconfParamsUpdated (bool updated)
	setReconfParamsUpdated
void	setSensorMountHeightMax (const float height)
	setSensorMountHeight sets the height of sensor mount (in meters) from ground
void	setSensorMountHeightMin (const float height)
	setSensorMountHeight sets the height of sensor mount (in meters) from ground
void	setSensorTiltAngleMax (const float angle)
	setSensorTiltAngle sets the sensor tilt angle (in degrees)
void	setSensorTiltAngleMin (const float angle)
	setSensorTiltAngle sets the sensor tilt angle (in degrees)
void	setUsedDepthHeight (const unsigned int height)
	setUsedDepthHeight
	~DepthSensorPose ()
Public Attributes
sensor_msgs::Image	new_depth_msg_
Private Member Functions
double	angleBetweenRays (const cv::Point3d &ray1, const cv::Point3d &ray2) const
void	calcDeltaAngleForImgRows (double vertical_fov)
	calcDeltaAngleForImgRows
void	calcGroundDistancesForImgRows (double mount_height, double tilt_angle, std::vector< unsigned int > &distances)
	Calculates distances to ground for every row of depth image.
void	fieldOfView (double &min, double &max, double x1, double y1, double xc, double yc, double x2, double y2)
void	getGroundPoints (const sensor_msgs::ImageConstPtr &depth_msg, pcl::PointCloud< pcl::PointXYZ >::Ptr &points)
double	lengthOfVector (const cv::Point3d &ray) const
void	sensorPoseCalibration (const sensor_msgs::ImageConstPtr &depth_msg, double &tilt, double &height)
Private Attributes
bool	cam_model_update_
	Determines if continuously cam model update is 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).
std::vector< unsigned int >	dist_to_ground_max_
std::vector< unsigned int >	dist_to_ground_min_
float	groundDistTolerance_
unsigned int	max_ground_points_
float	mount_height_est_
float	mount_height_max_
	Max height of sensor mount from ground.
float	mount_height_min_
	Min height of sensor mount from ground.
bool	publish_depth_enable_
	Determines if modified depth image should be published.
float	range_max_
	Stores the current maximum range to use.
float	range_min_
	Stores the current minimum range to use.
float	ransacDistanceThresh_
unsigned int	ransacMaxIter_
bool	reconf_serv_params_updated_
float	tilt_angle_est_
float	tilt_angle_max_
	Max angle of sensor tilt in degrees.
float	tilt_angle_min_
	Min angle of sensor tilt in degrees.
unsigned int	used_depth_height_
	Used depth height from img bottom (px)

Detailed Description

Definition at line 75 of file depth_sensor_pose.h.

Constructor & Destructor Documentation

DepthSensorPose::DepthSensorPose ( )

Definition at line 47 of file depth_sensor_pose.cpp.

DepthSensorPose::~DepthSensorPose ( )

Definition at line 52 of file depth_sensor_pose.cpp.

Member Function Documentation

double DepthSensorPose::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 255 of file depth_sensor_pose.cpp.

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

calcDeltaAngleForImgRows

Parameters:

vertical_fov

Definition at line 285 of file depth_sensor_pose.cpp.

void DepthSensorPose::calcGroundDistancesForImgRows	(	double	mount_height,
		double	tilt_angle,
		std::vector< unsigned int > &	distances
	)		`[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 297 of file depth_sensor_pose.cpp.

void DepthSensorPose::estimateParams	(	const sensor_msgs::ImageConstPtr &	depth_msg,
		const sensor_msgs::CameraInfoConstPtr &	info_msg
	)

Converts the information in a depth image (sensor_msgs::Image) to a sensor_msgs::LaserScan.

This function converts the information in the depth encoded image (UInt16) into a sensor_msgs::LaserScan as accurately as possible. To do this, it requires the synchornized Image/CameraInfo pair associated with the image.

Parameters:

depth_msg	UInt16 or Float32 encoded depth image.
info_msg	CameraInfo associated with depth_msg

Returns:: sensor_msgs::LaserScanPtr for the center row(s) of the depth image.

Definition at line 57 of file depth_sensor_pose.cpp.

void DepthSensorPose::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 263 of file depth_sensor_pose.cpp.

void DepthSensorPose::getGroundPoints	(	const sensor_msgs::ImageConstPtr &	depth_msg,
		pcl::PointCloud< pcl::PointXYZ >::Ptr &	points
	)		`[private]`

Definition at line 322 of file depth_sensor_pose.cpp.

bool depth_sensor_pose::DepthSensorPose::getPublishDepthEnable ( ) const [inline]

getPublishDepthEnable

Returns:

Definition at line 139 of file depth_sensor_pose.h.

float depth_sensor_pose::DepthSensorPose::getSensorMountHeight ( ) const [inline]

Definition at line 183 of file depth_sensor_pose.h.

float depth_sensor_pose::DepthSensorPose::getSensorTiltAngle ( ) const [inline]

Definition at line 179 of file depth_sensor_pose.h.

double DepthSensorPose::lengthOfVector ( const cv::Point3d & ray ) const [private]

Computes euclidean length of a cv::Point3d (as a ray from origin)

This function computes the length of a cv::Point3d assumed to be a vector starting at the origin (0,0,0).

Parameters:

ray	The ray for which the magnitude is desired.

Returns:: Returns the magnitude of the ray.

Definition at line 249 of file depth_sensor_pose.cpp.

void DepthSensorPose::sensorPoseCalibration	(	const sensor_msgs::ImageConstPtr &	depth_msg,
		double &	tilt,
		double &	height
	)		`[private]`

Definition at line 388 of file depth_sensor_pose.cpp.

void depth_sensor_pose::DepthSensorPose::setCamModelUpdate ( const bool u ) [inline]

setCamModelUpdate

Parameters:

u

Definition at line 144 of file depth_sensor_pose.h.

void DepthSensorPose::setDepthImgStepCol ( const int step )

setDepthImgStepCol

Parameters:

step

Definition at line 234 of file depth_sensor_pose.cpp.

void DepthSensorPose::setDepthImgStepRow ( const int step )

setDepthImgStepRow

Parameters:

step

Definition at line 222 of file depth_sensor_pose.cpp.

void depth_sensor_pose::DepthSensorPose::setGroundMaxPoints ( const unsigned int u ) [inline]

Definition at line 173 of file depth_sensor_pose.h.

void depth_sensor_pose::DepthSensorPose::setPublishDepthEnable ( const bool enable ) [inline]

setPublishDepthEnable

Parameters:

enable

Definition at line 134 of file depth_sensor_pose.h.

void DepthSensorPose::setRangeLimits	(	const float	rmin,
		const float	rmax
	)

Sets the minimum and maximum range for the sensor_msgs::LaserScan.

rangeMin is used to determine how close of a value to allow through when multiple radii correspond to the same angular increment. rangeMax is used to set the output message.

Parameters:

rangeMin	Minimum range to assign points to the laserscan, also minimum range to use points in the output scan.
rangeMax	Maximum range to use points in the output scan.

Definition at line 143 of file depth_sensor_pose.cpp.

void depth_sensor_pose::DepthSensorPose::setRansacDistanceThresh ( const float u ) [inline]

Definition at line 171 of file depth_sensor_pose.h.

void depth_sensor_pose::DepthSensorPose::setRansacMaxIter ( const unsigned int u ) [inline]

Definition at line 169 of file depth_sensor_pose.h.

void depth_sensor_pose::DepthSensorPose::setReconfParamsUpdated ( bool updated ) [inline]

setReconfParamsUpdated

Parameters:

updated

Definition at line 164 of file depth_sensor_pose.h.

void DepthSensorPose::setSensorMountHeightMax ( const float height )

setSensorMountHeight sets the height of sensor mount (in meters) from ground