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>
50 : disparity_threshold_max_(std::numeric_limits<float>::max())
53 template <
typename Po
intT>
56 template <
typename Po
intT>
63 template <
typename Po
intT>
70 template <
typename Po
intT>
77 template <
typename Po
intT>
84 template <
typename Po
intT>
88 focal_length_ = focal_length;
91 template <
typename Po
intT>
98 template <
typename Po
intT>
102 baseline_ = baseline;
105 template <
typename Po
intT>
112 template <
typename Po
intT>
115 const float disparity_threshold_min)
117 disparity_threshold_min_ = disparity_threshold_min;
120 template <
typename Po
intT>
124 return disparity_threshold_min_;
127 template <
typename Po
intT>
130 const float disparity_threshold_max)
132 disparity_threshold_max_ = disparity_threshold_max;
135 template <
typename Po
intT>
139 return disparity_threshold_max_;
142 template <
typename Po
intT>
150 disparity_map_width_ = image_->
width;
151 disparity_map_height_ = image_->height;
156 template <
typename Po
intT>
161 *image_pointer = *image_;
162 return image_pointer;
165 template <
typename Po
intT>
169 std::fstream disparity_file;
172 disparity_file.open(file_name.c_str(), std::fstream::in);
173 if (!disparity_file.is_open()) {
174 PCL_ERROR(
"[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
179 disparity_map_.resize(disparity_map_width_ * disparity_map_height_);
182 for (std::size_t row = 0; row < disparity_map_height_; ++row) {
183 for (std::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>
197 const std::size_t width,
198 const std::size_t height)
201 disparity_map_width_ = width;
202 disparity_map_height_ = height;
205 return loadDisparityMap(file_name);
208 template <
typename Po
intT>
211 const std::vector<float>& disparity_map)
213 disparity_map_ = disparity_map;
216 template <
typename Po
intT>
219 const std::vector<float>& disparity_map,
220 const std::size_t width,
221 const std::size_t height)
223 disparity_map_width_ = width;
224 disparity_map_height_ = height;
226 disparity_map_ = disparity_map;
229 template <
typename Po
intT>
233 return disparity_map_;
236 template <
typename Po
intT>
242 out_cloud.
width = disparity_map_width_;
243 out_cloud.
height = disparity_map_height_;
246 if (is_color_ && !image_) {
247 PCL_ERROR(
"[pcl::DisparityMapConverter::compute] Memory for the image was not "
252 for (std::size_t row = 0; row < disparity_map_height_; ++row) {
253 for (std::size_t column = 0; column < disparity_map_width_; ++column) {
255 std::size_t disparity_point = column + row * disparity_map_width_;
258 float disparity = disparity_map_[disparity_point];
266 intensity_accessor.
set(new_point,
267 static_cast<float>((*image_)[disparity_point].r +
268 (*image_)[disparity_point].g +
269 (*image_)[disparity_point].b) /
274 if (disparity_threshold_min_ < disparity &&
275 disparity < disparity_threshold_max_) {
277 PointXYZ point_3D(translateCoordinates(row, column, disparity));
278 new_point.x = point_3D.x;
279 new_point.y = point_3D.y;
280 new_point.z = point_3D.z;
283 new_point.x = std::numeric_limits<float>::quiet_NaN();
284 new_point.y = std::numeric_limits<float>::quiet_NaN();
285 new_point.z = std::numeric_limits<float>::quiet_NaN();
288 out_cloud[disparity_point] = new_point;
293 template <
typename Po
intT>
297 float disparity)
const
302 if (disparity != 0.0f) {
305 float z_value = focal_length_ * baseline_ / disparity;
306 point_3D.z = z_value;
307 point_3D.x = (
static_cast<float>(column) - center_x_) * (z_value / focal_length_);
308 point_3D.y = (
static_cast<float>(row) - center_y_) * (z_value / focal_length_);
314 #define PCL_INSTANTIATE_DisparityMapConverter(T) \
315 template class PCL_EXPORTS pcl::DisparityMapConverter<T>;
317 #endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_
shared_ptr< PointCloud< PointT > > Ptr
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.
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
float getDisparityThresholdMax() const
Get max disparity threshold.
std::uint32_t width
The point cloud width (if organized as an image-structure).
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.
void setDisparityThresholdMin(const float disparity_threshold_min)
Set min disparity threshold.
A point structure representing Euclidean xyz coordinates.
void clear()
Removes all points in a cloud and sets the width and height to 0.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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.
float getImageCenterY() const
Get y-coordinate of the image center.
void setImageCenterY(const float center_y)
Set y-coordinate of the image center.
shared_ptr< const PointCloud< PointT > > ConstPtr
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.
pcl::PointCloud< RGB >::Ptr getImage()
Get the image, that is used for coloring of the output cloud.