Point Cloud Library (PCL)  1.8.0
raycaster.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 
39 #ifndef PCL_KINFU_TSDF_RAYCASTERLS_H_
40 #define PCL_KINFU_TSDF_RAYCASTERLS_H_
41 
42 
43 #include <pcl/pcl_macros.h>
44 #include <pcl/point_types.h>
45 #include <pcl/gpu/containers/device_array.h>
46 #include <pcl/gpu/kinfu_large_scale/pixel_rgb.h>
47 #include <boost/shared_ptr.hpp>
48 //#include <boost/graph/buffer_concepts.hpp>
49 #include <Eigen/Geometry>
50 
51 #include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
52 
53 namespace pcl
54 {
55  namespace gpu
56  {
57  namespace kinfuLS
58  {
59  class TsdfVolume;
60 
61  /** \brief Class that performs raycasting for TSDF volume
62  * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
63  */
64  struct PCL_EXPORTS RayCaster
65  {
66  public:
67  typedef boost::shared_ptr<RayCaster> Ptr;
71 
72  /** \brief Image with height */
73  const int cols, rows;
74 
75  /** \brief Constructor
76  * \param[in] rows image rows
77  * \param[in] cols image cols
78  * \param[in] fx focal x
79  * \param[in] fy focal y
80  * \param[in] cx principal point x
81  * \param[in] cy principal point y
82  */
83  RayCaster(int rows = 480, int cols = 640, float fx = 525.f, float fy = 525.f, float cx = -1, float cy = -1);
84 
85  ~RayCaster();
86 
87  /** \brief Sets camera intrinsics */
88  void
89  setIntrinsics(float fx = 525.f, float fy = 525.f, float cx = -1, float cy = -1);
90 
91  /** \brief Runs raycasting algorithm from given camera pose. It writes results to internal fiels.
92  * \param[in] volume tsdf volume container
93  * \param[in] camera_pose camera pose
94  * \param buffer
95  */
96  void
97  run(const TsdfVolume& volume, const Eigen::Affine3f& camera_pose, tsdf_buffer* buffer);
98 
99  /** \brief Generates scene view using data raycasted by run method. So call it before.
100  * \param[out] view output array for RGB image
101  */
102  void
103  generateSceneView(View& view) const;
104 
105  /** \brief Generates scene view using data raycasted by run method. So call it before.
106  * \param[out] view output array for RGB image
107  * \param[in] light_source_pose pose of light source
108  */
109  void
110  generateSceneView(View& view, const Eigen::Vector3f& light_source_pose) const;
111 
112  /** \brief Generates depth image using data raycasted by run method. So call it before.
113  * \param[out] depth output array for depth image
114  */
115  void
116  generateDepthImage(Depth& depth) const;
117 
118  /** \brief Returns raycasterd vertex map. */
119  MapArr
120  getVertexMap() const;
121 
122  /** \brief Returns raycasterd normal map. */
123  MapArr
124  getNormalMap() const;
125 
126  private:
127  /** \brief Camera intrinsics. */
128  float fx_, fy_, cx_, cy_;
129 
130  /* Vertext/normal map internal representation example for rows=2 and cols=4
131  * X X X X
132  * X X X X
133  * Y Y Y Y
134  * Y Y Y Y
135  * Z Z Z Z
136  * Z Z Z Z
137  */
138 
139  /** \brief vertex map of 3D points*/
140  MapArr vertex_map_;
141 
142  /** \brief normal map of 3D points*/
143  MapArr normal_map_;
144 
145  /** \brief camera pose from which raycasting was done */
146  Eigen::Affine3f camera_pose_;
147 
148  /** \brief Last passed volume size */
149  Eigen::Vector3f volume_size_;
150 
151 public:
152 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153 
154  };
155 
156  /** \brief Converts from map representation to organized not-dence point cloud. */
157  template<typename PointType>
159  }
160  }
161 }
162 
163 #endif /* PCL_KINFU_TSDF_RAYCASTER_H_ */
pcl::gpu::DeviceArray2D< PixelRGB > View
Definition: raycaster.h:69
Structure to handle buffer addresses.
Definition: tsdf_buffer.h:51
pcl::gpu::DeviceArray2D< float > MapArr
Definition: raycaster.h:68
void convertMapToOranizedCloud(const RayCaster::MapArr &map, pcl::gpu::DeviceArray2D< PointType > &cloud)
Converts from map representation to organized not-dence point cloud.
TsdfVolume class.
Definition: tsdf_volume.h:62
pcl::gpu::DeviceArray2D< unsigned short > Depth
Definition: raycaster.h:70
Class that performs raycasting for TSDF volume.
Definition: raycaster.h:64
boost::shared_ptr< RayCaster > Ptr
Definition: raycaster.h:67