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>
43 #include <pcl/point_types.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_->points[disparity_point].r +
277 image_->points[disparity_point].g +
278 image_->points[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_
void setFocalLength(const float focal_length)
Set focal length.
float getDisparityThresholdMin() const
Get min disparity threshold.
float getFocalLength() const
Get focal length.
bool loadDisparityMap(const std::string &file_name)
Load the disparity map.
void setDisparityMap(const std::vector< float > &disparity_map)
Set the disparity map.
virtual void compute(PointCloud &out_cloud)
Compute the output cloud.
DisparityMapConverter()
DisparityMapConverter constructor.
std::vector< float > getDisparityMap()
Get the disparity map.
std::uint32_t width
The point cloud width (if organized as an image-structure).
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
float getDisparityThresholdMax() const
Get max disparity threshold.
float getImageCenterX() const
Get x-coordinate of the image center.
virtual ~DisparityMapConverter()
Empty destructor.
PointXYZ translateCoordinates(std::size_t row, std::size_t column, float disparity) const
Translate point from image coordinates and disparity to 3D-coordinates.
shared_ptr< PointCloud< PointT > > Ptr
void setDisparityThresholdMin(const float disparity_threshold_min)
Set min disparity threshold.
A point structure representing Euclidean xyz coordinates.
void set(PointT &p, float intensity) const
sets the intensity value of a point
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float getBaseline() const
Get baseline.
void setBaseline(const float baseline)
Set baseline.
shared_ptr< const PointCloud< PointT > > ConstPtr
float getImageCenterY() const
Get y-coordinate of the image center.
void setImageCenterY(const float center_y)
Set y-coordinate of the image center.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void setImage(const pcl::PointCloud< pcl::RGB >::ConstPtr &image)
Set an image, that will be used for coloring of the output cloud.
void setImageCenterX(const float center_x)
Set x-coordinate of the image center.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void resize(std::size_t n)
Resize the cloud.
void clear()
Removes all points in a cloud and sets the width and height to 0.
pcl::PointCloud< RGB >::Ptr getImage()
Get the image, that is used for coloring of the output cloud.