ROS学习总结十五：机器人SLAM（hector）（depthimage_to_laserscan）

1、使用depthimage_to_laserscan功能包
之前在上一章我们使用的是激光雷达进行的建图，但是同时在（机器人必备条件：深度信息）中提到了使用深度相机其实也是能达到建图的效果的。这里我们也可以试一下。首先运行了shenlan的mbot_kinect_nav_gazebo.launch文件。这个打开跟之前的第十章中的内容其实是差不多的，就是一个带有Kinect相机的机器人模型以及一个小环境。唯一不同的是这里使用到了一个depthimage_to_laserscan功能包。它能够将图像深度数据转化为模拟的激光雷达数据，从第十四章中：

<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen">
    <remap from="image" to="/kinect/depth/image_raw"/>
    <remap from="camera_info" to=/kinect/depth/camera_info"/>
    <remap froam="scan" to="/scan"/>
    <param name="output_frame_id" value="/camera_link"/>
</node>

这一段代码中，/kinect/depth/image_raw"/与/kinect/depth/camera_info是depthimage_to_laserscan从相机订阅的，经过运算之后发布一个/scan话题。这时候如果使用：

rostopic echo /+Tab键

可以看到下面会有个发布的scan话题。之前我们使用的hector所订阅的也就是这个话题。此时，我们可以同样的运行一下hector建图包，可以看到在rviz中应该是有地图出现的，然后我们运用一个键盘控制节点使机器人运动一下，围绕场景走一圈，看看建图效果。

实际上来说这个效果可能不是太好，可以看到它整个漂移非常厉害。一个是在于hector算法太依赖于扫描结果，当我上下帧的扫描结果相同的时候，就很难确定我的机器人是否运动，所以这里就出现了漂移；另外就是这里的depthimage_to_laserscan功能包的运算上似乎也是有一定问题的。如果使用

rostopic echo /scan

命令查看一下，会发现它最远的激光点的距离只有五米，那这其实应该是没有发挥出一个视觉的深度相机该有的特征，相机的深度数据应远大于一般的激光传感器。
2、分析depthimage_to_laserscan功能包
2.1cmakelist.txt
首先这个功能包来自于ROS自带的安装包，默认的安装目录在/opt/ros/kinetic/include下，为了不影响到ROS，这里不改动这个文件,重新在ROS的官网上下载一个。新建一个文件夹：

$ mkdir -p ~/depth/src
$ cd ~/depth/src
$ git clone https://github.com/ros-perception/depthimage_to_laserscan.git
$ cd ..
$ catkin_make

如果不出意外是可以正常编译的。
然后我们可以打开这个包的cmakelist文件看一下:

# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
cmake_minimum_required(VERSION 2.8)
project(depthimage_to_laserscan)

# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED dynamic_reconfigure image_geometry image_transport nodelet roscpp sensor_msgs)
#find_package(OpenCV REQUIRED)

# Dynamic reconfigure support
generate_dynamic_reconfigure_options(cfg/Depth.cfg)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES DepthImageToLaserScan DepthImageToLaserScanROS DepthImageToLaserScanNodelet
  CATKIN_DEPENDS dynamic_reconfigure image_geometry image_transport nodelet roscpp sensor_msgs
)

include_directories(include ${catkin_INCLUDE_DIRS})

add_library(DepthImageToLaserScan src/DepthImageToLaserScan.cpp)
target_link_libraries(DepthImageToLaserScan ${catkin_LIBRARIES})

add_library(DepthImageToLaserScanROS src/DepthImageToLaserScanROS.cpp)
add_dependencies(DepthImageToLaserScanROS ${PROJECT_NAME}_gencfg)
target_link_libraries(DepthImageToLaserScanROS DepthImageToLaserScan ${catkin_LIBRARIES})

add_library(DepthImageToLaserScanNodelet src/DepthImageToLaserScanNodelet.cpp)
target_link_libraries(DepthImageToLaserScanNodelet DepthImageToLaserScanROS ${catkin_LIBRARIES})

add_executable(depthimage_to_laserscan src/depthimage_to_laserscan.cpp)
target_link_libraries(depthimage_to_laserscan DepthImageToLaserScanROS ${catkin_LIBRARIES})

