ACloudViewer  3.9.4
A Modern Library for 3D Data Processing
ecvGriddedTools.cpp
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - CloudViewer: www.cloudViewer.org -
3 // ----------------------------------------------------------------------------
4 // Copyright (c) 2018-2024 www.cloudViewer.org
5 // SPDX-License-Identifier: MIT
6 // ----------------------------------------------------------------------------
7 
8 #include "ecvGriddedTools.h"
9 
10 // CV_CORE_LIB
11 #include "CVLog.h"
12 
13 // Local
14 #include "ecvGBLSensor.h"
15 
17 using AngleAndSpan = std::pair<PointCoordinateType, unsigned>;
18 
19 bool ccGriddedTools::DetectParameters(const ccPointCloud* cloud,
20  const ccPointCloud::Grid::Shared grid,
21  GridParameters& parameters,
22  bool verbose /*=false*/,
23  ccGLMatrix* cloudToSensorTrans /*=0*/) {
24  if (!cloud || !grid) {
25  assert(false);
26  return false;
27  }
28 
29  parameters.minPhi = static_cast<PointCoordinateType>(M_PI);
30  parameters.maxPhi = -parameters.minPhi;
31  parameters.minTheta = static_cast<PointCoordinateType>(M_PI);
32  parameters.maxTheta = -parameters.minTheta;
33  parameters.deltaPhiRad = 0;
34  parameters.deltaThetaRad = 0;
35  parameters.maxRange = 0;
36 
37  // we must test if the angles are shifted (i.e the scan spans above theta =
38  // pi) we'll compute all parameters for both cases, and choose the best one
39  // at the end!
40  PointCoordinateType minPhiShifted = parameters.minPhi,
41  maxPhiShifted = parameters.maxPhi;
42  PointCoordinateType minThetaShifted = parameters.minTheta,
43  maxThetaShifted = parameters.maxTheta;
44 
45  try {
46  // determine the PITCH angular step
47  {
48  std::vector<AngleAndSpan> angles;
49  std::vector<AngleAndSpan> anglesShifted;
50 
51  // for each ROW we determine the min and max valid grid point (i.e.
52  // index >= 0)
53  const int* _indexGrid = grid->indexes.data();
54  for (unsigned j = 0; j < grid->h; ++j) {
55  unsigned minIndex = grid->w;
56  unsigned maxIndex = 0;
57  for (unsigned i = 0; i < grid->w; ++i) {
58  if (_indexGrid[i] >= 0) {
59  if (i < minIndex) minIndex = i;
60  if (i > maxIndex) maxIndex = i;
61  }
62  }
63 
64  if (maxIndex > minIndex) {
65  PointCoordinateType minPhiCurrentLine = 0,
66  maxPhiCurrentLine = 0;
67  PointCoordinateType minPhiCurrentLineShifted = 0,
68  maxPhiCurrentLineShifted = 0;
69  for (unsigned k = minIndex; k <= maxIndex; ++k) {
70  int index = _indexGrid[k];
71  if (index >= 0) {
72  CCVector3 P = *(cloud->getPoint(
73  static_cast<unsigned>(index)));
74  if (cloudToSensorTrans)
75  cloudToSensorTrans->apply(P);
76  PointCoordinateType p = atan2(
77  P.z,
78  sqrt(P.x * P.x +
79  P.y * P.y)); // see
80  // ccGBLSensor::projectPoint
81  PointCoordinateType pShifted =
82  (p < 0 ? p + static_cast<
83  PointCoordinateType>(
84  2.0 * M_PI)
85  : p);
86  if (k != minIndex) {
87  if (minPhiCurrentLine > p)
88  minPhiCurrentLine = p;
89  else if (maxPhiCurrentLine < p)
90  maxPhiCurrentLine = p;
91 
92  if (minPhiCurrentLineShifted > pShifted)
93  minPhiCurrentLineShifted = pShifted;
94  else if (maxPhiCurrentLineShifted < pShifted)
95  maxPhiCurrentLineShifted = pShifted;
96  } else {
97  minPhiCurrentLine = maxPhiCurrentLine = p;
98  minPhiCurrentLineShifted =
99  maxPhiCurrentLineShifted = pShifted;
100  }
101 
102  // find max range
103  PointCoordinateType range = P.norm();
104  if (range > parameters.maxRange)
105  parameters.maxRange = range;
106  }
107  }
108 
109  if (parameters.minPhi > minPhiCurrentLine)
110  parameters.minPhi = minPhiCurrentLine;
111  if (parameters.maxPhi < maxPhiCurrentLine)
112  parameters.maxPhi = maxPhiCurrentLine;
113 
114  if (minPhiShifted > minPhiCurrentLineShifted)
115  minPhiShifted = minPhiCurrentLineShifted;
116  if (maxPhiShifted < maxPhiCurrentLineShifted)
117  maxPhiShifted = maxPhiCurrentLineShifted;
118 
119  unsigned span = maxIndex - minIndex + 1;
120  ScalarType angle_rad = static_cast<ScalarType>(
121  (maxPhiCurrentLine - minPhiCurrentLine) / span);
122  angles.emplace_back(angle_rad, span);
123 
124  ScalarType angleShifted_rad =
125  static_cast<ScalarType>((maxPhiCurrentLineShifted -
126  minPhiCurrentLineShifted) /
127  span);
128  anglesShifted.emplace_back(angleShifted_rad, span);
129  }
130 
131  _indexGrid += grid->w;
132  }
133 
134  if (!angles.empty()) {
135  // check the 'shifted' hypothesis
136  PointCoordinateType spanShifted = maxPhiShifted - minPhiShifted;
137  PointCoordinateType span =
138  parameters.maxPhi - parameters.minPhi;
139  if (spanShifted < 0.99 * span) {
140  // we prefer the shifted version!
141  angles = anglesShifted;
142  parameters.minPhi = minPhiShifted;
143  parameters.maxPhi = maxPhiShifted;
144  }
145 
146  // we simply take the biggest step evaluation for the widest
147  // span!
148  size_t maxSpanIndex = 0;
149  for (size_t i = 1; i < angles.size(); ++i) {
150  if (angles[i].second > angles[maxSpanIndex].second ||
151  (angles[i].second == angles[maxSpanIndex].second &&
152  angles[i].first > angles[maxSpanIndex].first)) {
153  maxSpanIndex = i;
154  }
155  }
156 
157  parameters.deltaPhiRad = static_cast<PointCoordinateType>(
158  angles[maxSpanIndex].first);
159  if (verbose) {
160  CVLog::Print(QString("[Scan grid] Detected pitch step: %1 "
161  "degrees (span [%2 - %3])")
162  .arg(cloudViewer::RadiansToDegrees(
163  parameters.deltaPhiRad))
164  .arg(cloudViewer::RadiansToDegrees(
165  parameters.minPhi))
166  .arg(cloudViewer::RadiansToDegrees(
167  parameters.maxPhi)));
168  }
169  } else {
170  CVLog::Warning(
171  "[Scan grid] Not enough valid points to compute the "
172  "scan angular step (pitch)!");
173  return false;
174  }
175  }
176 
177  // now determine the YAW angular step
178  {
179  std::vector<AngleAndSpan> angles;
180  std::vector<AngleAndSpan> anglesShifted;
181 
182  // for each COLUMN we determine the min and max valid grid point
183  // (i.e. index >= 0)
184  for (unsigned i = 0; i < grid->w; ++i) {
185  const int* _indexGrid = &(grid->indexes[i]);
186 
187  unsigned minIndex = grid->h;
188  unsigned maxIndex = 0;
189  for (unsigned j = 0; j < grid->h; ++j) {
190  if (_indexGrid[j * grid->w] >= 0) {
191  if (j < minIndex) minIndex = j;
192  if (j > maxIndex) maxIndex = j;
193  }
194  }
195 
196  if (maxIndex > minIndex) {
197  PointCoordinateType minThetaCurrentCol = 0,
198  maxThetaCurrentCol = 0;
199  PointCoordinateType minThetaCurrentColShifted = 0,
200  maxThetaCurrentColShifted = 0;
201  for (unsigned k = minIndex; k <= maxIndex; ++k) {
202  int index = _indexGrid[k * grid->w];
203  if (index >= 0) {
204  // warning: indexes are shifted (0 = no point)
205  CCVector3 P = *(cloud->getPoint(
206  static_cast<unsigned>(index)));
207  if (cloudToSensorTrans)
208  cloudToSensorTrans->apply(P);
209  PointCoordinateType t = atan2(
210  P.y, P.x); // see ccGBLSensor::projectPoint
211  PointCoordinateType tShifted =
212  (t < 0 ? t + static_cast<
213  PointCoordinateType>(
214  2.0 * M_PI)
215  : t);
216  if (k != minIndex) {
217  if (minThetaCurrentColShifted > tShifted)
218  minThetaCurrentColShifted = tShifted;
219  else if (maxThetaCurrentColShifted < tShifted)
220  maxThetaCurrentColShifted = tShifted;
221 
222  if (minThetaCurrentCol > t)
223  minThetaCurrentCol = t;
224  else if (maxThetaCurrentCol < t)
225  maxThetaCurrentCol = t;
226  } else {
227  minThetaCurrentCol = maxThetaCurrentCol = t;
228  minThetaCurrentColShifted =
229  maxThetaCurrentColShifted = tShifted;
230  }
231  }
232  }
233 
234  if (parameters.minTheta > minThetaCurrentCol)
235  parameters.minTheta = minThetaCurrentCol;
236  if (parameters.maxTheta < maxThetaCurrentCol)
237  parameters.maxTheta = maxThetaCurrentCol;
238 
239  if (minThetaShifted > minThetaCurrentColShifted)
240  minThetaShifted = minThetaCurrentColShifted;
241  if (maxThetaShifted < maxThetaCurrentColShifted)
242  maxThetaShifted = maxThetaCurrentColShifted;
243 
244  unsigned span = maxIndex - minIndex;
245  ScalarType angle_rad = static_cast<ScalarType>(
246  (maxThetaCurrentCol - minThetaCurrentCol) / span);
247  angles.emplace_back(angle_rad, span);
248 
249  ScalarType angleShifted_rad = static_cast<ScalarType>(
250  (maxThetaCurrentColShifted -
251  minThetaCurrentColShifted) /
252  span);
253  anglesShifted.emplace_back(angleShifted_rad, span);
254  }
255  }
256 
257  if (!angles.empty()) {
258  // check the 'shifted' hypothesis
259  PointCoordinateType spanShifted =
260  maxThetaShifted - minThetaShifted;
261  PointCoordinateType span =
262  parameters.maxTheta - parameters.minTheta;
263  if (spanShifted < 0.99 * span) {
264  // we prefer the shifted version!
265  angles = anglesShifted;
266  parameters.minTheta = minThetaShifted;
267  parameters.maxTheta = maxThetaShifted;
268  }
269 
270  // we simply take the biggest step evaluation for the widest
271  // span!
272  size_t maxSpanIndex = 0;
273  for (size_t i = 1; i < angles.size(); ++i) {
274  if (angles[i].second > angles[maxSpanIndex].second ||
275  (angles[i].second == angles[maxSpanIndex].second &&
276  angles[i].first > angles[maxSpanIndex].first)) {
277  maxSpanIndex = i;
278  }
279  }
280 
281  parameters.deltaThetaRad = static_cast<PointCoordinateType>(
282  angles[maxSpanIndex].first);
283  if (verbose) {
284  CVLog::Print(QString("[Scan grid] Detected yaw step: %1 "
285  "degrees (span [%2 - %3])")
286  .arg(cloudViewer::RadiansToDegrees(
287  parameters.deltaThetaRad))
288  .arg(cloudViewer::RadiansToDegrees(
289  parameters.minTheta))
290  .arg(cloudViewer::RadiansToDegrees(
291  parameters.maxTheta)));
292  }
293  } else {
294  CVLog::Warning(
295  "[Scan grid] Not enough valid points to compute the "
296  "scan angular steps!");
297  return false;
298  }
299  }
300  } catch (const std::bad_alloc&) {
301  CVLog::Warning(
302  "[Scan grid] Not enough memory to compute the scan angular "
303  "steps!");
304  return false;
305  }
306 
307  return true;
308 }
309 
310 ccGBLSensor* ccGriddedTools::ComputeBestSensor(
311  ccPointCloud* cloud,
312  ccPointCloud::Grid::Shared grid,
313  ccGLMatrix* cloudToSensorTrans /*=0*/) {
314  GridParameters parameters;
315  if (!DetectParameters(cloud, grid, parameters, true, cloudToSensorTrans)) {
316  return nullptr;
317  }
318 
319  ccGBLSensor* sensor = new ccGBLSensor(ccGBLSensor::YAW_THEN_PITCH);
320  if (sensor) {
321  sensor->setPitchStep(parameters.deltaPhiRad);
322  sensor->setPitchRange(parameters.minPhi, parameters.maxPhi);
323  sensor->setYawStep(parameters.deltaThetaRad);
324  sensor->setYawRange(parameters.minTheta, parameters.maxTheta);
325  sensor->setSensorRange(parameters.maxRange);
326  sensor->setGraphicScale(PC_ONE / 2);
327  sensor->setVisible(true);
328  sensor->setEnabled(false);
329  }
330 
331  return sensor;
332 }
PC_ONE
constexpr PointCoordinateType PC_ONE
'1' as a PointCoordinateType value
Definition: CVConst.h:67
M_PI
constexpr double M_PI
Pi.
Definition: CVConst.h:19
PointCoordinateType
float PointCoordinateType
Type of the coordinates of a (N-D) point.
Definition: CVTypes.h:16
CVLog::Warning
static bool Warning(const char *format,...)
Prints out a formatted warning message in console.
Definition: CVLog.cpp:133
CVLog::Print
static bool Print(const char *format,...)
Prints out a formatted message in console.
Definition: CVLog.cpp:113
Tuple3Tpl::y
Type y
Definition: CVGeom.h:137
Tuple3Tpl::x
Type x
Definition: CVGeom.h:137
Tuple3Tpl::z
Type z
Definition: CVGeom.h:137
Vector3Tpl::norm
Type norm() const
Returns vector norm.
Definition: CVGeom.h:424
ccDrawableObject::setVisible
virtual void setVisible(bool state)
Sets entity visibility.
Definition: ecvDrawableObject.h:40
ccGBLSensor
Ground-based Laser sensor.
Definition: ecvGBLSensor.h:26
ccGBLSensor::setYawRange
void setYawRange(PointCoordinateType minTheta, PointCoordinateType maxTheta)
Sets the yaw scanning limits.
Definition: ecvGBLSensor.cpp:100
ccGBLSensor::setPitchRange
void setPitchRange(PointCoordinateType minPhi, PointCoordinateType maxPhi)
Sets the pitch scanning limits.
Definition: ecvGBLSensor.cpp:82
ccGBLSensor::setSensorRange
void setSensorRange(PointCoordinateType range)
Sets the sensor max. range.
Definition: ecvGBLSensor.h:142
ccGBLSensor::setYawStep
void setYawStep(PointCoordinateType dTheta)
Sets the yaw step.
Definition: ecvGBLSensor.cpp:111
ccGBLSensor::setPitchStep
void setPitchStep(PointCoordinateType dPhi)
Sets the pitch step.
Definition: ecvGBLSensor.cpp:93
ccGLMatrixTpl::apply
void apply(float vec[3]) const
Applies transformation to a 3D vector (in place) - float version.
Definition: ecvGLMatrixTpl.h:1039
ccGLMatrix
Float version of ccGLMatrixTpl.
Definition: ecvGLMatrix.h:19
ccGriddedTools::ComputeBestSensor
static ccGBLSensor * ComputeBestSensor(ccPointCloud *cloud, ccPointCloud::Grid::Shared grid, ccGLMatrix *cloudToSensorTrans=0)
Definition: ecvGriddedTools.cpp:310
ccGriddedTools::DetectParameters
static bool DetectParameters(const ccPointCloud *cloud, const ccPointCloud::Grid::Shared grid, GridParameters &parameters, bool verbose=false, ccGLMatrix *cloudToSensorTrans=0)
Detects the given grid parameters (angular span, etc.)
Definition: ecvGriddedTools.cpp:19
ccObject::setEnabled
virtual void setEnabled(bool state)
Sets the "enabled" property.
Definition: ecvObject.h:102
ccPointCloud
A 3D cloud and its associated features (color, normals, scalar fields, etc.)
Definition: ecvPointCloud.h:147
ccSensor::setGraphicScale
void setGraphicScale(PointCoordinateType scale)
Sets the sensor graphic representation scale.
Definition: ecvSensor.h:125
cloudViewer::PointCloudTpl::getPoint
const CCVector3 * getPoint(unsigned index) const override
Definition: PointCloudTpl.h:143
AngleAndSpan
std::pair< PointCoordinateType, unsigned > AngleAndSpan
Association of an angle and the corresponding number of rows/columns.
Definition: ecvGriddedTools.cpp:17
cloudViewer::RadiansToDegrees
float RadiansToDegrees(int radians)
Convert radians to degrees.
Definition: CVMath.h:71
ccGriddedTools::GridParameters
Grid (angular) parameters.
Definition: ecvGriddedTools.h:20
ccGriddedTools::GridParameters::maxRange
PointCoordinateType maxRange
Definition: ecvGriddedTools.h:33
ccGriddedTools::GridParameters::deltaThetaRad
PointCoordinateType deltaThetaRad
Definition: ecvGriddedTools.h:32
ccGriddedTools::GridParameters::maxTheta
PointCoordinateType maxTheta
Definition: ecvGriddedTools.h:31
ccGriddedTools::GridParameters::minTheta
PointCoordinateType minTheta
Definition: ecvGriddedTools.h:31
ccGriddedTools::GridParameters::deltaPhiRad
PointCoordinateType deltaPhiRad
Definition: ecvGriddedTools.h:32
ccPointCloud::Grid::Shared
QSharedPointer< Grid > Shared
Shared type.
Definition: ecvPointCloud.h:385