RollingGrid.cxx 14.1 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//==============================================================================
// Copyright 2019-2020 Kitware, Inc., Kitware SAS
// Author: Guilbert Pierre (Kitware SAS)
//         Cadart Nicolas (Kitware SAS)
// Creation date: 2019-12-13
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//==============================================================================

20
#include "LidarSlam/RollingGrid.h"
21
#include "LidarSlam/Utilities.h"
22

23
24
#include <unsupported/Eigen/CXX11/Tensor>

25
26
27
28
29
30
31
// A new PCL Point is added so we need to recompile PCL to be able to use
// filters (pcl::VoxelGrid) with this new type
#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE
#endif
#include <pcl/filters/voxel_grid.h>

32
33
34
35
36
37
38
39
40
41
42
43
namespace
{
  template<typename T>
  RollingGrid::Grid3D<T> InitGrid3D(unsigned int size, T defaultValue = T())
  {
    return RollingGrid::Grid3D<T>(size,
                                  std::vector<std::vector<T>>(size,
                                                              std::vector<T>(size,
                                                                             defaultValue)));
  }
}

44
45
46
47
//==============================================================================
//   Initialization and parameters setters
//==============================================================================

48
//------------------------------------------------------------------------------
49
RollingGrid::RollingGrid(const Eigen::Vector3d& position)
50
{
51
  // Create rolling grid
52
  this->Grid = InitGrid3D<PointCloud::Ptr>(this->GridSize);
53

54
  this->Reset(position);
55
56
}

57
//------------------------------------------------------------------------------
58
void RollingGrid::Reset(const Eigen::Vector3d& position)
59
{
60
61
  // Clear/reset empty voxel grid
  this->Clear();
62

63
  // Initialize VoxelGrid center position
64
  // Position is rounded down to be a multiple of resolution
65
  this->VoxelGridPosition = (position.array() / this->VoxelResolution).floor() * this->VoxelResolution;
66
67
}

68
//------------------------------------------------------------------------------
69
70
void RollingGrid::Clear()
{
71
72
73
  for (int x = 0; x < this->GridSize; x++)
    for (int y = 0; y < this->GridSize; y++)
      for (int z = 0; z < this->GridSize; z++)
74
75
76
77
78
79
80
81
82
      {
        // If voxel is not already initialized, allocate memory.
        // Otherwise, just clear data without freeing dedicating memory for faster processing.
        auto& voxel = this->Grid[x][y][z];
        if (voxel)
          voxel->clear();
        else
          voxel.reset(new PointCloud);
      }
83
84
}

85
86
87
88
89
90
91
92
93
94
95
96
//------------------------------------------------------------------------------
void RollingGrid::SetGridSize(int size)
{
  PointCloud::Ptr prevMap = this->Get();

  // Resize voxel grid
  this->GridSize = size;
  this->Grid = InitGrid3D<PointCloud::Ptr>(this->GridSize);
  // Clear current voxel grid and allocate new voxels
  this->Clear();

  // Add points back so that they now lie in the right voxel
97
98
  if (!prevMap->empty())
    this->Add(prevMap);
99
100
101
102
103
104
105
106
107
108
109
110
111
}

//------------------------------------------------------------------------------
void RollingGrid::SetVoxelResolution(double resolution)
{
  this->VoxelResolution = resolution;

  // Round down VoxelGrid center position to be a multiple of resolution
  this->VoxelGridPosition = (this->VoxelGridPosition / this->VoxelResolution).floor() * this->VoxelResolution;

  // Move points so that they now lie in the right voxel
  PointCloud::Ptr prevMap = this->Get();
  this->Clear();
112
113
  if (!prevMap->empty())
    this->Add(prevMap);
114
115
116
117
118
119
120
}

//==============================================================================
//   Main use
//==============================================================================

