depth_cloud_mld.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: Jan 22, 2013
30  * Author: jkammerl
31  */
32 
33 #ifndef RVIZ_MULTI_LAYER_DEPTH_H_
34 #define RVIZ_MULTI_LAYER_DEPTH_H_
35 
36 #include <boost/shared_ptr.hpp>
37 #include <boost/thread/mutex.hpp>
38 
39 #include <sensor_msgs/Image.h>
40 #include <sensor_msgs/PointCloud2.h>
41 
42 #include <vector>
43 #include <exception>
44 
45 namespace rviz
46 {
47 class MultiLayerDepthException : public std::exception
48 {
49 public:
50  MultiLayerDepthException(const std::string& error_msg) : std::exception(), error_msg_(error_msg)
51  {
52  }
53  ~MultiLayerDepthException() throw() override
54  {
55  }
56 
57  const char* what() const throw() override
58  {
59  return error_msg_.c_str();
60  }
61 
62 protected:
63  std::string error_msg_;
64 };
65 
67 {
68 public:
70  virtual ~MultiLayerDepth()
71  {
72  }
73 
74  void setShadowTimeOut(double time_out)
75  {
76  shadow_time_out_ = time_out;
77  }
78 
79  void enableOcclusionCompensation(bool occlusion_compensation)
80  {
81  occlusion_compensation_ = occlusion_compensation;
82  reset();
83  }
84 
85  sensor_msgs::PointCloud2Ptr
86  generatePointCloudFromDepth(const sensor_msgs::ImageConstPtr& depth_msg,
87  const sensor_msgs::ImageConstPtr& color_msg,
88  sensor_msgs::CameraInfoConstPtr camera_info_msg);
89  void reset()
90  {
92  {
93  // reset shadow buffer
94  memset(&shadow_depth_[0], 0, sizeof(float) * shadow_depth_.size());
95  memset(&shadow_buffer_[0], 0, sizeof(uint8_t) * shadow_buffer_.size());
96  memset(&shadow_timestamp_[0], 0, sizeof(double) * shadow_timestamp_.size());
97  }
98  }
99 
100 protected:
102  void initializeConversion(const sensor_msgs::ImageConstPtr& depth_msg,
103  sensor_msgs::CameraInfoConstPtr& camera_info_msg);
104 
106  template <typename T>
107  void convertColor(const sensor_msgs::ImageConstPtr& color_msg, std::vector<uint32_t>& rgba_color_raw);
108 
110  template <typename T>
111  sensor_msgs::PointCloud2Ptr generatePointCloudSL(const sensor_msgs::ImageConstPtr& depth_msg,
112  std::vector<uint32_t>& rgba_color_raw);
113 
115  template <typename T>
116  sensor_msgs::PointCloud2Ptr generatePointCloudML(const sensor_msgs::ImageConstPtr& depth_msg,
117  std::vector<uint32_t>& rgba_color_raw);
118 
119  // Helpers to generate pointcloud2 message
120  sensor_msgs::PointCloud2Ptr initPointCloud();
121  void finalizingPointCloud(sensor_msgs::PointCloud2Ptr& point_cloud, std::size_t size);
122 
123  std::vector<float> projection_map_x_;
124  std::vector<float> projection_map_y_;
125 
126  // shadow buffers
127  std::vector<float> shadow_depth_;
128  std::vector<double> shadow_timestamp_;
129  std::vector<uint8_t> shadow_buffer_;
130 
131  // configuration
135 };
136 
137 } // namespace rviz
138 #endif /* MULTI_LAYER_DEPTH_H_ */
rviz::MultiLayerDepth::convertColor
void convertColor(const sensor_msgs::ImageConstPtr &color_msg, std::vector< uint32_t > &rgba_color_raw)
Convert color data to RGBA format.
Definition: depth_cloud_mld.cpp:386
rviz::MultiLayerDepth::generatePointCloudFromDepth
sensor_msgs::PointCloud2Ptr generatePointCloudFromDepth(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &color_msg, sensor_msgs::CameraInfoConstPtr camera_info_msg)
Definition: depth_cloud_mld.cpp:454
rviz::MultiLayerDepthException::MultiLayerDepthException
MultiLayerDepthException(const std::string &error_msg)
Definition: depth_cloud_mld.h:50
rviz::MultiLayerDepth::shadow_time_out_
double shadow_time_out_
Definition: depth_cloud_mld.h:133
rviz::MultiLayerDepth::generatePointCloudSL
sensor_msgs::PointCloud2Ptr generatePointCloudSL(const sensor_msgs::ImageConstPtr &depth_msg, std::vector< uint32_t > &rgba_color_raw)
Generate single-layered depth cloud (depth only)
Definition: depth_cloud_mld.cpp:174
rviz::MultiLayerDepth::shadow_depth_
std::vector< float > shadow_depth_
Definition: depth_cloud_mld.h:127
rviz::MultiLayerDepth::shadow_distance_
float shadow_distance_
Definition: depth_cloud_mld.h:134
rviz::MultiLayerDepthException::what
const char * what() const override
Definition: depth_cloud_mld.h:57
rviz::MultiLayerDepth
Definition: depth_cloud_mld.h:66
rviz::MultiLayerDepth::enableOcclusionCompensation
void enableOcclusionCompensation(bool occlusion_compensation)
Definition: depth_cloud_mld.h:79
rviz::MultiLayerDepth::shadow_buffer_
std::vector< uint8_t > shadow_buffer_
Definition: depth_cloud_mld.h:129
rviz::MultiLayerDepth::reset
void reset()
Definition: depth_cloud_mld.h:89
rviz::MultiLayerDepthException
Definition: depth_cloud_mld.h:47
rviz
Definition: add_display_dialog.cpp:54
rviz::MultiLayerDepth::shadow_timestamp_
std::vector< double > shadow_timestamp_
Definition: depth_cloud_mld.h:128
rviz::MultiLayerDepth::projection_map_x_
std::vector< float > projection_map_x_
Definition: depth_cloud_mld.h:123
rviz::MultiLayerDepth::~MultiLayerDepth
virtual ~MultiLayerDepth()
Definition: depth_cloud_mld.h:70
rviz::MultiLayerDepth::initializeConversion
void initializeConversion(const sensor_msgs::ImageConstPtr &depth_msg, sensor_msgs::CameraInfoConstPtr &camera_info_msg)
Precompute projection matrix, initialize buffers.
Definition: depth_cloud_mld.cpp:90
rviz::MultiLayerDepth::projection_map_y_
std::vector< float > projection_map_y_
Definition: depth_cloud_mld.h:124
std
rviz::MultiLayerDepth::setShadowTimeOut
void setShadowTimeOut(double time_out)
Definition: depth_cloud_mld.h:74
rviz::MultiLayerDepth::MultiLayerDepth
MultiLayerDepth()
Definition: depth_cloud_mld.h:69
rviz::MultiLayerDepth::initPointCloud
sensor_msgs::PointCloud2Ptr initPointCloud()
Definition: depth_cloud_mld.cpp:543
rviz::MultiLayerDepth::occlusion_compensation_
bool occlusion_compensation_
Definition: depth_cloud_mld.h:132
rviz::MultiLayerDepthException::error_msg_
std::string error_msg_
Definition: depth_cloud_mld.h:63
rviz::MultiLayerDepth::finalizingPointCloud
void finalizingPointCloud(sensor_msgs::PointCloud2Ptr &point_cloud, std::size_t size)
Definition: depth_cloud_mld.cpp:583
rviz::MultiLayerDepth::generatePointCloudML
sensor_msgs::PointCloud2Ptr generatePointCloudML(const sensor_msgs::ImageConstPtr &depth_msg, std::vector< uint32_t > &rgba_color_raw)
Generate multi-layered depth cloud (depth+shadow)
Definition: depth_cloud_mld.cpp:253
rviz::MultiLayerDepthException::~MultiLayerDepthException
~MultiLayerDepthException() override
Definition: depth_cloud_mld.h:53


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:09