36 #ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_ 37 #define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_ 39 #include <pcl/stereo/disparity_map_converter.h> 45 #include <pcl/console/print.h> 46 #include <pcl/common/intensity.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 ())
62 template <
typename Po
intT>
67 template <
typename Po
intT>
inline void 73 template <
typename Po
intT>
inline float 79 template <
typename Po
intT>
inline void 85 template <
typename Po
intT>
inline float 91 template <
typename Po
intT>
inline void 94 focal_length_ = focal_length;
97 template <
typename Po
intT>
inline float 100 return focal_length_;
103 template <
typename Po
intT>
inline void 106 baseline_ = baseline;
109 template <
typename Po
intT>
inline float 115 template <
typename Po
intT>
inline void 117 const float disparity_threshold_min)
119 disparity_threshold_min_ = disparity_threshold_min;
122 template <
typename Po
intT>
inline float 125 return disparity_threshold_min_;
128 template <
typename Po
intT>
inline void 130 const float disparity_threshold_max)
132 disparity_threshold_max_ = disparity_threshold_max;
135 template <
typename Po
intT>
inline float 138 return disparity_threshold_max_;
141 template <
typename Po
intT>
void 148 disparity_map_width_ = image_->
width;
149 disparity_map_height_ = image_->height;
158 *image_pointer = *image_;
159 return image_pointer;
162 template <
typename Po
intT>
bool 164 const std::string &file_name)
166 std::fstream disparity_file;
169 disparity_file.open (file_name.c_str (), std::fstream::in);
170 if (!disparity_file.is_open ())
172 PCL_ERROR (
"[pcl::DisparityMapConverter::loadDisparityMap] Can't load the file.\n");
177 disparity_map_.resize (disparity_map_width_ * disparity_map_height_);
180 for (
size_t row = 0; row < disparity_map_height_; ++row)
182 for (
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>
bool 196 const std::string &file_name,
const size_t width,
const size_t height)
199 disparity_map_width_ = width;
200 disparity_map_height_ = height;
203 return loadDisparityMap (file_name);
206 template <
typename Po
intT>
void 208 const std::vector<float> &disparity_map)
210 disparity_map_ = disparity_map;
213 template <
typename Po
intT>
void 215 const std::vector<float> &disparity_map,
216 const size_t width,
const size_t height)
218 disparity_map_width_ = width;
219 disparity_map_height_ = height;
221 disparity_map_ = disparity_map;
224 template <
typename Po
intT> std::vector<float>
227 return disparity_map_;
230 template <
typename Po
intT>
void 235 out_cloud.
width = disparity_map_width_;
236 out_cloud.
height = disparity_map_height_;
239 if (is_color_ && !image_)
241 PCL_ERROR (
"[pcl::DisparityMapConverter::compute] Memory for the image was not allocated.\n");
245 for (
size_t row = 0; row < disparity_map_height_; ++row)
247 for (
size_t column = 0; column < disparity_map_width_; ++column)
250 size_t disparity_point = column + row * disparity_map_width_;
253 float disparity = disparity_map_[disparity_point];
262 intensity_accessor.
set (new_point, static_cast<float> (
263 image_->points[disparity_point].r +
264 image_->points[disparity_point].g +
265 image_->points[disparity_point].b) / 3.0f);
269 if (disparity_threshold_min_ < disparity && disparity < disparity_threshold_max_)
272 PointXYZ point_3D (translateCoordinates (row, column, disparity));
273 new_point.x = point_3D.x;
274 new_point.y = point_3D.y;
275 new_point.z = point_3D.z;
279 new_point.x = std::numeric_limits<float>::quiet_NaN ();
280 new_point.y = std::numeric_limits<float>::quiet_NaN ();
281 new_point.z = std::numeric_limits<float>::quiet_NaN ();
284 out_cloud[disparity_point] = new_point;
291 size_t row,
size_t column,
float disparity)
const 296 if (disparity != 0.0f)
299 float z_value = focal_length_ * baseline_ / disparity;
300 point_3D.z = z_value;
301 point_3D.x = (
static_cast<float> (column) - center_x_) * (z_value / focal_length_);
302 point_3D.y = (
static_cast<float> (row) - center_y_) * (z_value / focal_length_);
308 #define PCL_INSTANTIATE_DisparityMapConverter(T) template class PCL_EXPORTS pcl::DisparityMapConverter<T>; 310 #endif // PCL_DISPARITY_MAP_CONVERTER_IMPL_H_ void setFocalLength(const float focal_length)
Set focal length.
float getImageCenterX() const
Get x-coordinate of the image center.
float getBaseline() const
Get baseline.
float getFocalLength() const
Get focal length.
bool loadDisparityMap(const std::string &file_name)
Load the disparity map.
float getDisparityThresholdMin() const
Get min disparity threshold.
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.
PointXYZ translateCoordinates(size_t row, size_t column, float disparity) const
Translate point from image coordinates and disparity to 3D-coordinates.
void setDisparityThresholdMax(const float disparity_threshold_max)
Set max disparity threshold.
uint32_t height
The point cloud height (if organized as an image-structure).
boost::shared_ptr< PointCloud< PointT > > Ptr
virtual ~DisparityMapConverter()
Empty destructor.
uint32_t width
The point cloud width (if organized as an image-structure).
Defines all the PCL implemented PointT point type structures.
void setDisparityThresholdMin(const float disparity_threshold_min)
Set min disparity threshold.
A point structure representing Euclidean xyz coordinates.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
void clear()
Removes all points in a cloud and sets the width and height to 0.
float getImageCenterY() const
Get y-coordinate of the image center.
float getDisparityThresholdMax() const
Get max disparity threshold.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setBaseline(const float baseline)
Set baseline.
void setImageCenterY(const float center_y)
Set y-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.
void resize(size_t n)
Resize the 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 set(PointT &p, float intensity) const
sets the intensity value of a point
pcl::PointCloud< RGB >::Ptr getImage()
Get the image, that is used for coloring of the output cloud.