RemoveSensorBias.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2018,
6 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 #include "RemoveSensorBias.h"
36 
37 #include "PointMatcherPrivate.h"
38 
39 #include <string>
40 #include <vector>
41 
42 #include <boost/format.hpp>
43 #include <ciso646>
44 #include <cmath>
45 
46 
47 template<typename T>
49  PointMatcher<T>::DataPointsFilter("RemoveSensorBiasDataPointsFilter",
50  RemoveSensorBiasDataPointsFilter::availableParameters(), params),
51  sensorType(SensorType(Parametrizable::get<int>("sensorType"))),
52  angleThreshold(Parametrizable::get<T>("angleThreshold")/180.*M_PI)
53 {
54 }
55 
56 template<typename T>
59 {
60  DataPoints output(input);
61  inPlaceFilter(output);
62  return output;
63 }
64 
65 template<typename T>
67 {
68  //Check if there is normals info
69  if (!cloud.descriptorExists("incidenceAngles"))
70  throw InvalidField("RemoveSensorBiasDataPointsFilter: Error, cannot find incidence angles in descriptors.");
71  //Check if there is normals info
72  if (!cloud.descriptorExists("observationDirections"))
73  throw InvalidField("RemoveSensorBiasDataPointsFilter: Error, cannot find observationDirections in descriptors.");
74 
75  const auto& incidenceAngles = cloud.getDescriptorViewByName("incidenceAngles");
76  const auto& observationDirections = cloud.getDescriptorViewByName("observationDirections");
77 
78  double aperture, k1, k2;
79 
80  switch(sensorType)
81  {
82  case LMS_1XX:
83  {
84  aperture = SensorParameters::LMS_1XX.aperture;
85  k1 = SensorParameters::LMS_1XX.k1;
86  k2 = SensorParameters::LMS_1XX.k2;
87  break;
88  }
89  case HDL_32E:
90  {
91  aperture = SensorParameters::HDL_32E.aperture;
92  k1 = SensorParameters::HDL_32E.k1;
93  k2 = SensorParameters::HDL_32E.k2;
94  break;
95  }
96  default:
97  throw InvalidParameter(
98  (boost::format("RemoveSensorBiasDataPointsFilter: Error, cannot remove bias for sensorType id %1% .") % sensorType).str());
99  }
100 
101  const std::size_t nbPts = cloud.getNbPoints();
102  const std::size_t dim = cloud.features.rows();
103 
104 
105  assert(dim == 3 or dim == 4); //check 2D or 3D
106 
107  std::size_t j = 0;
108  for(std::size_t i = 0; i < nbPts; ++i)
109  {
110  const Vector vObs = observationDirections.col(i);
111 
112  const double depth = vObs.norm();
113  const T incidence = incidenceAngles(0, i);
114 
115  //check if the incidence angle could be estimated.
116  //For angles very close to 90 degrees, a small error of estimation could change drastically the correction, so we skip those points.
117  if(not std::isnan(incidence) and incidence >= 0. and incidence < angleThreshold)
118  {
119  const double correction = k1 * diffDist(depth, incidence, aperture) + k2 * ratioCurvature(depth, incidence, aperture);
120 
121  Vector p = cloud.features.col(i);
122  p.head(dim-1) += correction * vObs.normalized();
123  cloud.features.col(i) = p;
124  cloud.setColFrom(j, cloud, i);
125  ++j;
126  }
127  }
128  cloud.conservativeResize(j);
129 }
130 
131 template<typename T>
132 std::array<double, 4> RemoveSensorBiasDataPointsFilter<T>::getCoefficients(const double depth, const T theta, const double aperture) const
133 {
134  const double sigma = tau / std::sqrt(2. * M_PI);
135  const double w0 = lambda_light / (M_PI * aperture);
136 
137  const double A = 2. * std::pow(depth * std::tan(theta), 2) / std::pow(sigma * c, 2) + 2. / std::pow(aperture, 2);
138  const double K1 = std::pow(std::cos(theta), 3);
139  const double K2 = 3. * std::pow(std::cos(theta), 2) * std::sin(theta);
140  const double L1 = pulse_intensity * std::pow(w0 / (aperture * depth * std::cos(theta)), 2) *
141  std::sqrt(M_PI) * std::erf(aperture * std::sqrt(A)) / (2. * std::pow(A, 3. / 2.));
142  const double L2 = pulse_intensity * std::pow(w0 / (aperture * depth * std::cos(theta)), 2) * K2 / (2. * A);
143 
144  const double a0 = 2. * A * K1 * L1;
145  const double a1 = -(2. * std::tan(theta) * depth *
146  (L1 * K2 - 2. * L2 * aperture * std::exp(-A * std::pow(aperture, 2)))) / (std::pow(sigma, 2) * c);
147  const double a2 = -L1 * 2. * A * K1 * (std::pow(sigma * c * std::cos(theta), 2) * A + 2. * std::pow(std::cos(theta) * depth, 2) - 2. * std::pow(depth, 2)) /
148  (2 * std::pow(c * std::cos(theta), 2) * std::pow(sigma, 4) * A);
149  const double a3 = L1 * K2 * depth * std::tan(theta) * (std::pow(sigma * c, 2) * A - 2. * std::pow(depth * std::tan(theta), 2)) /
150  (std::pow(sigma, 6) * std::pow(c, 3) * A);
151 
152  return {a0, a1, a2, a3};
153 }
154 
155 template<typename T>
156 double RemoveSensorBiasDataPointsFilter<T>::diffDist(const double depth, const T theta, const double aperture) const
157 {
158  static constexpr double epsilon = 1e-5;
159 
160  const std::array<double, 4> a = getCoefficients(depth, theta, aperture);
161 
162  double Tmax;
163 
164  if(theta < epsilon) // approx. 0
165  Tmax = 0.;
166  else // theta >0
167  Tmax = (-2 * a[2] - std::sqrt(4 * pow(a[2], 2) - 12 * a[1] * a[3])) / (6 * a[3]);
168 
169  return Tmax * c / 2.;
170 }
171 
172 template<typename T>
173 double RemoveSensorBiasDataPointsFilter<T>::ratioCurvature(const double depth, const T theta, const double aperture) const
174 {
175  static constexpr double epsilon = 1e-5;
176  const std::array<double,4> a = getCoefficients(depth, theta, aperture);
177  const std::array<double,4> b = getCoefficients(depth, 0., aperture);
178 
179  double Tmax;
180 
181  if(theta < epsilon) // approx. 0
182  Tmax = 0.;
183  else // theta >0
184  Tmax = ( -2 * a[2] - std::sqrt(4 * std::pow(a[2], 2) - 12 * a[1] * a[3])) / (6 * a[3]);
185 
186  return 1. - 2 * b[2] / (2 * a[2] + 6 * Tmax * a[3]);
187 }
188 
RemoveSensorBiasDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
Definition: RemoveSensorBias.cpp:66
build_map.T
T
Definition: build_map.py:34
RemoveSensorBiasDataPointsFilter::Parameters
Parametrizable::Parameters Parameters
Definition: RemoveSensorBias.h:45
PointMatcher::DataPoints::descriptorExists
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
Definition: pointmatcher/DataPoints.cpp:583
RemoveSensorBiasDataPointsFilter
Definition: RemoveSensorBias.h:41
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcherPrivate.h
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
RemoveSensorBiasDataPointsFilter::Vector
PM::Vector Vector
Definition: RemoveSensorBias.h:54
RemoveSensorBiasDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy.
Definition: RemoveSensorBias.cpp:58
RemoveSensorBias.h
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
RemoveSensorBiasDataPointsFilter::RemoveSensorBiasDataPointsFilter
RemoveSensorBiasDataPointsFilter(const Parameters &params=Parameters())
Definition: RemoveSensorBias.cpp:48
align_sequence.params
params
Definition: align_sequence.py:13
RemoveSensorBiasDataPointsFilter::getCoefficients
std::array< double, 4 > getCoefficients(const double depth, const T theta, const double aperture) const
Definition: RemoveSensorBias.cpp:132
PointMatcher::DataPoints::setColFrom
void setColFrom(Index thisCol, const DataPoints &that, Index thatCol)
Set column thisCol equal to column thatCol of that, copy features and descriptors if any....
Definition: pointmatcher/DataPoints.cpp:393
PointMatcher::DataPointsFilter
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
Definition: PointMatcher.h:440
InvalidParameter
Parametrizable::InvalidParameter InvalidParameter
Definition: pypoint_matcher_helper.h:42
PointMatcher::DataPoints::getNbPoints
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
Definition: pointmatcher/DataPoints.cpp:158
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
PointMatcherSupport::Parametrizable
The superclass of classes that are constructed using generic parameters. This class provides the para...
Definition: Parametrizable.h:141
PointMatcherSupport::get
const M::mapped_type & get(const M &m, const typename M::key_type &k)
Definition: Bibliography.cpp:57
icp_advance_api.dim
dim
Definition: icp_advance_api.py:152
RemoveSensorBiasDataPointsFilter::ratioCurvature
double ratioCurvature(const double depth, const T theta, const double aperture) const
Definition: RemoveSensorBias.cpp:173
PointMatcher::DataPoints::conservativeResize
void conservativeResize(Index pointCount)
Resize the cloud to pointCount points, conserving existing ones.
Definition: pointmatcher/DataPoints.cpp:328
PointMatcher::DataPoints::getDescriptorViewByName
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:555
PointMatcherSupport::pow
constexpr T pow(const T base, const std::size_t exponent)
Definition: utils.h:47
RemoveSensorBiasDataPointsFilter::diffDist
double diffDist(const double depth, const T theta, const double aperture) const
Definition: RemoveSensorBias.cpp:156
RemoveSensorBiasDataPointsFilter::SensorType
SensorType
Definition: RemoveSensorBias.h:80


libpointmatcher
Author(s):
autogenerated on Mon Jan 1 2024 03:24:43