ROS的depthimage_to_laserscan剖析

当我们想把RGBD相机的深度图转成单线激光雷达来使时，经常考虑到使用depthimage_to_laserscan的ros包。

RGBD极坐标系下的极限角

其中有关于通过深度图和相机内参转成基于极坐标系下的单线激光雷达最大和最小角度这两个参数，用到的代码如下：

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

其中用到的公式主要是基于两个向量来算的，默认相机中心为原点（0，0，0），如下图：

 

 因此左边点跟原点是向量a，右边点和原点构成向量b，则根据公式就可算出来角度。

变换一下就可以得到角度：

 RGBD极坐标系下的坐标值

极角是通过公式：

 极长计算分两步：

1. 先算x

变换就可以得到 

 

 2.再算极长

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

 带状数据处理

带状的意思主要是scan_height,如果scan_height为1时，该代码只找到深度图中间的cy的那根线，如果大于1就变成带状的结构。

带状的数据按照数据有效和最近原则筛选：

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