if(CATKIN_ENABLE_TESTING)
  # Test the library
  catkin_add_gtest(libtest test/DepthImageToLaserScanTest.cpp)
  target_link_libraries(libtest DepthImageToLaserScan ${catkin_LIBRARIES})
endif()

# add the test executable, keep it from being built by "make all"
add_executable(test_dtl EXCLUDE_FROM_ALL test/depthimage_to_laserscan_rostest.cpp)

# Install targets
install(TARGETS DepthImageToLaserScan DepthImageToLaserScanROS DepthImageToLaserScanNodelet depthimage_to_laserscan
	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

install(DIRECTORY include/${PROJECT_NAME}/
        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(FILES nodelets.xml
        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

前面两行是版本号以及项目名称，这个应该不用管。然后从find_package这里开始，这里的含义应该是添加依赖项，也就是说运行下面的包需要用到这些依赖。往下：

generate_dynamic_reconfigure_options(cfg/Depth.cfg)

添加了一个动态参数配置，文件在功能包的cfg文件夹下，打开以后可以看到是一个python文件，配置的内容主要是扫描的高度、时间、范围等。
往下是：

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES DepthImageToLaserScan DepthImageToLaserScanROS DepthImageToLaserScanNodelet
  CATKIN_DEPENDS dynamic_reconfigure image_geometry image_transport nodelet roscpp sensor_msgs
)

cantin_package的功能有点类似于depend，但是它可以用于自己写的依赖项，也就是头文件，可以参考一下：https://blog.51cto.com/14335413/2404246当中写的比较。
再往下是：

include_directories(include ${catkin_INCLUDE_DIRS})

add_library(DepthImageToLaserScan src/DepthImageToLaserScan.cpp)
target_link_libraries(DepthImageToLaserScan ${catkin_LIBRARIES})

add_library(DepthImageToLaserScanROS src/DepthImageToLaserScanROS.cpp)
add_dependencies(DepthImageToLaserScanROS ${PROJECT_NAME}_gencfg)
target_link_libraries(DepthImageToLaserScanROS DepthImageToLaserScan ${catkin_LIBRARIES})

add_library(DepthImageToLaserScanNodelet src/DepthImageToLaserScanNodelet.cpp)
target_link_libraries(DepthImageToLaserScanNodelet DepthImageToLaserScanROS ${catkin_LIBRARIES})

add_executable(depthimage_to_laserscan src/depthimage_to_laserscan.cpp)
target_link_libraries(depthimage_to_laserscan DepthImageToLaserScanROS ${catkin_LIBRARIES})

add_library是生成的库文件，用于被其他文件的调用。add_executable是生成可执行文件，可以在ros中启动，也就是说这里我生成了三个库文件以及一个可执行文件。
最后面的部分是生成一个测试的可执行文件，以及三个install，关于install可以参考：https://www.cnblogs.com/coderfenghc/archive/2012/08/12/2627561.html。
2.2depthimage_to_laserscan.cpp
然后我们打开depthimage_to_laserscan.cpp这个文件:

#include 

int main(int argc, char **argv){
  ros::init(argc, argv, "depthimage_to_laserscan");
  ros::NodeHandle n;
  ros::NodeHandle pnh("~");

  depthimage_to_laserscan::DepthImageToLaserScanROS dtl(n, pnh);

  ros::spin();

  return 0;
}

这个是之前cmakelist中用于编译成可执行文件的那个，打开后会发现它其实很简单，就一个头文件一个主函数，头文件包含的是include目录下的depthimagetolaserscanros.h文件，主函数调用的也是DepthImageToLaserScanROS这个函数。然后我们打开DepthImageToLaserScanROS.cpp文件：

#include 

using namespace depthimage_to_laserscan;

DepthImageToLaserScanROS::DepthImageToLaserScanROS(ros::NodeHandle& n, ros::NodeHandle& pnh):pnh_(pnh), it_(n), srv_(pnh) {
  boost::mutex::scoped_lock lock(connect_mutex_);

  // Dynamic Reconfigure
  dynamic_reconfigure::Server<depthimage_to_laserscan::DepthConfig>::CallbackType f;
  f = boost::bind(&DepthImageToLaserScanROS::reconfigureCb, this, _1, _2);
  srv_.setCallback(f);

  // Lazy subscription to depth image topic
  pub_ = n.advertise<sensor_msgs::LaserScan>("scan", 10, boost::bind(&DepthImageToLaserScanROS::connectCb, this, _1), boost::bind(&DepthImageToLaserScanROS::disconnectCb, this, _1));
}

DepthImageToLaserScanROS::~DepthImageToLaserScanROS(){
  sub_.shutdown();
}



void DepthImageToLaserScanROS::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
        const sensor_msgs::CameraInfoConstPtr& info_msg){
  try
  {
    sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg, info_msg);
    pub_.publish(scan_msg);
  }
  catch (std::runtime_error& e)
  {
    ROS_ERROR_THROTTLE(1.0, "Could not convert depth image to laserscan: %s", e.what());
  }
}

