1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2014-, Open Perception, Inc. 6 * 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 13 * * Redistributions of source code must retain the above copyright 14 * notice, this list of conditions and the following disclaimer. 15 * * Redistributions in binary form must reproduce the above 16 * copyright notice, this list of conditions and the following 17 * disclaimer in the documentation and/or other materials provided 18 * with the distribution. 19 * * Neither the name of the copyright holder(s) nor the names of its 20 * contributors may be used to endorse or promote products derived 21 * from this software without specific prior written permission. 22 * 23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 * POSSIBILITY OF SUCH DAMAGE. 35 */ 36 37 #pragma once 38 39 #include <pcl/point_cloud.h> 40 #include <pcl/point_types.h> 41 42 #include <cstring> 43 #include <vector> 44 45 namespace pcl { 46 47 /** \brief Compute point cloud from the disparity map. 48 * 49 * Example of usage: 50 * 51 * \code 52 * pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new 53 * pcl::PointCloud<pcl::PointXYZI>); 54 * pcl::PointCloud<pcl::RGB>::Ptr left_image (new 55 * pcl::PointCloud<pcl::RGB>); 56 * // Fill left image cloud. 57 * 58 * pcl::DisparityMapConverter<pcl::PointXYZI> dmc; 59 * dmc.setBaseline (0.8387445f); 60 * dmc.setFocalLength (368.534700f); 61 * dmc.setImageCenterX (318.112200f); 62 * dmc.setImageCenterY (224.334900f); 63 * dmc.setDisparityThresholdMin(15.0f); 64 * 65 * // Left view of the scene. 66 * dmc.setImage (left_image); 67 * // Disparity map of the scene. 68 * dmc.loadDisparityMap ("disparity_map.txt", 640, 480); 69 * 70 * dmc.compute(*cloud); 71 * \endcode 72 * 73 * \author Timur Ibadov (ibadov.timur@gmail.com) 74 * \ingroup stereo 75 */ 76 template <typename PointT> 77 class DisparityMapConverter { 78 protected: 79 using PointCloud = pcl::PointCloud<PointT>; 80 81 public: 82 /** \brief DisparityMapConverter constructor. */ 83 DisparityMapConverter(); 84 85 /** \brief Empty destructor. */ 86 virtual ~DisparityMapConverter(); 87 88 /** \brief Set x-coordinate of the image center. 89 * \param[in] center_x x-coordinate of the image center. 90 */ 91 inline void 92 setImageCenterX(const float center_x); 93 94 /** \brief Get x-coordinate of the image center. 95 * \return x-coordinate of the image center. 96 */ 97 inline float 98 getImageCenterX() const; 99 100 /** \brief Set y-coordinate of the image center. 101 * \param[in] center_y y-coordinate of the image center. 102 */ 103 inline void 104 setImageCenterY(const float center_y); 105 106 /** \brief Get y-coordinate of the image center. 107 * \return y-coordinate of the image center. 108 */ 109 inline float 110 getImageCenterY() const; 111 112 /** \brief Set focal length. 113 * \param[in] focal_length the focal length. 114 */ 115 inline void 116 setFocalLength(const float focal_length); 117 118 /** \brief Get focal length. 119 * \return the focal length. 120 */ 121 inline float 122 getFocalLength() const; 123 124 /** \brief Set baseline. 125 * \param[in] baseline baseline. 126 */ 127 inline void 128 setBaseline(const float baseline); 129 130 /** \brief Get baseline. 131 * \return the baseline. 132 */ 133 inline float 134 getBaseline() const; 135 136 /** \brief Set min disparity threshold. 137 * \param[in] disparity_threshold_min min disparity threshold. 138 */ 139 inline void 140 setDisparityThresholdMin(const float disparity_threshold_min); 141 142 /** \brief Get min disparity threshold. 143 * \return min disparity threshold. 144 */ 145 inline float 146 getDisparityThresholdMin() const; 147 148 /** \brief Set max disparity threshold. 149 * \param[in] disparity_threshold_max max disparity threshold. 150 */ 151 inline void 152 setDisparityThresholdMax(const float disparity_threshold_max); 153 154 /** \brief Get max disparity threshold. 155 * \return max disparity threshold. 156 */ 157 inline float 158 getDisparityThresholdMax() const; 159 160 /** \brief Set an image, that will be used for coloring of the output cloud. 161 * \param[in] image the image. 162 */ 163 void 164 setImage(const pcl::PointCloud<pcl::RGB>::ConstPtr& image); 165 166 /** \brief Get the image, that is used for coloring of the output cloud. 167 * \return the image. 168 */ 169 pcl::PointCloud<RGB>::Ptr 170 getImage(); 171 172 /** \brief Load the disparity map. 173 * \param[in] file_name the name of the disparity map file. 174 * \return "true" if the disparity map was successfully loaded; "false" otherwise 175 */ 176 bool 177 loadDisparityMap(const std::string& file_name); 178 179 /** \brief Load the disparity map and initialize it's size. 180 * \param[in] file_name the name of the disparity map file. 181 * \param[in] width width of the disparity map. 182 * \param[in] height height of the disparity map. 183 * \return "true" if the disparity map was successfully loaded; "false" otherwise 184 */ 185 bool 186 loadDisparityMap(const std::string& file_name, 187 const std::size_t width, 188 const std::size_t height); 189 190 /** \brief Set the disparity map. 191 * \param[in] disparity_map the disparity map. 192 */ 193 void 194 setDisparityMap(const std::vector<float>& disparity_map); 195 196 /** \brief Set the disparity map and initialize it's size. 197 * \param[in] disparity_map the disparity map. 198 * \param[in] width width of the disparity map. 199 * \param[in] height height of the disparity map. 200 * \return "true" if the disparity map was successfully loaded; "false" otherwise 201 */ 202 void 203 setDisparityMap(const std::vector<float>& disparity_map, 204 const std::size_t width, 205 const std::size_t height); 206 207 /** \brief Get the disparity map. 208 * \return the disparity map. 209 */ 210 std::vector<float> 211 getDisparityMap(); 212 213 /** \brief Compute the output cloud. 214 * \param[out] out_cloud the variable to return the resulting cloud. 215 */ 216 virtual void 217 compute(PointCloud& out_cloud); 218 219 protected: 220 /** \brief Translate point from image coordinates and disparity to 3D-coordinates 221 * \param[in] row 222 * \param[in] column 223 * \param[in] disparity 224 * \return the 3D point, that corresponds to the input parametres and the camera 225 * calibration. 226 */ 227 PointXYZ 228 translateCoordinates(std::size_t row, std::size_t column, float disparity) const; 229 230 /** \brief X-coordinate of the image center. */ 231 float center_x_; 232 /** \brief Y-coordinate of the image center. */ 233 float center_y_; 234 /** \brief Focal length. */ 235 float focal_length_; 236 /** \brief Baseline. */ 237 float baseline_; 238 239 /** \brief Is color image is set. */ 240 bool is_color_; 241 /** \brief Color image of the scene. */ 242 pcl::PointCloud<pcl::RGB>::ConstPtr image_; 243 244 /** \brief Vector for the disparity map. */ 245 std::vector<float> disparity_map_; 246 /** \brief X-size of the disparity map. */ 247 std::size_t disparity_map_width_; 248 /** \brief Y-size of the disparity map. */ 249 std::size_t disparity_map_height_; 250 251 /** \brief Thresholds of the disparity. */ 252 float disparity_threshold_min_; 253 float disparity_threshold_max_; 254 }; 255 256 } // namespace pcl 257 258 #include <pcl/stereo/impl/disparity_map_converter.hpp> 259