Merge pull request #5 from WHILL/fix/variable_image_size

Fix: Missing image size conversion for vx-disparity
whill-labs · Feb 18, 2020 · f7af1a9 · f7af1a9
2 parents cc9259f + 9aa44b2
commit f7af1a9
Show file tree

Hide file tree

Showing 5 changed files with 72 additions and 23 deletions.
diff --git a/include/gpu_stereo_image_proc/libsgm_sgbm_processor.h b/include/gpu_stereo_image_proc/libsgm_sgbm_processor.h
@@ -47,7 +47,8 @@ class LibSGMStereoSGBMProcessor : public StereoSGBMProcessor
     stereo_matcher_.reset(new sgm::LibSGMWrapper(disparity_range_, P1_, P2_, uniqueness_ratio_, true));
   }
 
-  virtual void processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect,
+  virtual void processDisparity(const cv::Mat&                           left_rect,
+                                const cv::Mat&                           right_rect,
                                 const image_geometry::StereoCameraModel& model,
                                 stereo_msgs::DisparityImage&             disparity) const;
 
@@ -57,9 +58,17 @@ class LibSGMStereoSGBMProcessor : public StereoSGBMProcessor
     ROS_INFO("Uniqueness  : %5.1f", uniqueness_ratio_);
     ROS_INFO("P1/P2       : P1 %d, P2, %d", P1_, P2_);
     ROS_INFO("Min/Max Disp: min %d, max %d", min_disparity_, max_disparity_);
-    ROS_INFO("Path Type   : %s", (path_type_ == sgm::PathType::SCAN_4PATH ? "SCAN_4PATH" : "SCAN_8PATH" ));
+    ROS_INFO("Path Type   : %s", (path_type_ == sgm::PathType::SCAN_4PATH ? "SCAN_4PATH" : "SCAN_8PATH"));
     ROS_INFO("===================================");
-    stereo_matcher_.reset(new sgm::LibSGMWrapper(disparity_range_, P1_, P2_, uniqueness_ratio_, true, path_type_, min_disparity_));
+    stereo_matcher_.reset(
+        new sgm::LibSGMWrapper(disparity_range_, P1_, P2_, uniqueness_ratio_, true, path_type_, min_disparity_));
+  }
+
+  bool setImageSize(cv::Size image_size)
+  {
+    ROS_WARN("Member variable 'image_size_' is not currently used.");
+    image_size_ = image_size;
+    return true;
   }
 
   float getUniquenessRatio() const
@@ -132,12 +141,10 @@ class LibSGMStereoSGBMProcessor : public StereoSGBMProcessor
     return ret;
   }
 
-
-
 private:
   std::shared_ptr<sgm::LibSGMWrapper> stereo_matcher_;
 
-  float uniqueness_ratio_;
+  float         uniqueness_ratio_;
   sgm::PathType path_type_;
 };
 

diff --git a/include/gpu_stereo_image_proc/sgbm_processor.h b/include/gpu_stereo_image_proc/sgbm_processor.h
@@ -74,12 +74,19 @@ class StereoSGBMProcessor
     ALL        = LEFT_ALL | RIGHT_ALL | STEREO_ALL
   };
 
-  StereoSGBMProcessor() : image_size_(640, 480), min_disparity_(0), max_disparity_(128), disparity_range_(64), P1_(200), P2_(400)
+  StereoSGBMProcessor()
+    : image_size_(640, 480), min_disparity_(0), max_disparity_(128), disparity_range_(64), P1_(200), P2_(400)
   {
   }
 
   virtual ~StereoSGBMProcessor(){};
 
+  cv::Size getImageSize() const
+  {
+    return image_size_;
+  }
+  virtual bool setImageSize(cv::Size image_size) = 0;
+
   int  getInterpolation() const;
   void setInterpolation(int interp);
 
@@ -95,15 +102,17 @@ class StereoSGBMProcessor
   int  getDisparityRange() const;
   bool setDisparityRange(int range);
 
-  int  getP1() const
-  {return P1_;}
+  int getP1() const
+  {
+    return P1_;
+  }
   void setP1(int P1)
   {
     ROS_INFO("%s, in %d", __func__, P1);
     P1_ = P1;
   }
 
-  int  getP2() const
+  int getP2() const
   {
     return P2_;
   }
@@ -113,18 +122,28 @@ class StereoSGBMProcessor
     P2_ = P2;
   }
 
-  bool process(const sensor_msgs::ImageConstPtr& left_raw, const sensor_msgs::ImageConstPtr& right_raw,
-               const image_geometry::StereoCameraModel& model, StereoImageSet& output, ImageProcFlag flags) const;
+  bool process(const sensor_msgs::ImageConstPtr&        left_raw,
+               const sensor_msgs::ImageConstPtr&        right_raw,
+               const image_geometry::StereoCameraModel& model,
+               StereoImageSet&                          output,
+               ImageProcFlag                            flags) const;
 
