Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #pragma once
00036
00037 #include "PointMatcher.h"
00038 #include <array>
00039
00040 template<typename T>
00041 struct RemoveSensorBiasDataPointsFilter: public PointMatcher<T>::DataPointsFilter
00042 {
00043 typedef PointMatcherSupport::Parametrizable Parametrizable;
00044 typedef PointMatcherSupport::Parametrizable P;
00045 typedef Parametrizable::Parameters Parameters;
00046 typedef Parametrizable::ParameterDoc ParameterDoc;
00047 typedef Parametrizable::ParametersDoc ParametersDoc;
00048 typedef Parametrizable::InvalidParameter InvalidParameter;
00049
00050 typedef PointMatcher<T> PM;
00051 typedef typename PM::DataPoints DataPoints;
00052 typedef typename PM::DataPointsFilter DataPointsFilter;
00053 typedef typename PM::Matrix Matrix;
00054 typedef typename PM::Vector Vector;
00055
00056 typedef typename DataPoints::InvalidField InvalidField;
00057
00058 inline static const std::string description()
00059 {
00060 return "Remove the bias induced by the angle of incidence\n\n"
00061 "Required descriptors: incidenceAngles, observationDirections.\n"
00062 "Produced descritors: none.\n"
00063 "Altered descriptors: none.\n"
00064 "Altered features: points coordinates and number of points.";
00065 }
00066
00067 inline static const ParametersDoc availableParameters()
00068 {
00069 return {
00070 {"sensorType", "Type of the sensor used. Choices: 0=Sick LMS-1xx, 1=Velodyne HDL-32E", "0", "0", "1", &P::Comp < int >},
00071 {"angleThreshold", "Threshold at which angle the correction is not applied, in degrees", "88.", "0.", "90.", &P::Comp < T >}
00072 };
00073 }
00074
00075 RemoveSensorBiasDataPointsFilter(const Parameters& params = Parameters());
00076 virtual DataPoints filter(const DataPoints& input);
00077 virtual void inPlaceFilter(DataPoints& cloud);
00078
00079 private:
00080 enum SensorType : int { LMS_1XX=0, HDL_32E=1 };
00081
00082 struct SensorParameters{
00083 private:
00084 SensorParameters(double aperture_, double k1_, double k2_): aperture(aperture_), k1(k1_), k2(k2_) {}
00085 public:
00086 const double aperture;
00087 const double k1;
00088 const double k2;
00089
00090 static const SensorParameters LMS_1XX;
00091 static const SensorParameters HDL_32E;
00092 };
00093
00094 static constexpr double tau = 50e-9;
00095 static constexpr double pulse_intensity = 0.39;
00096 static constexpr double lambda_light = 905e-9;
00097 static constexpr double c = 299792458.0;
00098
00099 const SensorType sensorType;
00100 const T angleThreshold;
00101
00102 std::array<double,4> getCoefficients(const double depth, const T theta, const double aperture) const;
00103 double diffDist(const double depth, const T theta, const double aperture) const;
00104 double ratioCurvature(const double depth, const T theta, const double aperture) const;
00105 };
00106
00107 template<typename T>
00108 const typename RemoveSensorBiasDataPointsFilter<T>::SensorParameters RemoveSensorBiasDataPointsFilter<T>::SensorParameters::LMS_1XX =
00109 RemoveSensorBiasDataPointsFilter<T>::SensorParameters(0.0075049, 6.08040951e+00, 3.17921789e-03 );
00110
00111 template<typename T>
00112 const typename RemoveSensorBiasDataPointsFilter<T>::SensorParameters RemoveSensorBiasDataPointsFilter<T>::SensorParameters::HDL_32E =
00113 RemoveSensorBiasDataPointsFilter<T>::SensorParameters(0.0014835, 1.03211569e+01, 7.07893371e-03);
00114
00115