Point Cloud Library (PCL)  1.8.1
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_RAYCASTER_H_
40 #define PCL_KINFU_TSDF_RAYCASTER_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/pixel_rgb.h>
47 #include <boost/shared_ptr.hpp>
48 #include <Eigen/Geometry>
49 
50 namespace pcl
51 {
52  namespace gpu
53  {
54  class TsdfVolume;
55 
56  /** \brief Class that performs raycasting for TSDF volume
57  * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
58  */
59  struct PCL_EXPORTS RayCaster
60  {
61  public:
62  typedef boost::shared_ptr<RayCaster> Ptr;
66 
67  /** \brief Image with height */
68  const int cols, rows;
69 
70  /** \brief Constructor
71  * \param[in] rows image rows
72  * \param[in] cols image cols
73  * \param[in] fx focal x
74  * \param[in] fy focal y
75  * \param[in] cx principal point x
76  * \param[in] cy principal point y
77  */
78  RayCaster(int rows = 480, int cols = 640, float fx = 525.f, float fy = 525.f, float cx = -1, float cy = -1);
79  ~RayCaster();
80 
81  /** \brief Sets camera intrinsics */
82  void
83  setIntrinsics(float fx = 525.f, float fy = 525.f, float cx = -1, float cy = -1);
84 
85  /** \brief Runs raycasting algorithm from given camera pose. It writes results to internal fiels.
86  * \param[in] volume tsdf volume container
87  * \param[in] camera_pose camera pose
88  */
89  void
90  run(const TsdfVolume& volume, const Eigen::Affine3f& camera_pose);
91 
92  /** \brief Generates scene view using data raycasted by run method. So call it before.
93  * \param[out] view output array for RGB image
94  */
95  void
96  generateSceneView(View& view) const;
97 
98  /** \brief Generates scene view using data raycasted by run method. So call it before.
99  * \param[out] view output array for RGB image
100  * \param[in] light_source_pose pose of light source
101  */
102  void
103  generateSceneView(View& view, const Eigen::Vector3f& light_source_pose) const;
104 
105  /** \brief Generates depth image using data raycasted by run method. So call it before.
106  * \param[out] depth output array for depth image
107  */
108  void
109  generateDepthImage(Depth& depth) const;
110 
111  /** \brief Returns raycasterd vertex map. */
112  MapArr
113  getVertexMap() const;
114 
115  /** \brief Returns raycasterd normal map. */
116  MapArr
117  getNormalMap() const;
118 
119  private:
120  /** \brief Camera intrinsics. */
121  float fx_, fy_, cx_, cy_;
122 
123  /* Vertext/normal map internal representation example for rows=2 and cols=4
124  * X X X X
125  * X X X X
126  * Y Y Y Y
127  * Y Y Y Y
128  * Z Z Z Z
129  * Z Z Z Z
130  */
131 
132  /** \brief vertex map of 3D points*/
133  MapArr vertex_map_;
134 
135  /** \brief normal map of 3D points*/
136  MapArr normal_map_;
137 
138  /** \brief camera pose from which raycasting was done */
139  Eigen::Affine3f camera_pose_;
140 
141  /** \brief Last passed volume size */
142  Eigen::Vector3f volume_size_;
143 
144 public:
145 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146  };
147 
148  /** \brief Converts from map representation to organized not-dence point cloud. */
149  template<typename PointType>
151  }
152 }
153 
154 #endif /* PCL_KINFU_TSDF_RAYCASTER_H_ */
DeviceArray2D< PixelRGB > View
Definition: raycaster.h:64
Class that performs raycasting for TSDF volume.
Definition: raycaster.h:59
boost::shared_ptr< RayCaster > Ptr
Definition: raycaster.h:62
const int rows
Definition: raycaster.h:68
DeviceArray2D< float > MapArr
Definition: raycaster.h:63
TsdfVolume class.
Definition: tsdf_volume.h:55
void convertMapToOranizedCloud(const RayCaster::MapArr &map, DeviceArray2D< PointType > &cloud)
Converts from map representation to organized not-dence point cloud.
Defines all the PCL implemented PointT point type structures.
DeviceArray2D< unsigned short > Depth
Definition: raycaster.h:65