Program Listing for File stereo_processor.hpp

↰ Return to documentation for file (include/stereo_image_proc/stereo_processor.hpp)

// Copyright (c) 2008, Willow Garage, Inc.
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// 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 OWNER 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 STEREO_IMAGE_PROC__STEREO_PROCESSOR_HPP_
#define STEREO_IMAGE_PROC__STEREO_PROCESSOR_HPP_

#include <string>

#include "image_geometry/stereo_camera_model.hpp"

#include <image_proc/processor.hpp>
#include <sensor_msgs/msg/point_cloud.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <stereo_msgs/msg/disparity_image.hpp>

namespace stereo_image_proc
{

struct StereoImageSet
{
  image_proc::ImageSet left;
  image_proc::ImageSet right;
  stereo_msgs::msg::DisparityImage disparity;
  sensor_msgs::msg::PointCloud points;
  sensor_msgs::msg::PointCloud2 points2;
};

class StereoProcessor
{
public:
  StereoProcessor()
  {
    block_matcher_ = cv::StereoBM::create();
    sg_block_matcher_ = cv::StereoSGBM::create(1, 1, 10);
  }

  enum StereoType
  {
    BM, SGBM
  };

  enum
  {
    LEFT_MONO        = 1 << 0,
    LEFT_RECT        = 1 << 1,
    LEFT_COLOR       = 1 << 2,
    LEFT_RECT_COLOR  = 1 << 3,
    RIGHT_MONO       = 1 << 4,
    RIGHT_RECT       = 1 << 5,
    RIGHT_COLOR      = 1 << 6,
    RIGHT_RECT_COLOR = 1 << 7,
    DISPARITY        = 1 << 8,
    POINT_CLOUD      = 1 << 9,
    POINT_CLOUD2     = 1 << 10,
    LEFT_ALL = LEFT_MONO | LEFT_RECT | LEFT_COLOR | LEFT_RECT_COLOR,
    RIGHT_ALL = RIGHT_MONO | RIGHT_RECT | RIGHT_COLOR | RIGHT_RECT_COLOR,
    STEREO_ALL = DISPARITY | POINT_CLOUD | POINT_CLOUD2,
    ALL = LEFT_ALL | RIGHT_ALL | STEREO_ALL
  };

  inline StereoType getStereoType() const
  {
    return current_stereo_algorithm_;
  }

  inline void setStereoType(StereoType type)
  {
    current_stereo_algorithm_ = type;
  }

  inline int getInterpolation() const
  {
    return mono_processor_.interpolation_;
  }

  inline void setInterpolation(int interp)
  {
    mono_processor_.interpolation_ = interp;
  }

  inline int getPreFilterCap() const
  {
    if (current_stereo_algorithm_ == BM) {
      return block_matcher_->getPreFilterCap();
    }
    return sg_block_matcher_->getPreFilterCap();
  }

  inline void setPreFilterCap(int param)
  {
    block_matcher_->setPreFilterCap(param);
    sg_block_matcher_->setPreFilterCap(param);
  }

  inline int getCorrelationWindowSize() const
  {
    if (current_stereo_algorithm_ == BM) {
      return block_matcher_->getBlockSize();
    }
    return sg_block_matcher_->getBlockSize();
  }

  inline void setCorrelationWindowSize(int param)
  {
    block_matcher_->setBlockSize(param);
    sg_block_matcher_->setBlockSize(param);
  }

  inline int getMinDisparity() const
  {
    if (current_stereo_algorithm_ == BM) {
      return block_matcher_->getMinDisparity();
    }
    return sg_block_matcher_->getMinDisparity();
  }

  inline void setMinDisparity(int param)
  {
    block_matcher_->setMinDisparity(param);
    sg_block_matcher_->setMinDisparity(param);
  }

  inline int getDisparityRange() const
  {
    if (current_stereo_algorithm_ == BM) {
      return block_matcher_->getNumDisparities();
    }
    return sg_block_matcher_->getNumDisparities();
  }

  inline void setDisparityRange(int param)
  {
    block_matcher_->setNumDisparities(param);
    sg_block_matcher_->setNumDisparities(param);
  }

