37 #ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
38 #define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
40 #include <pcl/stereo/disparity_map_converter.h>
45 #include <pcl/common/intensity.h>
46 #include <pcl/console/print.h>
49 template <
typename Po
intT>
56 , disparity_map_width_(640)
57 , disparity_map_height_(480)
58 , disparity_threshold_min_(0.0f)
59 , disparity_threshold_max_(std::numeric_limits<float>::max())
62 template <
typename Po
intT>
66 template <
typename Po
intT>
73 template <
typename Po
intT>
80 template <
typename Po
intT>
87 template <
typename Po
intT>
94 template <
typename Po
intT>
98 focal_length_ = focal_length;
101 template <
typename Po
intT>
105 return focal_length_;
108 template <
typename Po
intT>
112 baseline_ = baseline;
115 template <
typename Po
intT>
122 template <
typename Po
intT>
125 const float disparity_threshold_min)
127 disparity_threshold_min_ = disparity_threshold_min;
130 template <
typename Po
intT>
134 return disparity_threshold_min_;
137 template <
typename Po
intT>
140 const float disparity_threshold_max)
142 disparity_threshold_max_ = disparity_threshold_max;
145 template <
typename Po
intT>
149 return disparity_threshold_max_;
152 template <
typename Po
intT>
160 disparity_map_width_ = image_->
width;
161 disparity_map_height_ = image_->height;
166 template <
typename Po
intT>
171 *image_pointer = *image_;
172 return image_pointer;
175 template <
typename Po
intT>
179 std::fstream disparity_file;
182 disparity_file.open(file_name.c_str(), std::fstream::in);
183 if (!disparity_file.is_open()) {
184 PCL_ERROR(
"[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
189 disparity_map_.resize(disparity_map_width_ * disparity_map_height_);
192 for (std::size_t row = 0; row < disparity_map_height_; ++row) {
193 for (std::size_t column = 0; column < disparity_map_width_; ++column) {
195 disparity_file >> disparity;
197 disparity_map_[column + row * disparity_map_width_] = disparity;
204 template <
typename Po
intT>
207 const std::size_t width,
208 const std::size_t height)
211 disparity_map_width_ = width;
212 disparity_map_height_ = height;
215 return loadDisparityMap(file_name);
218 template <
typename Po
intT>
221 const std::vector<float>& disparity_map)
223 disparity_map_ = disparity_map;
226 template <
typename Po
intT>
229 const std::vector<float>& disparity_map,
230 const std::size_t width,
231 const std::size_t height)
233 disparity_map_width_ = width;
234 disparity_map_height_ = height;
236 disparity_map_ = disparity_map;
239 template <
typename Po
intT>
243 return disparity_map_;
246 template <
typename Po
intT>
252 out_cloud.
width = disparity_map_width_;
253 out_cloud.
height = disparity_map_height_;
256 if (is_color_ && !image_) {
257 PCL_ERROR(
"[pcl::DisparityMapConverter::compute] Memory for the image was not "
262 for (std::size_t row = 0; row < disparity_map_height_; ++row) {
263 for (std::size_t column = 0; column < disparity_map_width_; ++column) {
265 std::size_t disparity_point = column + row * disparity_map_width_;
268 float disparity = disparity_map_[disparity_point];
276 intensity_accessor.
set(new_point,
277 static_cast<float>(image_->points[disparity_point].r +
278 image_->points[disparity_point].g +
279 image_->points[disparity_point].b) /
284 if (disparity_threshold_min_ < disparity &&
285 disparity < disparity_threshold_max_) {
287 PointXYZ point_3D(translateCoordinates(row, column, disparity));
288 new_point.x = point_3D.x;
289 new_point.y = point_3D.y;
290 new_point.z = point_3D.z;
293 new_point.x = std::numeric_limits<float>::quiet_NaN();
294 new_point.y = std::numeric_limits<float>::quiet_NaN();
295 new_point.z = std::numeric_limits<float>::quiet_NaN();
298 out_cloud[disparity_point] = new_point;
303 template <
typename Po
intT>
307 float disparity)
const
312 if (disparity != 0.0f) {
315 float z_value = focal_length_ * baseline_ / disparity;
316 point_3D.z = z_value;
317 point_3D.x = (static_cast<float>(column) - center_x_) * (z_value / focal_length_);
318 point_3D.y = (static_cast<float>(row) - center_y_) * (z_value / focal_length_);
324 #define PCL_INSTANTIATE_DisparityMapConverter(T) \
325 template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
327 #endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_