void DepthImageToLaserScanROS::connectCb(const ros::SingleSubscriberPublisher& pub) {
  boost::mutex::scoped_lock lock(connect_mutex_);
  if (!sub_ && pub_.getNumSubscribers() > 0) {
    ROS_DEBUG("Connecting to depth topic.");
    image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_);
    sub_ = it_.subscribeCamera("image", 10, &DepthImageToLaserScanROS::depthCb, this, hints);
  }
}

void DepthImageToLaserScanROS::disconnectCb(const ros::SingleSubscriberPublisher& pub) {
  boost::mutex::scoped_lock lock(connect_mutex_);
  if (pub_.getNumSubscribers() == 0) {
    ROS_DEBUG("Unsubscribing from depth topic.");
    sub_.shutdown();
  }
}

void DepthImageToLaserScanROS::reconfigureCb(depthimage_to_laserscan::DepthConfig& config, uint32_t level){
    dtl_.set_scan_time(config.scan_time);
    dtl_.set_range_limits(config.range_min, config.range_max);
    dtl_.set_scan_height(config.scan_height);
    dtl_.set_output_frame(config.output_frame_id);
}

这里的内容比较多，但是大致还是可以看出来，这个文件的作用主要是调用以及发布。
从：

  pub_ = n.advertise<sensor_msgs::LaserScan>("scan", 10, boost::bind(&DepthImageToLaserScanROS::connectCb, this, _1), boost::bind(&DepthImageToLaserScanROS::disconnectCb, this, _1));
}

可以看出所发布的话题名称以及频率
在

void DepthImageToLaserScanROS::connectCb(const ros::SingleSubscriberPublisher& pub) {
  boost::mutex::scoped_lock lock(connect_mutex_);
  if (!sub_ && pub_.getNumSubscribers() > 0) {
    ROS_DEBUG("Connecting to depth topic.");
    image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_);
    sub_ = it_.subscribeCamera("image", 10, &DepthImageToLaserScanROS::depthCb, this, hints);
  }
}

这个函数中应该用于调用相机数据。
最后面的

void DepthImageToLaserScanROS::reconfigureCb(depthimage_to_laserscan::DepthConfig& config, uint32_t level){
    dtl_.set_scan_time(config.scan_time);
    dtl_.set_range_limits(config.range_min, config.range_max);
    dtl_.set_scan_height(config.scan_height);
    dtl_.set_output_frame(config.output_frame_id);
}

给出了发布数据的一些参数，这些参数应该来自于动态参数。也就是上面提到的cfg文件夹中的python文件。
然后打开DepthImageToLaserScan.cpp文件看一下：

#include 

using namespace depthimage_to_laserscan;

DepthImageToLaserScan::DepthImageToLaserScan()
  : scan_time_(1./30.)
  , range_min_(0.45)
  , range_max_(10.0)
  , scan_height_(1)
{
}

DepthImageToLaserScan::~DepthImageToLaserScan(){
}

double DepthImageToLaserScan::magnitude_of_ray(const cv::Point3d& ray) const{
  return sqrt(pow(ray.x, 2.0) + pow(ray.y, 2.0) + pow(ray.z, 2.0));
}

double DepthImageToLaserScan::angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const{
  const double dot_product = ray1.x*ray2.x + ray1.y*ray2.y + ray1.z*ray2.z;
  const double magnitude1 = magnitude_of_ray(ray1);
  const double magnitude2 = magnitude_of_ray(ray2);;
  return acos(dot_product / (magnitude1 * magnitude2));
}

