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