37 #ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
38 #define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
40 #include <pcl/common/intensity.h>
41 #include <pcl/console/print.h>
42 #include <pcl/stereo/disparity_map_converter.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())
61 template <
typename Po
intT>
65 template <
typename Po
intT>
72 template <
typename Po
intT>
79 template <
typename Po
intT>
86 template <
typename Po
intT>
93 template <
typename Po
intT>
97 focal_length_ = focal_length;
100 template <
typename Po
intT>
104 return focal_length_;
107 template <
typename Po
intT>
111 baseline_ = baseline;
114 template <
typename Po
intT>
121 template <
typename Po
intT>
124 const float disparity_threshold_min)
126 disparity_threshold_min_ = disparity_threshold_min;
129 template <
typename Po
intT>
133 return disparity_threshold_min_;
136 template <
typename Po
intT>
139 const float disparity_threshold_max)
141 disparity_threshold_max_ = disparity_threshold_max;
144 template <
typename Po
intT>
148 return disparity_threshold_max_;
151 template <
typename Po
intT>
159 disparity_map_width_ = image_->
width;
160 disparity_map_height_ = image_->height;
165 template <
typename Po
intT>
170 *image_pointer = *image_;
171 return image_pointer;
174 template <
typename Po
intT>
178 std::fstream disparity_file;
181 disparity_file.open(file_name.c_str(), std::fstream::in);
182 if (!disparity_file.is_open()) {
183 PCL_ERROR(
"[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
188 disparity_map_.resize(disparity_map_width_ * disparity_map_height_);
191 for (std::size_t row = 0; row < disparity_map_height_; ++row) {
192 for (std::size_t column = 0; column < disparity_map_width_; ++column) {
194 disparity_file >> disparity;
196 disparity_map_[column + row * disparity_map_width_] = disparity;
203 template <
typename Po
intT>
206 const std::size_t width,
207 const std::size_t height)
210 disparity_map_width_ = width;
211 disparity_map_height_ = height;
214 return loadDisparityMap(file_name);
217 template <
typename Po
intT>
220 const std::vector<float>& disparity_map)
222 disparity_map_ = disparity_map;
225 template <
typename Po
intT>
228 const std::vector<float>& disparity_map,
229 const std::size_t width,
230 const std::size_t height)
232 disparity_map_width_ = width;
233 disparity_map_height_ = height;
235 disparity_map_ = disparity_map;
238 template <
typename Po
intT>
242 return disparity_map_;
245 template <
typename Po
intT>
251 out_cloud.
width = disparity_map_width_;
252 out_cloud.
height = disparity_map_height_;
255 if (is_color_ && !image_) {
256 PCL_ERROR(
"[pcl::DisparityMapConverter::compute] Memory for the image was not "
261 for (std::size_t row = 0; row < disparity_map_height_; ++row) {
262 for (std::size_t column = 0; column < disparity_map_width_; ++column) {
264 std::size_t disparity_point = column + row * disparity_map_width_;
267 float disparity = disparity_map_[disparity_point];
275 intensity_accessor.
set(new_point,
276 static_cast<float>((*image_)[disparity_point].r +
277 (*image_)[disparity_point].g +
278 (*image_)[disparity_point].b) /
283 if (disparity_threshold_min_ < disparity &&
284 disparity < disparity_threshold_max_) {
286 PointXYZ point_3D(translateCoordinates(row, column, disparity));
287 new_point.x = point_3D.x;
288 new_point.y = point_3D.y;
289 new_point.z = point_3D.z;
292 new_point.x = std::numeric_limits<float>::quiet_NaN();
293 new_point.y = std::numeric_limits<float>::quiet_NaN();
294 new_point.z = std::numeric_limits<float>::quiet_NaN();
297 out_cloud[disparity_point] = new_point;
302 template <
typename Po
intT>
306 float disparity)
const
311 if (disparity != 0.0f) {
314 float z_value = focal_length_ * baseline_ / disparity;
315 point_3D.z = z_value;
316 point_3D.x = (
static_cast<float>(column) - center_x_) * (z_value / focal_length_);
317 point_3D.y = (
static_cast<float>(row) - center_y_) * (z_value / focal_length_);
323 #define PCL_INSTANTIATE_DisparityMapConverter(T) \
324 template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
326 #endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_