bool DepthImageToLaserScan::use_point(const float new_value, const float old_value, const float range_min, const float range_max) const{
  // Check for NaNs and Infs, a real number within our limits is more desirable than these.
  const bool new_finite = std::isfinite(new_value);
  const bool old_finite = std::isfinite(old_value);

  // Infs are preferable over NaNs (more information)
  if(!new_finite && !old_finite){ // Both are not NaN or Inf.
    if(!std::isnan(new_value)){ // new is not NaN, so use it's +-Inf value.
      return true;
    }
    return false; // Do not replace old_value
  }

  // If not in range, don't bother
  const bool range_check = range_min <= new_value && new_value <= range_max;
  if(!range_check){
    return false;
  }

  if(!old_finite){ // New value is in range and finite, use it.
    return true;
  }

  // Finally, if they are both numerical and new_value is closer than old_value, use new_value.
  const bool shorter_check = new_value < old_value;
  return shorter_check;
}

sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_msg,
        const sensor_msgs::CameraInfoConstPtr& info_msg){
  // Set camera model
  cam_model_.fromCameraInfo(info_msg);

  // Calculate angle_min and angle_max by measuring angles between the left ray, right ray, and optical center ray
  cv::Point2d raw_pixel_left(0, cam_model_.cy());
  cv::Point2d rect_pixel_left = cam_model_.rectifyPoint(raw_pixel_left);
  cv::Point3d left_ray = cam_model_.projectPixelTo3dRay(rect_pixel_left);

  cv::Point2d raw_pixel_right(depth_msg->width-1, cam_model_.cy());
  cv::Point2d rect_pixel_right = cam_model_.rectifyPoint(raw_pixel_right);
  cv::Point3d right_ray = cam_model_.projectPixelTo3dRay(rect_pixel_right);

  cv::Point2d raw_pixel_center(cam_model_.cx(), cam_model_.cy());
  cv::Point2d rect_pixel_center = cam_model_.rectifyPoint(raw_pixel_center);
  cv::Point3d center_ray = cam_model_.projectPixelTo3dRay(rect_pixel_center);

  const double angle_max = angle_between_rays(left_ray, center_ray);
  const double angle_min = -angle_between_rays(center_ray, right_ray); // Negative because the laserscan message expects an opposite rotation of that from the depth image

  // Fill in laserscan message
  sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan());
  scan_msg->header = depth_msg->header;
  if(output_frame_id_.length() > 0){
    scan_msg->header.frame_id = output_frame_id_;
  }
  scan_msg->angle_min = angle_min;
  scan_msg->angle_max = angle_max;
  scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / (depth_msg->width - 1);
  scan_msg->time_increment = 0.0;
  scan_msg->scan_time = scan_time_;
  scan_msg->range_min = range_min_;
  scan_msg->range_max = range_max_;

  // Check scan_height vs image_height
  if(scan_height_/2 > cam_model_.cy() || scan_height_/2 > depth_msg->height - cam_model_.cy()){
    std::stringstream ss;
    ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height.";
    throw std::runtime_error(ss.str());
  }

  // Calculate and fill the ranges
  const uint32_t ranges_size = depth_msg->width;
  scan_msg->ranges.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());

  if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1)
  {
    convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_);
  }
  else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
  {
    convert<float>(depth_msg, cam_model_, scan_msg, scan_height_);
  }
  else
  {
    std::stringstream ss;
    ss << "Depth image has unsupported encoding: " << depth_msg->encoding;
    throw std::runtime_error(ss.str());
  }

  return scan_msg;
}

void DepthImageToLaserScan::set_scan_time(const float scan_time){
  scan_time_ = scan_time;
}

void DepthImageToLaserScan::set_range_limits(const float range_min, const float range_max){
  range_min_ = range_min;
  range_max_ = range_max;
}

void DepthImageToLaserScan::set_scan_height(const int scan_height){
  scan_height_ = scan_height;
}

void DepthImageToLaserScan::set_output_frame(const std::string& output_frame_id){
  output_frame_id_ = output_frame_id;
}

这个文件就是基本都是数学运算的。前面的：