-  virtual void processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect,
+  virtual void processDisparity(const cv::Mat&                           left_rect,
+                                const cv::Mat&                           right_rect,
                                 const image_geometry::StereoCameraModel& model,
                                 stereo_msgs::DisparityImage&             disparity) const;
 
-  void processPoints(const stereo_msgs::DisparityImage& disparity, const cv::Mat& color, const std::string& encoding,
-                     const image_geometry::StereoCameraModel& model, sensor_msgs::PointCloud& points) const;
-
-  void processPoints2(const stereo_msgs::DisparityImage& disparity, const cv::Mat& coloer, const std::string& encoding,
-                      const image_geometry::StereoCameraModel& model, sensor_msgs::PointCloud2& points) const;
+  void processPoints(const stereo_msgs::DisparityImage&       disparity,
+                     const cv::Mat&                           color,
+                     const std::string&                       encoding,
+                     const image_geometry::StereoCameraModel& model,
+                     sensor_msgs::PointCloud&                 points) const;
+
+  void processPoints2(const stereo_msgs::DisparityImage&       disparity,
+                      const cv::Mat&                           coloer,
+                      const std::string&                       encoding,
+                      const image_geometry::StereoCameraModel& model,
+                      sensor_msgs::PointCloud2&                points) const;
 
 protected:
   image_proc::Processor       mono_processor_;

diff --git a/include/gpu_stereo_image_proc/vx_sgbm_processor.h b/include/gpu_stereo_image_proc/vx_sgbm_processor.h
@@ -48,7 +48,8 @@ class VXStereoSGBMProcessor : public StereoSGBMProcessor
     stereo_matcher_.reset(new VXStereoMatcher(image_size_.width, image_size_.height));
   }
 
-  virtual void processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect,
+  virtual void processDisparity(const cv::Mat&                           left_rect,
+                                const cv::Mat&                           right_rect,
                                 const image_geometry::StereoCameraModel& model,
                                 stereo_msgs::DisparityImage&             disparity) const;
 
@@ -71,6 +72,17 @@ class VXStereoSGBMProcessor : public StereoSGBMProcessor
                                               clip_, max_diff_, uniqueness_ratio_, scanline_mask_, flags_));
   }
 
+  bool setImageSize(cv::Size image_size)
+  {
+    if(image_size.width % 4 != 0)
+    {
+      ROS_WARN("Image Width must be divisible by 4.");
+      return false;
+    }
+    image_size_ = image_size;
+    return true;
+  }
+
   float getUniquenessRatio() const
   {
     return static_cast<float>(uniqueness_ratio_);

diff --git a/libSGM b/libSGM
diff --git a/src/nodelets/vx_disparity.cpp b/src/nodelets/vx_disparity.cpp
@@ -94,7 +94,9 @@ class VXDisparityNodelet : public nodelet::Nodelet
 
   void connectCb();
 
-  void imageCb(const ImageConstPtr& l_image_msg, const CameraInfoConstPtr& l_info_msg, const ImageConstPtr& r_image_msg,
+  void imageCb(const ImageConstPtr&      l_image_msg,
+               const CameraInfoConstPtr& l_info_msg,
+               const ImageConstPtr&      r_image_msg,
                const CameraInfoConstPtr& r_info_msg);
 
   void configCb(Config& config, uint32_t level);
@@ -161,8 +163,10 @@ void VXDisparityNodelet::connectCb()
   }
 }
 
-void VXDisparityNodelet::imageCb(const ImageConstPtr& l_image_msg, const CameraInfoConstPtr& l_info_msg,
-                                 const ImageConstPtr& r_image_msg, const CameraInfoConstPtr& r_info_msg)
+void VXDisparityNodelet::imageCb(const ImageConstPtr&      l_image_msg,
+                                 const CameraInfoConstPtr& l_info_msg,
+                                 const ImageConstPtr&      r_image_msg,
+                                 const CameraInfoConstPtr& r_info_msg)
 {
   // Update the camera model
   model_.fromCameraInfo(l_info_msg, r_info_msg);
@@ -189,6 +193,13 @@ void VXDisparityNodelet::imageCb(const ImageConstPtr& l_image_msg, const CameraI
   const cv::Mat_<uint8_t> l_image = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8)->image;
   const cv::Mat_<uint8_t> r_image = cv_bridge::toCvShare(r_image_msg, sensor_msgs::image_encodings::MONO8)->image;
 
+  const cv::Size image_size = block_matcher_.getImageSize();
+  if(image_size.width != l_image.cols || image_size.height != l_image.rows)
+  {
+    block_matcher_.setImageSize(cv::Size(l_image.cols, l_image.rows));
+    block_matcher_.applyConfig();
+  }
+
   // Perform block matching to find the disparities
   block_matcher_.processDisparity(l_image, r_image, model_, *disp_msg);
+1 −1		CMakeLists.txt
+22 −3		include/libsgm.h
+14 −11		include/libsgm_wrapper.h
+12 −10		sample/image/stereosgm_image.cpp
+8 −8		src/check_consistency.cu
+2 −2		src/internal.h
+3 −2		src/libsgm_wrapper.cpp
+1 −1		src/stereo_sgm.cpp