8000 Add parameters for depth sensor uncertainty equation · geromidg/GA_SLAM@25c72e8 · GitHub
[go: up one dir, main page]
Skip to content

Commit 25c72e8

Browse files
geromidggeromidg
committed
Add parameters for depth sensor uncertainty equation
1 parent b68d70a commit 25c72e8

File tree

5 files changed

+61
-12
lines changed

5 files changed

+61
-12
lines changed

‎ga_slam/GaSlam.cc

+7-2
Original file line numberDiff line numberDiff line change
@@ -47,13 +47,17 @@ GaSlam::GaSlam(void)
4747
void GaSlam::configure(
4848
double mapLength, double mapResolution,
4949
double minElevation, double maxElevation, double voxelSize,
50-
int numParticles, int resampleFrequency,
50+
double depthSigmaCoeff1, double depthSigmaCoeff2,
51+
double depthSigmaCoeff3, int numParticles, int resampleFrequency,
5152
double initialSigmaX, double initialSigmaY, double initialSigmaYaw,
5253
double predictSigmaX, double predictSigmaY, double predictSigmaYaw,
5354
double traversedDistanceThreshold, double minSlopeThreshold,
5455
double slopeSumThresholdMultiplier, double matchAcceptanceThreshold,
5556
double globalMapLength, double globalMapResolution) {
5657
voxelSize_ = voxelSize;
58+
depthSigmaCoeff1_ = depthSigmaCoeff1;
59+
depthSigmaCoeff2_ = depthSigmaCoeff2;
60+
depthSigmaCoeff3_ = depthSigmaCoeff3;
5761

5862
poseEstimation_.configure(numParticles, resampleFrequency,
5963
initialSigmaX, initialSigmaY, initialSigmaYaw,
@@ -91,7 +95,8 @@ void GaSlam::cloudCallback(
9195
std::vector<float> cloudVariances;
9296

9397
CloudProcessing::processCloud(cloud, processedCloud, cloudVariances,
94-
getPose(), mapToSensorTF, mapParameters, voxelSize_);
98+
getPose(), mapToSensorTF, mapParameters, voxelSize_,
99+
depthSigmaCoeff1_, depthSigmaCoeff2_, depthSigmaCoeff3_);
95100

96101
dataRegistration_.updateMap(processedCloud, cloudVariances);
97102

‎ga_slam/GaSlam.h

+13-1
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,12 @@ class GaSlam {
9191
* @param[in] minElevation minimum cutoff height of the voxelized cloud
9292
* @param[in] maxElevation maximum cutoff height of the voxelized cloud
9393
* @param[in] voxelSize dimension of each voxel of the point cloud
94+
* @param[in] depthSigmaCoeff1 the first coefficient of the uncertainty
95+
* equation of the depth sensor
96+
* @param[in] depthSigmaCoeff2 the second coefficient of the uncertainty
97+
* equation of the depth sensor
98+
* @param[in] depthSigmaCoeff3 the third coefficient of the uncertainty
99+
* equation of the depth sensor
94100
* @param[in] numParticles number of particles used in the particle filter
95101
* @param[in] resampleFrequency number of iterations of the particle filter
96102
* before resampling
@@ -117,7 +123,8 @@ class GaSlam {
117123
void configure(
118124
double mapLength, double mapResolution,
119125
double minElevation, double maxElevation, double voxelSize,
120-
int numParticles, int resampleFrequency,
126+
double depthSigmaCoeff1, double depthSigmaCoeff2,
127+
double depthSigmaCoeff3, int numParticles, int resampleFrequency,
121128
double initialSigmaX, double initialSigmaY, double initialSigmaYaw,
122129
double predictSigmaX, double predictSigmaY, double predictSigmaYaw,
123130
double traversedDistanceThreshold, double minSlopeThreshold,
@@ -196,6 +203,11 @@ class GaSlam {
196203

197204
/// Voxel size of the downsampled point cloud after it it processed
198205
double voxelSize_;
206+
207+
/// Coefficients of the uncertainty equation of the depth sensor
208+
double depthSigmaCoeff1_;
209+
double depthSigmaCoeff2_;
210+
double depthSigmaCoeff3_;
199211
};
200212

201213
} // namespace ga_slam

‎ga_slam/localization/PoseCorrection.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ void PoseCorrection::configure(
6161
void PoseCorrection::createGlobalMap(
6262
const Cloud::ConstPtr& globalCloud,
6363
const Pose& globalCloudPose) {
64-
constexpr double globalCloudVariance = 1.f;
64+
constexpr float globalCloudVariance = 1.f;
6565
std::vector<float> globalCloudVariances;
6666
globalCloudVariances.resize(globalCloud->size(), globalCloudVariance);
6767

‎ga_slam/processing/CloudProcessing.cc

+17-5
Original file line numberDiff line numberDiff line change
@@ -47,11 +47,15 @@ void CloudProcessing::processCloud(
4747
const Pose& robotPose,
4848
const Pose& mapToSensorTF,
4949
const MapParameters& mapParameters,
50-
double voxelSize) {
50+
double voxelSize,
51+
double depthSigmaCoeff1,
52+
double depthSigmaCoeff2,
53+
double depthSigmaCoeff3) {
5154
downsampleCloud(inputCloud, outputCloud, voxelSize);
5255
transformCloudToMap(outputCloud, mapToSensorTF);
5356
cropCloudToMap(outputCloud, robotPose, mapParameters);
54-
calculateCloudVariances(outputCloud, cloudVariances);
57+
calculateCloudVariances(outputCloud, cloudVariances,
58+
depthSigmaCoeff1, depthSigmaCoeff2, depthSigmaCoeff3);
5559
}
5660

5761
void CloudProcessing::downsampleCloud(
@@ -92,12 +96,20 @@ void CloudProcessing::cropCloudToMap(
9296

9397
void CloudProcessing::calculateCloudVariances(
9498
const Cloud::ConstPtr& cloud,
95-
std::vector<float>& variances) {
99+
std::vector<float>& variances,
100+
double depthSigmaCoeff1,
101+
double depthSigmaCoeff2,
102+
double depthSigmaCoeff3) {
96103
variances.clear();
97104
variances.reserve(cloud->size());
98105

99-
for (const auto& point : cloud->points)
100-
variances.push_back(1.f);
106+
for (const auto& point : cloud->points) {
107+
const auto depth = Eigen::Vector3d(point.x, point.y, point.z).norm();
108+
const auto sigma = depthSigmaCoeff1 * (depth * depth) +
109+
depthSigmaCoeff2 * depth + depthSigmaCoeff3;
110+
111+
variances.push_back(sigma * sigma);
112+
}
101113
}
102114

103115
void CloudProcessing::convertMapToCloud(const Map& map, Cloud::Ptr& cloud) {

‎ga_slam/processing/CloudProcessing.h

+23-3
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,12 @@ class CloudProcessing {
5353
* @param[in] sensorToMapTF the transformation to be applied to the cloud
5454
* @param[in] mapParameters the structure containing the map's parameters
5555
* @param[in] voxelSize the voxel size needed to downsample the cloud
56+
* @param[in] depthSigmaCoeff1 the first coefficient of the uncertainty
57+
* equation of the depth sensor
58+
* @param[in] depthSigmaCoeff2 the second coefficient of the uncertainty
59+
* equation of the depth sensor
60+
* @param[in] depthSigmaCoeff3 the third coefficient of the uncertainty
61+
* equation of the depth sensor
5662
*/
5763
static void processCloud(
5864
const Cloud::ConstPtr& inputCloud,
@@ -61,7 +67,10 @@ class CloudProcessing {
6167
const Pose& robotPose,
6268
const Pose& sensorToMapTF,
6369
const MapParameters& mapParameters,
64-
double voxelSize);
70+
double voxelSize,
71+
double depthSigmaCoeff1,
72+
double depthSigmaCoeff2,
73+
double depthSigmaCoeff3);
6574

6675
/** Downsamples a point cloud using a voxel grid to the specified resolution
6776
* @param[in] inputCloud the point cloud to be downsampled
@@ -89,13 +98,24 @@ class CloudProcessing {
8998
const MapParameters& mapParameters);
9099

91100
/** Calculates a variance value for each point of the point cloud using the
92-
* sensor's model
101+
* sensor's model. To make the calculation sensor agnostic, a quadratic
102+
* equation models the uncertainty depending on the distance of each
103+
* points
93104
* @param[in] cloud the input point cloud
94105
* @param[out] variances the vector of the computed variances
106+
* @param[in] depthSigmaCoeff1 the first coefficient of the uncertainty
107+
* equation of the depth sensor
108+
* @param[in] depthSigmaCoeff2 the second coefficient of the uncertainty
109+
* equation of the depth sensor
110+
* @param[in] depthSigmaCoeff3 the third coefficient of the uncertainty
111+
* equation of the depth sensor
95112
*/
96113
static void calculateCloudVariances(
97114
const Cloud::ConstPtr& cloud,
98-
std::vector<float>& variances);
115+
std::vector<float>& variances,
116+
double depthSigmaCoeff1,
117+
double depthSigmaCoeff2,
118+
double depthSigmaCoeff3);
99119

100120
/** Converts an elevation map to a non-organized dense point cloud
101121
* @param[in] map the elevation map to be converted

0 commit comments

Comments
 (0)
0