36 #ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
37 #define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
39 #include <pcl/stereo/disparity_map_converter.h>
45 #include <pcl/console/print.h>
46 #include <pcl/common/intensity.h>
48 template <
typename Po
intT>
55 disparity_map_width_ (640),
56 disparity_map_height_ (480),
57 disparity_threshold_min_ (0.0f),
58 disparity_threshold_max_ (
std::numeric_limits<float>::max ())
62 template <
typename Po
intT>
67 template <
typename Po
intT>
inline void
73 template <
typename Po
intT>
inline float
79 template <
typename Po
intT>
inline void
85 template <
typename Po
intT>
inline float
91 template <
typename Po
intT>
inline void
94 focal_length_ = focal_length;
97 template <
typename Po
intT>
inline float
100 return focal_length_;
103 template <
typename Po
intT>
inline void
106 baseline_ = baseline;
109 template <
typename Po
intT>
inline float
115 template <
typename Po
intT>
inline void
117 const float disparity_threshold_min)
119 disparity_threshold_min_ = disparity_threshold_min;
122 template <
typename Po
intT>
inline float
125 return disparity_threshold_min_;
128 template <
typename Po
intT>
inline void
130 const float disparity_threshold_max)
132 disparity_threshold_max_ = disparity_threshold_max;
135 template <
typename Po
intT>
inline float
138 return disparity_threshold_max_;
141 template <
typename Po
intT>
void
148 disparity_map_width_ = image_->
width;
149 disparity_map_height_ = image_->height;
158 *image_pointer = *image_;
159 return image_pointer;
162 template <
typename Po
intT>
bool
164 const std::string &file_name)
166 std::fstream disparity_file;
169 disparity_file.open (file_name.c_str (), std::fstream::in);
170 if (!disparity_file.is_open ())
172 PCL_ERROR (
"[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
177 disparity_map_.resize (disparity_map_width_ * disparity_map_height_);
180 for (
size_t row = 0; row < disparity_map_height_; ++row)
182 for (
size_t column = 0; column < disparity_map_width_; ++column)
185 disparity_file >> disparity;
187 disparity_map_[column + row * disparity_map_width_] = disparity;
194 template <
typename Po
intT>
bool
196 const std::string &file_name,
const size_t width,
const size_t height)
199 disparity_map_width_ = width;
200 disparity_map_height_ = height;
203 return loadDisparityMap (file_name);
206 template <
typename Po
intT>
void
208 const std::vector<float> &disparity_map)
210 disparity_map_ = disparity_map;
213 template <
typename Po
intT>
void
215 const std::vector<float> &disparity_map,
216 const size_t width,
const size_t height)
218 disparity_map_width_ = width;
219 disparity_map_height_ = height;
221 disparity_map_ = disparity_map;
224 template <
typename Po
intT> std::vector<float>
227 return disparity_map_;
230 template <
typename Po
intT>
void
235 out_cloud.
width = disparity_map_width_;
236 out_cloud.
height = disparity_map_height_;
239 if (is_color_ && !image_)
241 PCL_ERROR (
"[pcl::DisparityMapConverter::compute] Memory for the image was not allocated.\n");
245 for (
size_t row = 0; row < disparity_map_height_; ++row)
247 for (
size_t column = 0; column < disparity_map_width_; ++column)
250 size_t disparity_point = column + row * disparity_map_width_;
253 float disparity = disparity_map_[disparity_point];
262 intensity_accessor.
set (new_point,
static_cast<float> (
263 image_->points[disparity_point].r +
264 image_->points[disparity_point].g +
265 image_->points[disparity_point].b) / 3.0f);
269 if (disparity_threshold_min_ < disparity && disparity < disparity_threshold_max_)
272 PointXYZ point_3D (translateCoordinates (row, column, disparity));
273 new_point.x = point_3D.x;
274 new_point.y = point_3D.y;
275 new_point.z = point_3D.z;
279 new_point.x = std::numeric_limits<float>::quiet_NaN ();
280 new_point.y = std::numeric_limits<float>::quiet_NaN ();
281 new_point.z = std::numeric_limits<float>::quiet_NaN ();
284 out_cloud[disparity_point] = new_point;
291 size_t row,
size_t column,
float disparity)
const
296 if (disparity != 0.0f)
299 float z_value = focal_length_ * baseline_ / disparity;
300 point_3D.z = z_value;
301 point_3D.x = (
static_cast<float> (column) - center_x_) * (z_value / focal_length_);
302 point_3D.y = (
static_cast<float> (row) - center_y_) * (z_value / focal_length_);
308 #define PCL_INSTANTIATE_DisparityMapConverter(T) template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
310 #endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_