double DepthImageToLaserScan::magnitude_of_ray(const cv::Point3d& ray) const{
  return sqrt(pow(ray.x, 2.0) + pow(ray.y, 2.0) + pow(ray.z, 2.0));
}

double DepthImageToLaserScan::angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const{
  const double dot_product = ray1.x*ray2.x + ray1.y*ray2.y + ray1.z*ray2.z;
  const double magnitude1 = magnitude_of_ray(ray1);
  const double magnitude2 = magnitude_of_ray(ray2);;
  return acos(dot_product / (magnitude1 * magnitude2));
}

这里应该是计算了射线的距离以及每两帧之间的角度；

bool DepthImageToLaserScan::use_point(const float new_value, const float old_value, const float range_min, const float range_max) const{
  // Check for NaNs and Infs, a real number within our limits is more desirable than these.
  const bool new_finite = std::isfinite(new_value);
  const bool old_finite = std::isfinite(old_value);

  // Infs are preferable over NaNs (more information)
  if(!new_finite && !old_finite){ // Both are not NaN or Inf.
    if(!std::isnan(new_value)){ // new is not NaN, so use it's +-Inf value.
      return true;
    }
    return false; // Do not replace old_value
  }

  // If not in range, don't bother
  const bool range_check = range_min <= new_value && new_value <= range_max;
  if(!range_check){
    return false;
  }

  if(!old_finite){ // New value is in range and finite, use it.
    return true;
  }

  // Finally, if they are both numerical and new_value is closer than old_value, use new_value.
  const bool shorter_check = new_value < old_value;
  return shorter_check;
}

这里给出了数据更新。最后是：

sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_msg,
        const sensor_msgs::CameraInfoConstPtr& info_msg){
  // Set camera model
  cam_model_.fromCameraInfo(info_msg);

  // Calculate angle_min and angle_max by measuring angles between the left ray, right ray, and optical center ray
  cv::Point2d raw_pixel_left(0, cam_model_.cy());
  cv::Point2d rect_pixel_left = cam_model_.rectifyPoint(raw_pixel_left);
  cv::Point3d left_ray = cam_model_.projectPixelTo3dRay(rect_pixel_left);

  cv::Point2d raw_pixel_right(depth_msg->width-1, cam_model_.cy());
  cv::Point2d rect_pixel_right = cam_model_.rectifyPoint(raw_pixel_right);
  cv::Point3d right_ray = cam_model_.projectPixelTo3dRay(rect_pixel_right);

  cv::Point2d raw_pixel_center(cam_model_.cx(), cam_model_.cy());
  cv::Point2d rect_pixel_center = cam_model_.rectifyPoint(raw_pixel_center);
  cv::Point3d center_ray = cam_model_.projectPixelTo3dRay(rect_pixel_center);

  const double angle_max = angle_between_rays(left_ray, center_ray);
  const double angle_min = -angle_between_rays(center_ray, right_ray); // Negative because the laserscan message expects an opposite rotation of that from the depth image

  // Fill in laserscan message
  sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan());
  scan_msg->header = depth_msg->header;
  if(output_frame_id_.length() > 0){
    scan_msg->header.frame_id = output_frame_id_;
  }
  scan_msg->angle_min = angle_min;
  scan_msg->angle_max = angle_max;
  scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / (depth_msg->width - 1);
  scan_msg->time_increment = 0.0;
  scan_msg->scan_time = scan_time_;
  scan_msg->range_min = range_min_;
  scan_msg->range_max = range_max_;

  // Check scan_height vs image_height
  if(scan_height_/2 > cam_model_.cy() || scan_height_/2 > depth_msg->height - cam_model_.cy()){
    std::stringstream ss;
    ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height.";
    throw std::runtime_error(ss.str());
  }

  // Calculate and fill the ranges
  const uint32_t ranges_size = depth_msg->width;
  scan_msg->ranges.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());

  if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1)
  {
    convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_);
  }
  else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
  {
    convert<float>(depth_msg, cam_model_, scan_msg, scan_height_);
  }
  else
  {
    std::stringstream ss;
    ss << "Depth image has unsupported encoding: " << depth_msg->encoding;
    throw std::runtime_error(ss.str());
  }

  return scan_msg;
}