//------------------------------------------------------------------------------
121
RollingGrid::PointCloud::Ptr RollingGrid::Get(const Eigen::Array3d& minPoint, const Eigen::Array3d& maxPoint) const
122
{
123
124
  // Compute the position of the origin cell (0, 0, 0) of the grid
  Eigen::Array3i voxelGridOrigin = this->PositionToVoxel(this->VoxelGridPosition) - this->GridSize / 2;
125
126

  // Get sub-VoxelGrid bounds
127
128
  Eigen::Array3i intersectionMin = (this->PositionToVoxel(minPoint) - voxelGridOrigin).max(0);
  Eigen::Array3i intersectionMax = (this->PositionToVoxel(maxPoint) - voxelGridOrigin).min(this->GridSize - 1);
129
130
131
132
133
134
135
136
137
138
139

  // Get all voxel in intersection
  PointCloud::Ptr intersection(new PointCloud);
  for (int x = intersectionMin.x(); x <= intersectionMax.x(); x++)
    for (int y = intersectionMin.y(); y <= intersectionMax.y(); y++)
      for (int z = intersectionMin.z(); z <= intersectionMax.z(); z++)
        *intersection += *(this->Grid[x][y][z]);

  return intersection;
}

140
141
142
//------------------------------------------------------------------------------
RollingGrid::PointCloud::Ptr RollingGrid::Get(const PointCloud& pcToMatch) const
{
143
144
145
  // Identify voxels in which lie input points, and the bounding box of the non-empty voxels
  Eigen::Tensor<float, 3> pointsInVoxels(this->GridSize, this->GridSize, this->GridSize);
  pointsInVoxels.setZero();
146
  Eigen::Array3i voxelGridOrigin = this->PositionToVoxel(this->VoxelGridPosition) - this->GridSize / 2;
147
148
149
  Eigen::Array3i minCell, maxCell;
  minCell.setConstant(this->GridSize - 1);
  maxCell.setConstant(0);
150
151
152
153
  for (const Point& point : pcToMatch)
  {
    // Find the voxel containing this point
    Eigen::Array3i cubeIdx = this->PositionToVoxel(point.getArray3fMap()) - voxelGridOrigin;
154
    // Notify the voxel if it is within grid
155
    if (((0 <= cubeIdx) && (cubeIdx < this->GridSize)).all())
156
157
158
159
160
    {
      pointsInVoxels(cubeIdx.x(), cubeIdx.y(), cubeIdx.z())++;
      minCell = minCell.min(cubeIdx);
      maxCell = maxCell.max(cubeIdx);
    }
161
162
  }

163
164
165
166
167
168
169
170
171
172
173
  // Check if maxPoint is greater than minPoint.
  // If not, this means that no point from pcToMatch lies in rolling grid.
  if ((minCell > maxCell).any())
    return PointCloud::Ptr(new PointCloud);

  // Extract non empty part of rolling grid using bounding box
  // We do that to to save time by avoiding convolving the entire rolling grid
  Eigen::array<int, 3> offsets = {minCell.x(), minCell.y(), minCell.z()};
  Eigen::array<int, 3> extents = {maxCell.x() - minCell.x() + 1, maxCell.y() - minCell.y() + 1, maxCell.z() - minCell.z() + 1};
  Eigen::Tensor<float, 3> nonEmptyVoxels = pointsInVoxels.slice(offsets, extents);

174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
  // Dilate the votes by convolving with blur kernel
  Eigen::TensorFixedSize<float, Eigen::Sizes<3, 3, 3>> kernel;
  constexpr float centerWeight = 1.;
  constexpr float orthoWeight  = 0.4;
  constexpr float diagWeight   = 0.2;
  constexpr float cornerWeight = 0.1;
  kernel.setValues({{{cornerWeight,   diagWeight,  cornerWeight},
                     {  diagWeight,  orthoWeight,    diagWeight},
                     {cornerWeight,   diagWeight,  cornerWeight}},

                    {{  diagWeight,  orthoWeight,    diagWeight},
                     { orthoWeight, centerWeight,   orthoWeight},
                     {  diagWeight,  orthoWeight,    diagWeight}},

                    {{cornerWeight,   diagWeight,  cornerWeight},
                     {  diagWeight,  orthoWeight,    diagWeight},
                     {cornerWeight,   diagWeight,  cornerWeight}}});
  // Perform SAME convolution by adding padding
  Eigen::array<std::pair<int, int>, 3> padding = {{{1, 1}, {1, 1}, {1, 1}}};
  Eigen::array<int, 3> dims = {0, 1, 2};
  Eigen::Tensor<float, 3> voxelsToUse = nonEmptyVoxels.pad(padding).eval().convolve(kernel, dims);

196
  // Extract all points from voxels to use
197
198
  constexpr float THRESHOLD = 1.;
  PointCloud::Ptr intersection(new PointCloud);
199
200
201
  for (int x = 0; x < voxelsToUse.dimension(0); x++)
    for (int y = 0; y < voxelsToUse.dimension(1); y++)
      for (int z = 0; z < voxelsToUse.dimension(2); z++)
202
        if (voxelsToUse(x, y, z) >= THRESHOLD)
203
          *intersection += *(this->Grid[x + minCell.x()][y + minCell.y()][z + minCell.z()]);
204
205
206
207

  return intersection;
}

