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