DepthImageToLaserScan.cpp

// Copyright (c) 2012, Willow Garage, Inc.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//
//    * 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.
//
//    * 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.

/*
 * Author: Chad Rockey
 */

#include <depthimage_to_laserscan/DepthImageToLaserScan.hpp>

#include <cmath>
#include <limits>
#include <memory>
#include <sstream>
#include <string>

#include <opencv2/core/core.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

namespace depthimage_to_laserscan
{

DepthImageToLaserScan::DepthImageToLaserScan(
  float scan_time, float range_min, float range_max,
  int scan_height, float scan_offset, const std::string & frame_id)
: scan_time_(scan_time), range_min_(range_min), range_max_(range_max), scan_height_(scan_height),
  scan_offset_(scan_offset), output_frame_id_(frame_id)
{
}

DepthImageToLaserScan::~DepthImageToLaserScan()
{
}

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

double DepthImageToLaserScan::angle_between_rays(
  const cv::Point3d & ray1,
  const cv::Point3d & ray2) const
{
  double dot_product = ray1.x * ray2.x + ray1.y * ray2.y + ray1.z * ray2.z;
  double magnitude1 = magnitude_of_ray(ray1);
  double magnitude2 = magnitude_of_ray(ray2);
  return std::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.
  bool new_finite = std::isfinite(new_value);
  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.
    return !std::isnan(new_value);  // new is not NaN, so use it's +-Inf value.
  }

  // If not in range, don't bother
  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.
  bool shorter_check = new_value < old_value;
  return shorter_check;
}

sensor_msgs::msg::LaserScan::UniquePtr DepthImageToLaserScan::convert_msg(
  const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & 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);

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

  // Fill in laserscan message
  sensor_msgs::msg::LaserScan::UniquePtr scan_msg = std::make_unique<sensor_msgs::msg::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
  double center_row = cam_model_.cy() * 2 * scan_offset_;
  double bottom_row = center_row - scan_height_ / 2;
  double top_row = center_row + scan_height_ / 2;
  if (bottom_row < 0 || top_row >= depth_msg->height) {
    std::stringstream ss;
    ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height ( " <<
      depth_msg->height << " ) with a scan_offset of " << scan_offset_ << " and cy of " <<
      cam_model_.cy() << ". ";
    throw std::runtime_error(ss.str());
  }

  // Calculate and fill the ranges
  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_, scan_offset_);
  } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
    convert<float>(depth_msg, cam_model_, scan_msg, scan_height_, scan_offset_);
  } else {
    std::stringstream ss;
    ss << "Depth image has unsupported encoding: " << depth_msg->encoding;
    throw std::runtime_error(ss.str());
  }

  return scan_msg;
}

}  // namespace depthimage_to_laserscan