depth_sensor_pose.h

Go to the documentation of this file.

#pragma once

#include <sstream>
#include <limits>
#include <vector>
#include <cmath>
#include <string>
#include <iostream>
#include <fstream>
#include <list>

#include <ros/console.h>
#include <ros/ros.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/pinhole_camera_model.h>

#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <Eigen/Core>

namespace depth_sensor_pose {

class DepthSensorPose {
 public:
  DepthSensorPose() = default;
  ~DepthSensorPose() = default;

  DepthSensorPose (const DepthSensorPose &) = delete;
  DepthSensorPose & operator= (const DepthSensorPose &) = delete;

  void estimateParams(const sensor_msgs::ImageConstPtr& depth_msg,
                      const sensor_msgs::CameraInfoConstPtr& info_msg);
  void setRangeLimits(const float rmin, const float rmax);

  void setSensorMountHeightMin (const float height);
  void setSensorMountHeightMax (const float height);
  void setSensorTiltAngleMin (const float angle);
  void setSensorTiltAngleMax (const float angle);
  void setPublishDepthEnable (const bool enable) { publish_depth_enable_ = enable; }
  bool getPublishDepthEnable () const { return publish_depth_enable_; }
  void setCamModelUpdate (const bool u) { cam_model_update_ = u; }
  void setUsedDepthHeight(const unsigned height);
  void setDepthImgStepRow(const int step);
  void setDepthImgStepCol(const int step);
  void setReconfParamsUpdated(bool updated) {reconf_serv_params_updated_ = updated; }

  void setRansacMaxIter(const unsigned u) { ransacMaxIter_ = u; }

  void setRansacDistanceThresh(const float u) { ransacDistanceThresh_ = u; }

  void setGroundMaxPoints(const unsigned u) { max_ground_points_ = u; }

  float getSensorTiltAngle() const { return tilt_angle_est_; }

  float getSensorMountHeight() const { return mount_height_est_; }

  sensor_msgs::ImageConstPtr getDbgImage() const;

 protected:
  double lengthOfVector( const cv::Point3d& vec) const;
  double angleBetweenRays(const cv::Point3d& ray1, const cv::Point3d& ray2) const;
  void fieldOfView(double & min, double & max, double x1, double y1,
                  double xc, double yc, double x2, double y2 );

  void calcDeltaAngleForImgRows(double vertical_fov);
  void calcGroundDistancesForImgRows( double mount_height, double tilt_angle,
                                      std::vector<double>& distances);

  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);

  void sensorPoseCalibration(const sensor_msgs::ImageConstPtr& depth_msg,
                             double& tilt_angle, double& height);

  sensor_msgs::ImagePtr prepareDbgImage(const sensor_msgs::ImageConstPtr& depth_msg,
    const std::list<std::pair<unsigned, unsigned>>& ground_points_indices);

 private:
  // ROS parameters configurated with config files or dynamic_reconfigure
  float     range_min_{0};                
  float     range_max_{0};                
  float     mount_height_min_{0};         
  float     mount_height_max_{0};         
  float     tilt_angle_min_{0};           
  float     tilt_angle_max_{0};           

  bool      publish_depth_enable_{false}; 
  bool      cam_model_update_{false};     
  unsigned  used_depth_height_{0};        
  unsigned  depth_image_step_row_{0};     
  unsigned  depth_image_step_col_{0};     

  unsigned  max_ground_points_{0};
  unsigned  ransacMaxIter_{0};
  float     ransacDistanceThresh_{0};
  float     groundDistTolerance_{0};

  image_geometry::PinholeCameraModel camera_model_;

  sensor_msgs::ImagePtr dbg_image_;

  double mount_height_est_{0};
  double tilt_angle_est_{0};
  bool  reconf_serv_params_updated_{true};

  std::vector<double> delta_row_;
  std::vector<double>dist_to_ground_max_;
  std::vector<double>dist_to_ground_min_;
};

} // namespace depth_sensor_pose