这个应该是对数据的处理函数了，这里面应该定义了激光的边界点坐标、角度等信息。其中的这一段：

  cv::Point2d raw_pixel_center(cam_model_.cx(), cam_model_.cy());
  cv::Point2d rect_pixel_center = cam_model_.rectifyPoint(raw_pixel_center);
  cv::Point3d center_ray = cam_model_.projectPixelTo3dRay(rect_pixel_center);

应该是定义了激光的中心距离。但是到这里还是有个疑惑，之前的最远距离5米是在哪里设置的其实这里并没有看到。
后来是在.h文件中找到了一段算法：

    template<typename T>
    void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model,
        const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{
      // Use correct principal point from calibration
      const float center_x = cam_model.cx();
      const float center_y = cam_model.cy();

      // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
      const double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters( T(1) );
      const float constant_x = unit_scaling / cam_model.fx();

      const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
      const int row_step = depth_msg->step / sizeof(T);

      const int offset = (int)(center_y - scan_height/2);
      depth_row += offset*row_step; // Offset to center of image

      for(int v = offset; v < offset+scan_height_; ++v, depth_row += row_step){
        for (int u = 0; u < (int)depth_msg->width; ++u) // Loop over each pixel in row
        {
          const T depth = depth_row[u];

          double r = depth; // Assign to pass through NaNs and Infs
          const double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
          const int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;

          if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
            // Calculate in XYZ
            double x = (u - center_x) * depth * constant_x;
            double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);

            // Calculate actual distance
            r = hypot(x, z);
          }

          // Determine if this point should be used.
          if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
            scan_msg->ranges[index] = r;
          }
        }
      }
    }

    image_geometry::PinholeCameraModel cam_model_; ///< image_geometry helper class for managing sensor_msgs/CameraInfo messages.

    float scan_time_; ///< Stores the time between scans.
    float range_min_; ///< Stores the current minimum range to use.
    float range_max_; ///< Stores the current maximum range to use.
    int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area.
    std::string output_frame_id_; ///< Output frame_id for each laserscan.  This is likely NOT the camera's frame_id.
  };


}; // depthimage_to_laserscan

#endif

这里给出了一些关于距离的计算公式。例如这里：

      const int offset = (int)(center_y - scan_height/2);
      depth_row += offset*row_step; // Offset to center of image

给出了取值的高度等。但是通过这里我们大致可以看出它其实取值取的是Kinect点云的深度数据。也就是说如果我们修改了深度数据的话就可以使我们的点云深度改变了。
3、修改depthimage_to_laserscan功能包
为了防止因为不同工作路径下相同名称的功能包带来的冲突，我们尽量将这个功能包用一个新的名称替代掉。从上面的内容大概可以看出来，其实只是一个depth.cfg、3个.h文件、3个依赖文件以及最后的一个执行文件，其他的内容其实是无关的。我们可以把这部分内容提取出来
首先同样建立一个文件夹：

$ mkdir -p ~/test/src
$ cd ~/test/src

然后在src文件夹下建立一个新的文件夹，取名test1。将之前depth文件夹下中depthimage_to_laserscan文件夹下的cfg、include、src文件夹拷贝过来，另外camkelist以及nodelets.xml文件以及package.xml文件也拷过来。
修改camkelist.txt文件如下：

# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
cmake_minimum_required(VERSION 2.8)
project(test1)

# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED dynamic_reconfigure image_geometry image_transport nodelet roscpp sensor_msgs)
#find_package(OpenCV REQUIRED)

# Dynamic reconfigure support
generate_dynamic_reconfigure_options(cfg/Depth.cfg)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES DepthImageToLaserScan DepthImageToLaserScanROS DepthImageToLaserScanNodelet
  CATKIN_DEPENDS dynamic_reconfigure image_geometry image_transport nodelet roscpp sensor_msgs
)

include_directories(include ${catkin_INCLUDE_DIRS})

add_library(DepthImageToLaserScan src/DepthImageToLaserScan.cpp)
target_link_libraries(DepthImageToLaserScan ${catkin_LIBRARIES})

add_library(DepthImageToLaserScanROS src/DepthImageToLaserScanROS.cpp)
add_dependencies(DepthImageToLaserScanROS ${PROJECT_NAME}_gencfg)
target_link_libraries(DepthImageToLaserScanROS DepthImageToLaserScan ${catkin_LIBRARIES})

