Program Listing for File DepthImageToLaserScan.hpp

↰ Return to documentation for file (/tmp/ws/src/depthimage_to_laserscan/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp)

// 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
 */

#ifndef DEPTHIMAGE_TO_LASERSCAN__DEPTHIMAGETOLASERSCAN_HPP_
#define DEPTHIMAGE_TO_LASERSCAN__DEPTHIMAGETOLASERSCAN_HPP_

#include <cmath>
#include <string>

#include "depthimage_to_laserscan/DepthImageToLaserScan_export.h"
#include "depthimage_to_laserscan/depth_traits.hpp"
#if __has_include("image_geometry/pinhole_camera_model.hpp")
#include "image_geometry/pinhole_camera_model.hpp"
#else
// This header was deprecated as of https://github.com/ros-perception/vision_opencv/pull/448
// (for Iron), and will be completely removed for J-Turtle.  However, we still need it in
// Humble, since the .hpp doesn't exist there.
#include "image_geometry/pinhole_camera_model.h"
#endif
#include <opencv2/core/core.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
{
class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final
{
public:
  explicit DepthImageToLaserScan(
    float scan_time, float range_min, float range_max, int scan_height,
    const std::string & frame_id);

  ~DepthImageToLaserScan();

  sensor_msgs::msg::LaserScan::UniquePtr convert_msg(
    const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg);

private:
  double magnitude_of_ray(const cv::Point3d & ray) const;

  double angle_between_rays(const cv::Point3d & ray1, const cv::Point3d & ray2) const;

  bool use_point(
    const float new_value, const float old_value, const float range_min,
    const float range_max) const;

  template<typename T>
  void convert(
    const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
    const image_geometry::PinholeCameraModel & cam_model,
    const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg, const int & scan_height) const
  {
    // Use correct principal point from calibration
    float center_x = cam_model.cx();

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

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

    int offset = static_cast<int>(cam_model.cy() - static_cast<double>(scan_height) / 2.0);
    depth_row += offset * row_step;  // Offset to center of image
    for (int v = offset; v < offset + scan_height_; v++, depth_row += row_step) {
      for (uint32_t u = 0; u < depth_msg->width; u++) {  // Loop over each pixel in row
        T depth = depth_row[u];

        double r = depth;  // Assign to pass through NaNs and Infs
        // Atan2(x, z), but depth divides out
        double th = -std::atan2(static_cast<double>(u - center_x) * constant_x, unit_scaling);
        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 = std::sqrt(std::pow(x, 2.0) + std::pow(z, 2.0));
        }

        // 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_;

  float scan_time_;
  float range_min_;
  float range_max_;
  int scan_height_;
  std::string output_frame_id_;
};
}  // namespace depthimage_to_laserscan

#endif  // DEPTHIMAGE_TO_LASERSCAN__DEPTHIMAGETOLASERSCAN_HPP_