  inline float getUniquenessRatio() const
  {
    if (current_stereo_algorithm_ == BM) {
      return block_matcher_->getUniquenessRatio();
    }
    return sg_block_matcher_->getUniquenessRatio();
  }

  inline void setUniquenessRatio(float param)
  {
    block_matcher_->setUniquenessRatio(param);
    sg_block_matcher_->setUniquenessRatio(param);
  }

  inline int getSpeckleSize() const
  {
    if (current_stereo_algorithm_ == BM) {
      return block_matcher_->getSpeckleWindowSize();
    }
    return sg_block_matcher_->getSpeckleWindowSize();
  }

  inline void setSpeckleSize(int param)
  {
    block_matcher_->setSpeckleWindowSize(param);
    sg_block_matcher_->setSpeckleWindowSize(param);
  }

  inline int getSpeckleRange() const
  {
    if (current_stereo_algorithm_ == BM) {
      return block_matcher_->getSpeckleRange();
    }
    return sg_block_matcher_->getSpeckleRange();
  }

  inline void setSpeckleRange(int param)
  {
    block_matcher_->setSpeckleRange(param);
    sg_block_matcher_->setSpeckleRange(param);
  }

  // BM only

  inline int getPreFilterSize() const
  {
    return block_matcher_->getPreFilterSize();
  }

  inline void setPreFilterSize(int param)
  {
    block_matcher_->setPreFilterSize(param);
  }

  inline int getTextureThreshold() const
  {
    return block_matcher_->getTextureThreshold();
  }

  inline void setTextureThreshold(int param)
  {
    block_matcher_->setTextureThreshold(param);
  }

  // SGBM specific

  // getSgbmMode can return MODE_SGBM = 0, MODE_HH = 1. FullDP == 1 was MODE_HH so we're good
  inline int getSgbmMode() const
  {
    return sg_block_matcher_->getMode();
  }

  inline void setSgbmMode(int param)
  {
    sg_block_matcher_->setMode(param);
  }

  inline int getP1() const
  {
    return sg_block_matcher_->getP1();
  }

  inline void setP1(int param)
  {
    sg_block_matcher_->setP1(param);
  }

  inline int getP2() const
  {
    return sg_block_matcher_->getP2();
  }

  inline void setP2(int param)
  {
    sg_block_matcher_->setP2(param);
  }

  inline int getDisp12MaxDiff() const
  {
    return sg_block_matcher_->getDisp12MaxDiff();
  }

  inline void setDisp12MaxDiff(int param)
  {
    sg_block_matcher_->setDisp12MaxDiff(param);
  }

  // Do all the work!
  bool process(
    const sensor_msgs::msg::Image::ConstSharedPtr & left_raw,
    const sensor_msgs::msg::Image::ConstSharedPtr & right_raw,
    const image_geometry::StereoCameraModel & model,
    StereoImageSet & output,
    int flags) const;

  void processDisparity(
    const cv::Mat & left_rect,
    const cv::Mat & right_rect,
    const image_geometry::StereoCameraModel & model,
    stereo_msgs::msg::DisparityImage & disparity) const;

  void processPoints(
    const stereo_msgs::msg::DisparityImage & disparity,
    const cv::Mat & color,
    const std::string & encoding,
    const image_geometry::StereoCameraModel & model,
    sensor_msgs::msg::PointCloud & points) const;

  void processPoints2(
    const stereo_msgs::msg::DisparityImage & disparity,
    const cv::Mat & color,
    const std::string & encoding,
    const image_geometry::StereoCameraModel & model,
    sensor_msgs::msg::PointCloud2 & points) const;

private:
  image_proc::Processor mono_processor_;

  mutable cv::Mat_<int16_t> disparity16_;
  mutable cv::Ptr<cv::StereoBM> block_matcher_;
  mutable cv::Ptr<cv::StereoSGBM> sg_block_matcher_;
  StereoType current_stereo_algorithm_;
  mutable cv::Mat_<cv::Vec3f> dense_points_;
};

}  // namespace stereo_image_proc

#endif  // STEREO_IMAGE_PROC__STEREO_PROCESSOR_HPP_