add_library(DepthImageToLaserScanNodelet src/DepthImageToLaserScanNodelet.cpp)
target_link_libraries(DepthImageToLaserScanNodelet DepthImageToLaserScanROS ${catkin_LIBRARIES})

add_executable(test1 src/depthimage_to_laserscan.cpp)
target_link_libraries(test1 DepthImageToLaserScanROS ${catkin_LIBRARIES})

这里主要删除了下面一些没有用的安装项，包括了三个安装以及还有一个test。test用不到，安装项应该之前就已经装好了的。
然后再修改一下package.xml文件：

<package>
  <name>test1</name>
  <version>1.0.8</version>
  <description>test1</description>
  <maintainer email="[email protected]">Chad Rockey</maintainer>

  <license>BSD</license>

  <!--url type="website">http://ros.org/wiki/depthimage_to_laserscan</url-->
  <!--url type="bugtracker">https://github.com/ros-perception/depthimage_to_laserscan/issues</url-->
  <!--url type="repository">https://github.com/ros-perception/depthimage_to_laserscan</url-->

  <author>Chad Rockey</author>

  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>gtest</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>nodelet</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>image_geometry</build_depend>
  <build_depend>dynamic_reconfigure</build_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>nodelet</run_depend>
  <run_depend>image_transport</run_depend>
  <run_depend>image_geometry</run_depend>
  <run_depend>dynamic_reconfigure</run_depend>

  <export>
    <nodelet plugin="${prefix}/nodelets.xml"/>
  </export>
</package>

改完之后回到test目录下执行cmake

$ cd ~/test
$ catkin_make

编译没有报错的话，我们可以修改一下之前的launch文件，让它来调用一下我们现在的这个功能包：

    <node pkg="test1" type="test1" name="test1" output="screen">
        <remap from="image" to="/kinect/depth/image_raw" />
        <remap from="camera_info" to="/kinect/depth/camera_info" />
        <remap from="scan" to="/scan" />
        <param name="output_frame_id" value="/kinect_link" />
    </node>

如果没有问题这里是可以正常打开的。同样的可以启动一下hector建图包看一下，有效果的话说明我们的代码已经移植出来了。
这个时候我们可以修改一下关于距离参数。这个参数其实在很多地方都涉及到了。首先如果你去看模型的参数文件的话会发现它在调用libgazebo_ros_openni_kinect.so文件的时候定义了一个range_max=10。也就是最远距离为10米的意思。同样的在depthimage_to_laserscan的cfg动态参数文件中也有个range_max=10。同时注意到DepthImageToLaserScan.cpp文件中同样定义了range_max=10。理论上来说如果定义到10米的话不应该得到5米的数据。当然也并不是说这里的数据没有起作用，当你把这些数据都改小，例如设置到4米的时候，你会发现点云深度只有4米了，也就是说这些参数其实是在起作用的。很显然还存在一个参数在限制这个距离。
最后注意到在我们调用的libgazebo_ros_openni_kinect.so这个文件，它其实对应的内部也是设置了一个距离的，这个参数在gazebo_ros_pkgs/gazebo_plugins/src文件夹下的gazebo_ros_openni_kinect.cpp文件中，注意到有这么一段代码：

  if (!_sdf->HasElement("pointCloudCutoff"))
    this->point_cloud_cutoff_ = 0.4;
  else
    this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<double>();
  if (!_sdf->HasElement("pointCloudCutoffMax"))
    this->point_cloud_cutoff_max_ = 5.0;
  else

也就是这里限制了点云的最大距离是5米，所以你外参再怎么改变也是不能超过这个值的。于是我修改了这个参数，重新编译了这个文件。注：尽量使用不同的文件名，可以直接修改cmakelist中生成的target的名称即可。重新试验一下，会发现现在的点云深度就比以前更远了：

从这里可以看出，点云的距离比一开始远了很多，在左图的位置与右侧rviz中深度数据对比可以看到在现在的位置就可以看到最远处的墙壁了。右下的深度数据同样可以看出来我的深度点云数据差不多是8米了。一般来说Kinect相机的实际测距大概也是8米。