208
209
210
211
212
213
214
215
216
217
218
219
220
//------------------------------------------------------------------------------
RollingGrid::PointCloud::Ptr RollingGrid::Get() const
{
  // Merge all points into a single pointcloud
  PointCloud::Ptr intersection(new PointCloud);
  for (int x = 0; x < this->GridSize; x++)
    for (int y = 0; y < this->GridSize; y++)
      for (int z = 0; z < this->GridSize; z++)
        *intersection += *(this->Grid[x][y][z]);

  return intersection;
}

221
//------------------------------------------------------------------------------
222
void RollingGrid::Roll(const Eigen::Array3d& minPoint, const Eigen::Array3d& maxPoint)
223
{
224
  // Very basic implementation where the grid is not circular.
225
  // This only moves VoxelGrid so that the given bounding box can entirely fit in rolled map.
226

227
228
  // Compute how much the new frame does not fit in current grid
  double halfGridSize = static_cast<double>(this->GridSize) / 2 * this->VoxelResolution;
229
230
  Eigen::Array3d downOffset = minPoint - (VoxelGridPosition - halfGridSize);
  Eigen::Array3d upOffset   = maxPoint - (VoxelGridPosition + halfGridSize);
231
232
233
234
235
236
237
238
  Eigen::Array3d offset = (upOffset + downOffset) / 2;

  // Clamp the rolling movement so that it only moves what is really necessary
  offset = offset.max(downOffset.min(0)).min(upOffset.max(0));
  Eigen::Array3d voxelsOffset = (offset / this->VoxelResolution).round();

  // Update new rolling grid position
  this->VoxelGridPosition += voxelsOffset * this->VoxelResolution;
239

240
  // Shift the voxel grid to the -X direction.
241
  while (voxelsOffset.x() < 0)
242
  {
243
    for (int y = 0; y < this->GridSize; y++)
244
    {
245
      for (int z = 0; z < this->GridSize; z++)
246
      {
247
        for (int x = this->GridSize - 1; x > 0; x--)
248
        {
249
          this->Grid[x][y][z] = std::move(this->Grid[x - 1][y][z]);
250
        }
251
        this->Grid[0][y][z].reset(new PointCloud);
252
253
      }
    }
254
    voxelsOffset.x()++;
255
256
  }

257
  // Shift the voxel grid to the +X direction.
258
  while (voxelsOffset.x() > 0)
259
  {
260
    for (int y = 0; y < this->GridSize; y++)
261
    {
262
      for (int z = 0; z < this->GridSize; z++)
263
      {
264
        for (int x = 0; x < this->GridSize - 1; x++)
265
        {
266
          this->Grid[x][y][z] = std::move(this->Grid[x + 1][y][z]);
267
        }
268
        this->Grid[this->GridSize - 1][y][z].reset(new PointCloud);
269
270
      }
    }
271
    voxelsOffset.x()--;
272
273
  }

274
  // Shift the voxel grid to the -Y direction.
275
  while (voxelsOffset.y() < 0)
276
  {
277
    for (int x = 0; x < this->GridSize; x++)
278
    {
279
      for (int z = 0; z < this->GridSize; z++)
280
      {
281
        for (int y = this->GridSize - 1; y > 0; y--)
282
        {
283
          this->Grid[x][y][z] = std::move(this->Grid[x][y - 1][z]);
284
        }
285
        this->Grid[x][0][z].reset(new PointCloud);
286
287
      }
    }
288
    voxelsOffset.y()++;
289
290
  }

291
  // Shift the voxel grid to the +Y direction.
292
  while (voxelsOffset.y() > 0)
293
  {
294
    for (int x = 0; x < this->GridSize; x++)
295
    {
296
      for (int z = 0; z < this->GridSize; z++)
297
      {
298
        for (int y = 0; y < this->GridSize - 1; y++)
299
        {
300
          this->Grid[x][y][z] = std::move(this->Grid[x][y + 1][z]);
301
        }
302
        this->Grid[x][this->GridSize - 1][z].reset(new PointCloud);
303
304
      }
    }
305
    voxelsOffset.y()--;
306
307
  }

308
  // Shift the voxel grid to the -Z direction.
309
  while (voxelsOffset.z() < 0)
310
  {
311
    for (int x = 0; x < this->GridSize; x++)
312
    {
313
      for (int y = 0; y < this->GridSize; y++)
314
      {
315
        for (int z = this->GridSize - 1; z > 0; z--)
316
        {
317
          this->Grid[x][y][z] = std::move(this->Grid[x][y][z - 1]);
318
        }
319
        this->Grid[x][y][0].reset(new PointCloud);
320
321
      }
    }
322
    voxelsOffset.z()++;
323
324
  }

325
  // Shift the voxel grid to the +Z direction.
326
  while (voxelsOffset.z() > 0)
327
  {
328
    for (int x = 0; x < this->GridSize; x++)
329
    {
330
      for (int y = 0; y < this->GridSize; y++)
331
      {
332
        for (int z = 0; z < this->GridSize - 1; z++)
333
        {
334
          this->Grid[x][y][z] = std::move(this->Grid[x][y][z + 1]);
335
        }
336
        this->Grid[x][y][this->GridSize - 1].reset(new PointCloud);
337
338
      }
    }
339
    voxelsOffset.z()--;
340
341
342
343
  }
}

