iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
DepthToCloudImpl.H
Go to the documentation of this file.
1 /*! @file
2  @author Shane Grant
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 #ifndef INCLUDE_NRT_POINTCLOUD2_IO_DEPTHTOCLOUD_DETAILS_DEPTHTOCLOUDIMPL_H
36 #define INCLUDE_NRT_POINTCLOUD2_IO_DEPTHTOCLOUD_DETAILS_DEPTHTOCLOUDIMPL_H
37 
38 //! @cond PRIVATE_NEVERDEFINED
39 template <class T> inline
40 auto nrt::depthToPointCloud( Image<PixGray<T>> const depthImage, float focalLength, const bool keepNaN ) -> PointCloud2
41 {
42  PointCloud2 cloud( depthImage.size() );
43 
44  const int centerX = depthImage.width() / 2;
45  const int centerY = depthImage.height() / 2;
46 
47  auto imageIter = depthImage.const_begin();
48  auto cloudIter = cloud.geometry_begin();
49 
50  const float invFocal = 1.0f / focalLength;
51  size_t validPts = 0;
52 
53  for ( int v = centerY; v > -centerY; --v )
54  {
55  for ( int u = -centerX; u < centerX; ++u, ++imageIter )
56  {
57  const float z = imageIter->val();
58  const bool nan = std::isnan( z );
59 
60  if( nan && keepNaN )
61  *cloudIter = std::numeric_limits<PointCloud2::BaseType>::quiet_NaN();
62  else if( nan )
63  continue;
64  else
65  {
66  // Coordinate system = EAST-NORTH-UP, so
67  // x is u*zD
68  // y is z
69  // z is v * zD
70  const float zD = z * invFocal;
71  cloudIter->x() = u * zD;
72  cloudIter->y() = z;
73  cloudIter->z() = v * zD;
74  cloudIter->w() = 1;
75  }
76 
77  ++validPts;
78  ++cloudIter;
79  } // end centerX for
80  } // end centerY for
81 
82  // clip to valid points
83  cloud.resize( validPts );
84  return cloud;
85 }
86 
87 // ######################################################################
88 template <class T1, class T2>
89 auto nrt::depthToPointCloud( Image<PixRGBD<T1, T2>> const depthImage, float focalLength, const bool keepNaN ) -> PointCloud2
90 {
91  PointCloud2 cloud = PointCloud2::create<PixRGB<T1>>( depthImage.size() );
92 
93  const int centerX = depthImage.width() / 2;
94  const int centerY = depthImage.height() / 2;
95 
96  auto imageIter = depthImage.const_begin();
97  auto cloudIter = cloud.begin<PixRGB<T1>>();
98 
99  const float invFocal = 1.0f / focalLength;
100  size_t validPts = 0;
101 
102  for ( int v = centerY; v > -centerY; --v )
103  {
104  for ( int u = -centerX; u < centerX; ++u, ++imageIter )
105  {
106  const float z = imageIter->d();
107  const bool nan = std::isnan( z );
108 
109  if( nan && keepNaN )
110  cloudIter->geometry() = std::numeric_limits<PointCloud2::BaseType>::quiet_NaN();
111  else if( nan )
112  continue;
113  else
114  {
115  // Coordinate system = EAST-NORTH-UP, so
116  // x is u*zD
117  // y is z
118  // z is v * zD
119  auto & geometry = cloudIter->geometry();
120  auto & color = cloudIter->template get<PixRGB<T1>>();
121 
122  const float zD = z * invFocal;
123 
124  geometry.x() = u * zD;
125  geometry.y() = z;
126  geometry.z() = v * zD;
127  geometry.w() = 1;
128 
129  color = imageIter->rgb();
130  }
131 
132  ++cloudIter;
133  ++validPts;
134  } // end centerX for
135  } // end centerY for
136 
137  // clip to valid points
138  cloud.resize( validPts );
139  return cloud;
140 }
141 
142 // ######################################################################
143 template <class T1, class T2>
144 auto nrt::depthToPointCloud( Image<PixRGB<T1>> const rgbImage, Image<PixGray<T2>> const depthImage,
145  float focalLength, const bool keepNaN ) -> PointCloud2
146 {
147  if (depthImage.dims() != rgbImage.dims())
148  throw nrt::exception::Exception("RGB and Depth dims must match in depthToPointCloud");
149 
150  PointCloud2 cloud = PointCloud2::create<PixRGB<T1>>( depthImage.size() );
151 
152  const int centerX = depthImage.width() / 2;
153  const int centerY = depthImage.height() / 2;
154 
155  auto imageIter = rgbImage.const_begin();
156  auto depthIter = depthImage.const_begin();
157  auto cloudIter = cloud.begin<PixRGB<T1>>();
158 
159  const float invFocal = 1.0f / focalLength;
160  size_t validPts = 0;
161 
162  for ( int v = centerY; v > -centerY; --v )
163  {
164  for ( int u = -centerX; u < centerX; ++u, ++imageIter, ++depthIter )
165  {
166  const float z = depthIter->val();
167  const bool nan = std::isnan( z );
168 
169  if( nan && keepNaN )
170  cloudIter->geometry() = std::numeric_limits<PointCloud2::BaseType>::quiet_NaN();
171  else if( nan )
172  continue;
173  else
174  {
175  // Coordinate system = EAST-NORTH-UP, so
176  // x is u*zD
177  // y is z
178  // z is v * zD
179  auto & geometry = cloudIter->geometry();
180  auto & color = cloudIter->template get<PixRGB<T1>>();
181 
182  const float zD = z * invFocal;
183 
184  geometry.x() = u * zD;
185  geometry.y() = z;
186  geometry.z() = v * zD;
187  geometry.w() = 1;
188 
189  color = *imageIter;
190  }
191 
192  ++cloudIter;
193  ++validPts;
194  } // end centerX for
195  } // end centerY for
196 
197  // clip to valid points
198  cloud.resize( validPts );
199  return cloud;
200 }
201 
202 //! @endcond
203 
204 #endif // INCLUDE_NRT_POINTCLOUD2_IO_DEPTHTOCLOUD_DETAILS_DEPTHTOCLOUDIMPL_H