#include <depth_sensor_pose.h>
|
double | angleBetweenRays (const cv::Point3d &ray1, const cv::Point3d &ray2) const |
|
void | calcDeltaAngleForImgRows (double vertical_fov) |
|
void | calcGroundDistancesForImgRows (double mount_height, double tilt_angle, std::vector< double > &distances) |
| Calculates distances to ground for every row of depth image. More...
|
|
void | fieldOfView (double &min, double &max, double x1, double y1, double xc, double yc, double x2, double y2) |
|
template<typename T > |
void | getGroundPoints (const sensor_msgs::ImageConstPtr &depth_msg, pcl::PointCloud< pcl::PointXYZ >::Ptr &points, std::list< std::pair< unsigned, unsigned >> &points_indices) |
|
double | lengthOfVector (const cv::Point3d &vec) const |
|
sensor_msgs::ImagePtr | prepareDbgImage (const sensor_msgs::ImageConstPtr &depth_msg, const std::list< std::pair< unsigned, unsigned >> &ground_points_indices) |
|
void | sensorPoseCalibration (const sensor_msgs::ImageConstPtr &depth_msg, double &tilt_angle, double &height) |
|
Definition at line 31 of file depth_sensor_pose.h.
depth_sensor_pose::DepthSensorPose::DepthSensorPose |
( |
| ) |
|
|
default |
depth_sensor_pose::DepthSensorPose::~DepthSensorPose |
( |
| ) |
|
|
default |
depth_sensor_pose::DepthSensorPose::DepthSensorPose |
( |
const DepthSensorPose & |
| ) |
|
|
delete |
double depth_sensor_pose::DepthSensorPose::angleBetweenRays |
( |
const cv::Point3d & |
ray1, |
|
|
const cv::Point3d & |
ray2 |
|
) |
| const |
|
protected |
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 129 of file depth_sensor_pose.cpp.
void depth_sensor_pose::DepthSensorPose::calcDeltaAngleForImgRows |
( |
double |
vertical_fov | ) |
|
|
protected |
void depth_sensor_pose::DepthSensorPose::calcGroundDistancesForImgRows |
( |
double |
mount_height, |
|
|
double |
tilt_angle, |
|
|
std::vector< double > & |
distances |
|
) |
| |
|
protected |
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 166 of file depth_sensor_pose.cpp.
void depth_sensor_pose::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 7 of file depth_sensor_pose.cpp.
void depth_sensor_pose::DepthSensorPose::fieldOfView |
( |
double & |
min, |
|
|
double & |
max, |
|
|
double |
x1, |
|
|
double |
y1, |
|
|
double |
xc, |
|
|
double |
yc, |
|
|
double |
x2, |
|
|
double |
y2 |
|
) |
| |
|
protected |
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 136 of file depth_sensor_pose.cpp.
sensor_msgs::ImageConstPtr depth_sensor_pose::DepthSensorPose::getDbgImage |
( |
| ) |
const |
template<typename T >
void depth_sensor_pose::DepthSensorPose::getGroundPoints |
( |
const sensor_msgs::ImageConstPtr & |
depth_msg, |
|
|
pcl::PointCloud< pcl::PointXYZ >::Ptr & |
points, |
|
|
std::list< std::pair< unsigned, unsigned >> & |
points_indices |
|
) |
| |
|
protected |
bool depth_sensor_pose::DepthSensorPose::getPublishDepthEnable |
( |
| ) |
const |
|
inline |
float depth_sensor_pose::DepthSensorPose::getSensorMountHeight |
( |
| ) |
const |
|
inline |
float depth_sensor_pose::DepthSensorPose::getSensorTiltAngle |
( |
| ) |
const |
|
inline |
double depth_sensor_pose::DepthSensorPose::lengthOfVector |
( |
const cv::Point3d & |
vec | ) |
const |
|
protected |
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 125 of file depth_sensor_pose.cpp.
sensor_msgs::ImagePtr depth_sensor_pose::DepthSensorPose::prepareDbgImage |
( |
const sensor_msgs::ImageConstPtr & |
depth_msg, |
|
|
const std::list< std::pair< unsigned, unsigned >> & |
ground_points_indices |
|
) |
| |
|
protected |
void depth_sensor_pose::DepthSensorPose::sensorPoseCalibration |
( |
const sensor_msgs::ImageConstPtr & |
depth_msg, |
|
|
double & |
tilt_angle, |
|
|
double & |
height |
|
) |
| |
|
protected |
void depth_sensor_pose::DepthSensorPose::setCamModelUpdate |
( |
const bool |
u | ) |
|
|
inline |
void depth_sensor_pose::DepthSensorPose::setDepthImgStepCol |
( |
const int |
step | ) |
|
void depth_sensor_pose::DepthSensorPose::setDepthImgStepRow |
( |
const int |
step | ) |
|
void depth_sensor_pose::DepthSensorPose::setGroundMaxPoints |
( |
const unsigned |
u | ) |
|
|
inline |
void depth_sensor_pose::DepthSensorPose::setPublishDepthEnable |
( |
const bool |
enable | ) |
|
|
inline |
void depth_sensor_pose::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 34 of file depth_sensor_pose.cpp.
void depth_sensor_pose::DepthSensorPose::setRansacDistanceThresh |
( |
const float |
u | ) |
|
|
inline |
void depth_sensor_pose::DepthSensorPose::setRansacMaxIter |
( |
const unsigned |
u | ) |
|
|
inline |
void depth_sensor_pose::DepthSensorPose::setReconfParamsUpdated |
( |
bool |
updated | ) |
|
|
inline |
void depth_sensor_pose::DepthSensorPose::setSensorMountHeightMax |
( |
const float |
height | ) |
|
setSensorMountHeight sets the height of sensor mount (in meters) from ground
- Parameters
-
height | Value of sensor mount height (in meters). |
Definition at line 61 of file depth_sensor_pose.cpp.
void depth_sensor_pose::DepthSensorPose::setSensorMountHeightMin |
( |
const float |
height | ) |
|
setSensorMountHeight sets the height of sensor mount (in meters) from ground
- Parameters
-
height | Value of sensor mount height (in meters). |
Definition at line 51 of file depth_sensor_pose.cpp.
void depth_sensor_pose::DepthSensorPose::setSensorTiltAngleMax |
( |
const float |
angle | ) |
|
setSensorTiltAngle sets the sensor tilt angle (in degrees)
- Parameters
-
Definition at line 81 of file depth_sensor_pose.cpp.
void depth_sensor_pose::DepthSensorPose::setSensorTiltAngleMin |
( |
const float |
angle | ) |
|
setSensorTiltAngle sets the sensor tilt angle (in degrees)
- Parameters
-
Definition at line 71 of file depth_sensor_pose.cpp.
void depth_sensor_pose::DepthSensorPose::setUsedDepthHeight |
( |
const unsigned |
height | ) |
|
bool depth_sensor_pose::DepthSensorPose::cam_model_update_ {false} |
|
private |
sensor_msgs::ImagePtr depth_sensor_pose::DepthSensorPose::dbg_image_ |
|
private |
std::vector<double> depth_sensor_pose::DepthSensorPose::delta_row_ |
|
private |
unsigned depth_sensor_pose::DepthSensorPose::depth_image_step_col_ {0} |
|
private |
unsigned depth_sensor_pose::DepthSensorPose::depth_image_step_row_ {0} |
|
private |
std::vector<double> depth_sensor_pose::DepthSensorPose::dist_to_ground_max_ |
|
private |
std::vector<double> depth_sensor_pose::DepthSensorPose::dist_to_ground_min_ |
|
private |
float depth_sensor_pose::DepthSensorPose::groundDistTolerance_ {0} |
|
private |
unsigned depth_sensor_pose::DepthSensorPose::max_ground_points_ {0} |
|
private |
double depth_sensor_pose::DepthSensorPose::mount_height_est_ {0} |
|
private |
float depth_sensor_pose::DepthSensorPose::mount_height_max_ {0} |
|
private |
float depth_sensor_pose::DepthSensorPose::mount_height_min_ {0} |
|
private |
bool depth_sensor_pose::DepthSensorPose::publish_depth_enable_ {false} |
|
private |
float depth_sensor_pose::DepthSensorPose::range_max_ {0} |
|
private |
float depth_sensor_pose::DepthSensorPose::range_min_ {0} |
|
private |
float depth_sensor_pose::DepthSensorPose::ransacDistanceThresh_ {0} |
|
private |
unsigned depth_sensor_pose::DepthSensorPose::ransacMaxIter_ {0} |
|
private |
bool depth_sensor_pose::DepthSensorPose::reconf_serv_params_updated_ {true} |
|
private |
double depth_sensor_pose::DepthSensorPose::tilt_angle_est_ {0} |
|
private |
float depth_sensor_pose::DepthSensorPose::tilt_angle_max_ {0} |
|
private |
float depth_sensor_pose::DepthSensorPose::tilt_angle_min_ {0} |
|
private |
unsigned depth_sensor_pose::DepthSensorPose::used_depth_height_ {0} |
|
private |
The documentation for this class was generated from the following files: