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>
64 template <
typename Po
intT>
71 template <
typename Po
intT>
78 template <
typename Po
intT>
85 template <
typename Po
intT>
92 template <
typename Po
intT>
96 focal_length_ = focal_length;
99 template <
typename Po
intT>
103 return focal_length_;
106 template <
typename Po
intT>
110 baseline_ = baseline;
113 template <
typename Po
intT>
120 template <
typename Po
intT>
123 const float disparity_threshold_min)
125 disparity_threshold_min_ = disparity_threshold_min;
128 template <
typename Po
intT>
132 return disparity_threshold_min_;
135 template <
typename Po
intT>
138 const float disparity_threshold_max)
140 disparity_threshold_max_ = disparity_threshold_max;
143 template <
typename Po
intT>
147 return disparity_threshold_max_;
150 template <
typename Po
intT>
158 disparity_map_width_ = image_->
width;
159 disparity_map_height_ = image_->height;
164 template <
typename Po
intT>
169 *image_pointer = *image_;
170 return image_pointer;
173 template <
typename Po
intT>
177 std::fstream disparity_file;
180 disparity_file.open(file_name.c_str(), std::fstream::in);
181 if (!disparity_file.is_open()) {
182 PCL_ERROR(
"[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
187 disparity_map_.resize(disparity_map_width_ * disparity_map_height_);
190 for (std::size_t row = 0; row < disparity_map_height_; ++row) {
191 for (std::size_t column = 0; column < disparity_map_width_; ++column) {
193 disparity_file >> disparity;
195 disparity_map_[column + row * disparity_map_width_] = disparity;
202 template <
typename Po
intT>
205 const std::size_t width,
206 const std::size_t height)
209 disparity_map_width_ = width;
210 disparity_map_height_ = height;
213 return loadDisparityMap(file_name);
216 template <
typename Po
intT>
219 const std::vector<float>& disparity_map)
221 disparity_map_ = disparity_map;
224 template <
typename Po
intT>
227 const std::vector<float>& disparity_map,
228 const std::size_t width,
229 const std::size_t height)
231 disparity_map_width_ = width;
232 disparity_map_height_ = height;
234 disparity_map_ = disparity_map;
237 template <
typename Po
intT>
241 return disparity_map_;
244 template <
typename Po
intT>
250 out_cloud.
width = disparity_map_width_;
251 out_cloud.
height = disparity_map_height_;
254 if (is_color_ && !image_) {
255 PCL_ERROR(
"[pcl::DisparityMapConverter::compute] Memory for the image was not "
260 for (std::size_t row = 0; row < disparity_map_height_; ++row) {
261 for (std::size_t column = 0; column < disparity_map_width_; ++column) {
263 std::size_t disparity_point = column + row * disparity_map_width_;
266 float disparity = disparity_map_[disparity_point];
274 intensity_accessor.
set(new_point,
275 static_cast<float>((*image_)[disparity_point].r +
276 (*image_)[disparity_point].g +
277 (*image_)[disparity_point].b) /
282 if (disparity_threshold_min_ < disparity &&
283 disparity < disparity_threshold_max_) {
285 PointXYZ point_3D(translateCoordinates(row, column, disparity));
286 new_point.x = point_3D.x;
287 new_point.y = point_3D.y;
288 new_point.z = point_3D.z;
291 new_point.x = std::numeric_limits<float>::quiet_NaN();
292 new_point.y = std::numeric_limits<float>::quiet_NaN();
293 new_point.z = std::numeric_limits<float>::quiet_NaN();
296 out_cloud[disparity_point] = new_point;
301 template <
typename Po
intT>
305 float disparity)
const
310 if (disparity != 0.0f) {
313 float z_value = focal_length_ * baseline_ / disparity;
314 point_3D.z = z_value;
315 point_3D.x = (
static_cast<float>(column) - center_x_) * (z_value / focal_length_);
316 point_3D.y = (
static_cast<float>(row) - center_y_) * (z_value / focal_length_);
322 #define PCL_INSTANTIATE_DisparityMapConverter(T) \
323 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