//------------------------------------------------------------------------------
344
void RollingGrid::Add(const PointCloud::Ptr& pointcloud, bool roll)
345
{
346
  if (pointcloud->empty())
347
  {
348
    PRINT_WARNING("Pointcloud is empty, voxel grid not updated.");
349
350
351
    return;
  }

352
353
354
355
356
357
358
359
  // Optionally roll the map so that all new points can fit in rolled map
  if (roll)
  {
    Eigen::Vector4f minPoint, maxPoint;
    pcl::getMinMax3D(*pointcloud, minPoint, maxPoint);
    this->Roll(minPoint.head<3>().cast<double>().array(), maxPoint.head<3>().cast<double>().array());
  }

360
  // Voxels to filter because new points were added
361
  Grid3D<uint8_t> voxelToFilter = InitGrid3D<uint8_t>(this->GridSize, 0);
362

363
364
  // Compute the "position" of the lowest cell of the VoxelGrid in voxels dimensions
  Eigen::Array3i voxelGridOrigin = this->PositionToVoxel(this->VoxelGridPosition) - this->GridSize / 2;
365

366
  // Add points in the rolling grid
367
  for (const Point& point : *pointcloud)
368
  {
369
    // Find the voxel containing this point
370
    Eigen::Array3i cubeIdx = this->PositionToVoxel(point.getArray3fMap()) - voxelGridOrigin;
371

372
373
    // Add point to grid if it is indeed within bounds
    if (((0 <= cubeIdx) && (cubeIdx < this->GridSize)).all())
374
    {
375
376
      voxelToFilter[cubeIdx.x()][cubeIdx.y()][cubeIdx.z()] = 1;
      this->Grid[cubeIdx.x()][cubeIdx.y()][cubeIdx.z()]->push_back(point);
377
378
379
380
381
382
    }
  }

  // Filter the modified pointCloud
  pcl::VoxelGrid<Point> downSizeFilter;
  downSizeFilter.setLeafSize(this->LeafSize, this->LeafSize, this->LeafSize);
383
  for (int x = 0; x < this->GridSize; x++)
384
  {
385
    for (int y = 0; y < this->GridSize; y++)
386
    {
387
      for (int z = 0; z < this->GridSize; z++)
388
      {
389
        if (voxelToFilter[x][y][z])
390
        {
391
          PointCloud::Ptr tmp(new PointCloud);
392
          downSizeFilter.setInputCloud(this->Grid[x][y][z]);
393
          downSizeFilter.filter(*tmp);
394
          this->Grid[x][y][z] = tmp;
395
396
397
398
399
        }
      }
    }
  }
}