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 
48 class MultiLayerDepthException: public std::exception
49 {
50 public:
51  MultiLayerDepthException(const std::string& error_msg) :
52  std::exception(), error_msg_(error_msg) {
53  }
54  virtual ~MultiLayerDepthException() throw () {}
55 
56  virtual const char * what() const throw () {
57  return error_msg_.c_str();
58  }
59 
60 protected:
61  std::string error_msg_;
62 };
63 
65 {
66 public:
68  shadow_time_out_(30.0),
69  shadow_distance_(0.01)
70  {};
71  virtual ~MultiLayerDepth() {
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 generatePointCloudFromDepth (sensor_msgs::ImageConstPtr depth_msg,
86  sensor_msgs::ImageConstPtr color_msg,
87  sensor_msgs::CameraInfoConstPtr camera_info_msg);
88  void reset()
89  {
90  if (occlusion_compensation_)
91  {
92  // reset shadow buffer
93  memset(&shadow_depth_[0], 0, sizeof(float)*shadow_depth_.size());
94  memset(&shadow_buffer_[0], 0, sizeof(uint8_t)*shadow_buffer_.size());
95  memset(&shadow_timestamp_[0], 0, sizeof(double)*shadow_timestamp_.size());
96  }
97  }
98 
99 protected:
101  void initializeConversion(const sensor_msgs::ImageConstPtr& depth_msg,
102  sensor_msgs::CameraInfoConstPtr& camera_info_msg);
103 
105  template<typename T>
106  void convertColor(const sensor_msgs::ImageConstPtr& color_msg,
107  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 
138 }
139 #endif /* MULTI_LAYER_DEPTH_H_ */
std::vector< double > shadow_timestamp_
void enableOcclusionCompensation(bool occlusion_compensation)
MultiLayerDepthException(const std::string &error_msg)
std::vector< uint8_t > shadow_buffer_
virtual const char * what() const
std::vector< float > projection_map_y_
std::vector< float > projection_map_x_
void setShadowTimeOut(double time_out)
std::vector< float > shadow_depth_


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:50