depth_sensor_pose.h

Go to the documentation of this file.

/******************************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2016, Michal Drwiega (drwiega.michal@gmail.com)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     1. Redistributions of source code must retain the above copyright
 *        notice, this list of conditions and the following disclaimer.
 *     2. Redistributions in binary form must reproduce the above copyright
 *        notice, this list of conditions and the following disclaimer in the
 *        documentation and/or other materials provided with the distribution.
 *     3. Neither the name of the copyright holder nor the names of its
 *        contributors may be used to endorse or promote products derived
 *        from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *****************************************************************************/
#ifndef DEPTH_SENSOR_POSE
#define DEPTH_SENSOR_POSE

#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 <sstream>
#include <limits.h>
#include <math.h>
#include <vector>
#include <cmath>
#include <string>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>

#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>

#define DEBUG 1

//#define               DATA_TO_FILE
//#define       DEBUG_INFO
//#define               DEBUG_CALIBRATION

namespace depth_sensor_pose
{ 
class DepthSensorPose
{
  //----------------------------------------------------------------------------
public:
  DepthSensorPose();
  ~DepthSensorPose();

  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 int height);
  void setDepthImgStepRow(const int step);
  void setDepthImgStepCol(const int step);
  void setReconfParamsUpdated (bool updated) {reconf_serv_params_updated_ = updated; }




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

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

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

  float getSensorTiltAngle () const { return tilt_angle_est_; }
  float getSensorMountHeight () const { return mount_height_est_; }

  //---------------------------------------------------------------------------------------------
private: // Private methods
  double lengthOfVector( const cv::Point3d& ray) 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<unsigned int>& distances);
  void getGroundPoints(const sensor_msgs::ImageConstPtr& depth_msg,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr& points);
  void sensorPoseCalibration(const sensor_msgs::ImageConstPtr& depth_msg, double & tilt, double & height);

  //-----------------------------------------------------------------------------------------------
public: // Public fields

  sensor_msgs::Image new_depth_msg_;

private: // Private fields

  //----------------------------------------------------------------------------
  // ROS parameters configurated with config files or dynamic_reconfigure
  float        range_min_;            
  float        range_max_;            

  float        mount_height_min_;     
  float        mount_height_max_;     
  float        tilt_angle_min_;       
  float        tilt_angle_max_;       

  bool         publish_depth_enable_; 
  bool         cam_model_update_;     
  unsigned int used_depth_height_;    
  unsigned int depth_image_step_row_; 
  unsigned int depth_image_step_col_; 

  unsigned int max_ground_points_;

  unsigned int ransacMaxIter_;
  float ransacDistanceThresh_;
  float groundDistTolerance_;


  //----------------------------------------------------------------------------

  image_geometry::PinholeCameraModel camera_model_;

  std::vector<double> delta_row_;

  float mount_height_est_;
  float tilt_angle_est_;

  bool  reconf_serv_params_updated_;

  std::vector<unsigned int>dist_to_ground_max_;
  std::vector<unsigned int>dist_to_ground_min_;

};

template <typename T>
std::string NumberToString ( T Number )
{
  std::ostringstream ss;
  ss << Number;
  return ss.str();
}

} // depth_sensor_pose

#endif