Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
tsdf_volume.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 */
36
37#ifndef TSDF_VOLUME_HPP_
38#define TSDF_VOLUME_HPP_
39
40#include "tsdf_volume.h"
41
42#include <fstream>
43
44
45template <typename VoxelT, typename WeightT> bool
46pcl::TSDFVolume<VoxelT, WeightT>::load (const std::string &filename, bool binary)
47{
48 pcl::console::print_info ("Loading TSDF volume from "); pcl::console::print_value ("%s ... ", filename.c_str());
49 std::cout << std::flush;
50
51 std::ifstream file (filename.c_str());
52
53 if (file.is_open())
54 {
55 if (binary)
56 {
57 // read HEADER
58 file.read ((char*) &header_, sizeof (Header));
59 /* file.read (&header_.resolution, sizeof(Eigen::Array3i));
60 file.read (&header_.volume_size, sizeof(Eigen::Vector3f));
61 file.read (&header_.volume_element_size, sizeof(int));
62 file.read (&header_.weights_element_size, sizeof(int)); */
63
64 // check if element size fits to data
65 if (header_.volume_element_size != sizeof(VoxelT))
66 {
67 pcl::console::print_error ("[TSDFVolume::load] Error: Given volume element size (%d) doesn't fit data (%d)", sizeof(VoxelT), header_.volume_element_size);
68 return false;
69 }
70 if ( header_.weights_element_size != sizeof(WeightT))
71 {
72 pcl::console::print_error ("[TSDFVolume::load] Error: Given weights element size (%d) doesn't fit data (%d)", sizeof(WeightT), header_.weights_element_size);
73 return false;
74 }
75
76 // read DATA
77 int num_elements = header_.getVolumeSize();
78 volume_->resize (num_elements);
79 weights_->resize (num_elements);
80 file.read ((char*) &(*volume_)[0], num_elements * sizeof(VoxelT));
81 file.read ((char*) &(*weights_)[0], num_elements * sizeof(WeightT));
82 }
83 else
84 {
85 pcl::console::print_error ("[TSDFVolume::load] Error: ASCII loading not implemented.\n");
86 }
87
88 file.close ();
89 }
90 else
91 {
92 pcl::console::print_error ("[TSDFVolume::load] Error: Cloudn't read file %s.\n", filename.c_str());
93 return false;
94 }
95
96 const Eigen::Vector3i &res = this->gridResolution();
97 pcl::console::print_info ("done [%d voxels, res %dx%dx%d]\n", this->size(), res[0], res[1], res[2]);
98
99 return true;
100}
101
102
103template <typename VoxelT, typename WeightT> bool
104pcl::TSDFVolume<VoxelT, WeightT>::save (const std::string &filename, bool binary) const
105{
106 pcl::console::print_info ("Saving TSDF volume to "); pcl::console::print_value ("%s ... ", filename.c_str());
107 std::cout << std::flush;
108
109 std::ofstream file (filename.c_str(), binary ? std::ios_base::binary : std::ios_base::out);
110
111 if (file.is_open())
112 {
113 if (binary)
114 {
115 // HEADER
116 // write resolution and size of volume
117 file.write ((char*) &header_, sizeof (Header));
118 /* file.write ((char*) &header_.resolution, sizeof(Eigen::Vector3i));
119 file.write ((char*) &header_.volume_size, sizeof(Eigen::Vector3f));
120 // write element size
121 int volume_element_size = sizeof(VolumeT);
122 file.write ((char*) &volume_element_size, sizeof(int));
123 int weights_element_size = sizeof(WeightT);
124 file.write ((char*) &weights_element_size, sizeof(int)); */
125
126 // DATA
127 // write data
128 file.write ((char*) &(volume_->at(0)), volume_->size()*sizeof(VoxelT));
129 file.write ((char*) &(weights_->at(0)), weights_->size()*sizeof(WeightT));
130 }
131 else
132 {
133 // write resolution and size of volume and element size
134 file << header_.resolution(0) << " " << header_.resolution(1) << " " << header_.resolution(2) << std::endl;
135 file << header_.volume_size(0) << " " << header_.volume_size(1) << " " << header_.volume_size(2) << std::endl;
136 file << sizeof (VoxelT) << " " << sizeof(WeightT) << std::endl;
137
138 // write data
139 for (typename std::vector<VoxelT>::const_iterator iter = volume_->begin(); iter != volume_->end(); ++iter)
140 file << *iter << std::endl;
141 }
142
143 file.close();
144 }
145 else
146 {
147 pcl::console::print_error ("[saveTsdfVolume] Error: Couldn't open file %s.\n", filename.c_str());
148 return false;
149 }
150
151 pcl::console::print_info ("done [%d voxels]\n", this->size());
152
153 return true;
154}
155
156
157template <typename VoxelT, typename WeightT> void
159 const unsigned step) const
160{
161 int sx = header_.resolution(0);
162 int sy = header_.resolution(1);
163 int sz = header_.resolution(2);
164
165 const int cloud_size = header_.getVolumeSize() / (step*step*step);
166
167 cloud->clear();
168 cloud->reserve (std::min (cloud_size/10, 500000));
169
170 int volume_idx = 0, cloud_idx = 0;
171 for (int z = 0; z < sz; z+=step)
172 for (int y = 0; y < sy; y+=step)
173 for (int x = 0; x < sx; x+=step, ++cloud_idx)
174 {
175 volume_idx = sx*sy*z + sx*y + x;
176 // pcl::PointXYZI &point = (*cloud)[cloud_idx];
177
178 if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
179 continue;
180
181 pcl::PointXYZI point;
182 point.x = x; point.y = y; point.z = z;//*64;
183 point.intensity = volume_->at(volume_idx);
184 cloud->push_back (point);
185 }
186
187 // cloud->width = cloud_size;
188 // cloud->height = 1;
189}
190
191
192template <typename VoxelT, typename WeightT> template <typename PointT> void
193pcl::TSDFVolume<VoxelT, WeightT>::getVoxelCoord (const PointT &point, Eigen::Vector3i &coord) const
194{
195 static Eigen::Array3f voxel_size = voxelSize().array();
196
197 // point coordinates in world coordinate frame and voxel coordinates
198 Eigen::Array3f point_coord (point.x, point.y, point.z);
199 Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f; // 0.5f offset due to voxel center vs grid
200 coord(0) = round(voxel_coord(0));
201 coord(1) = round(voxel_coord(1));
202 coord(2) = round(voxel_coord(2));
203}
204
205
206/** \brief Returns the 3D voxel coordinate and point offset wrt. to the voxel center (in mm) */
207template <typename VoxelT, typename WeightT> template <typename PointT> void
209 Eigen::Vector3i &coord, Eigen::Vector3f &offset) const
210{
211 static Eigen::Array3f voxel_size = voxelSize().array();
212
213 // point coordinates in world coordinate frame and voxel coordinates
214 Eigen::Array3f point_coord (point.x, point.y, point.z);
215 Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f; // 0.5f offset due to voxel center vs grid
216 coord(0) = round(voxel_coord(0));
217 coord(1) = round(voxel_coord(1));
218 coord(2) = round(voxel_coord(2));
219
220 // offset of point wrt. to voxel center
221 offset = (voxel_coord - coord.cast<float>().array() * voxel_size).matrix();
222}
223
224
225template <typename VoxelT, typename WeightT> bool
226pcl::TSDFVolume<VoxelT, WeightT>::extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
227 VoxelTVec &neighborhood) const
228{
229 // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
230 int shift = (neighborhood_size - 1) / 2;
231 Eigen::Vector3i min_index = voxel_coord.array() - shift;
232 Eigen::Vector3i max_index = voxel_coord.array() + shift;
233
234 // check that index is within range
235 if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
236 {
237 pcl::console::print_info ("[extractNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
238 return false;
239 }
240
241 static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
242 neighborhood.resize (descriptor_size);
243
244 const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
245
246 // loop over all voxels in 3D neighborhood
247 #pragma omp parallel for \
248 default(none)
249 for (int z = min_index(2); z <= max_index(2); ++z)
250 {
251 for (int y = min_index(1); y <= max_index(1); ++y)
252 {
253 for (int x = min_index(0); x <= max_index(0); ++x)
254 {
255 // linear voxel index in volume and index in descriptor vector
256 Eigen::Vector3i point (x,y,z);
257 int volume_idx = getLinearVoxelIndex (point);
258 int descr_idx = offset_vector * (point - min_index);
259
260/* std::cout << "linear index " << volume_idx << std::endl;
261 std::cout << "weight " << weights_->at (volume_idx) << std::endl;
262 std::cout << "volume " << volume_->at (volume_idx) << std::endl;
263 std::cout << "descr " << neighborhood.rows() << "x" << neighborhood.cols() << ", val = " << neighborhood << std::endl;
264 std::cout << "descr index = " << descr_idx << std::endl;
265*/
266 // get the TSDF value and store as descriptor entry
267 if (weights_->at (volume_idx) != 0)
268 neighborhood (descr_idx) = volume_->at (volume_idx);
269 else
270 neighborhood (descr_idx) = -1.0; // if never visited we assume inside of object (outside captured and thus filled with positive values)
271 }
272 }
273 }
274
275 return true;
276}
277
278
279template <typename VoxelT, typename WeightT> bool
280pcl::TSDFVolume<VoxelT, WeightT>::addNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
281 const VoxelTVec &neighborhood, WeightT voxel_weight)
282{
283 // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
284 int shift = (neighborhood_size - 1) / 2;
285 Eigen::Vector3i min_index = voxel_coord.array() - shift;
286 Eigen::Vector3i max_index = voxel_coord.array() + shift;
287
288 // check that index is within range
289 if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
290 {
291 pcl::console::print_info ("[addNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
292 return false;
293 }
294
295 // static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
296 const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
297
298 Eigen::Vector3i index = min_index;
299 // loop over all voxels in 3D neighborhood
300 #pragma omp parallel for \
301 default(none)
302 for (int z = min_index(2); z <= max_index(2); ++z)
303 {
304 for (int y = min_index(1); y <= max_index(1); ++y)
305 {
306 for (int x = min_index(0); x <= max_index(0); ++x)
307 {
308 // linear voxel index in volume and index in descriptor vector
309 Eigen::Vector3i point (x,y,z);
310 int volume_idx = getLinearVoxelIndex (point);
311 int descr_idx = offset_vector * (point - min_index);
312
313 // add the descriptor entry to the volume
314 VoxelT &voxel = volume_->at (volume_idx);
315 WeightT &weight = weights_->at (volume_idx);
316
317 // TODO check that this simple lock works correctly!!
318 #pragma omp atomic
319 voxel += neighborhood (descr_idx);
320
321 #pragma omp atomic
322 weight += voxel_weight;
323 }
324 }
325 }
326
327 return true;
328}
329
330template <typename VoxelT, typename WeightT> void
332{
333 #pragma omp parallel for \
334 default(none)
335 for (std::size_t i = 0; i < volume_->size(); ++i)
336 {
337 WeightT &w = weights_->at(i);
338 if (w > 0.0)
339 {
340 volume_->at(i) /= w;
341 w = 1.0;
342 }
343 }
344}
345
346#define PCL_INSTANTIATE_TSDFVolume(VT,WT) template class PCL_EXPORTS pcl::reconstruction::TSDFVolume<VT,WT>;
347
348#endif /* TSDF_VOLUME_HPP_ */
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void clear()
Removes all points in a cloud and sets the width and height to 0.
void reserve(std::size_t n)
shared_ptr< PointCloud< PointT > > Ptr
bool extractNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const
extracts voxels in neighborhood of given voxel
void getVoxelCoordAndOffset(const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const
Returns the 3D voxel coordinate and point offset wrt.
Eigen::VectorXf VoxelTVec
Definition tsdf_volume.h:66
bool addNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, const VoxelTVec &neighborhood, WeightT voxel_weight)
adds voxel values in local neighborhood
void convertToTsdfCloud(pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const unsigned step=2) const
Converts volume to cloud of TSDF values.
bool save(const std::string &filename="tsdf_volume.dat", bool binary=true) const
Saves volume to file.
void averageValues()
averages voxel values by the weight value
void getVoxelCoord(const PointT &point, Eigen::Vector3i &voxel_coord) const
Converts the volume to a surface representation via a point cloud.
bool load(const std::string &filename, bool binary=true)
Loads volume from file.
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
PCL_EXPORTS void print_value(const char *format,...)
Print a value message on stream with colors.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Structure storing voxel grid resolution, volume size (in mm) and element_size of stored data.
Definition tsdf_volume.h:70