Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include <algorithm>
00017 #include <iostream>
00018
00019 #include <rotors_gazebo_plugins/depth_noise_model.hpp>
00020
00021 inline bool DepthNoiseModel::InRange(const float depth) const {
00022 return depth > this->min_depth && depth < this->max_depth;
00023 }
00024
00025 void D435DepthNoiseModel::ApplyNoise(const uint32_t width,
00026 const uint32_t height, float *data) {
00027 if (data == nullptr) {
00028 return;
00029 }
00030
00031 float f = 0.5f * (width / tanf(h_fov / 2.0f));
00032 float multiplier = (subpixel_err) / (f * baseline * 1e6f);
00033 Eigen::Map<Eigen::VectorXf> data_vector_map(data, width * height);
00034
00035
00036
00037
00038 Eigen::VectorXf rms_noise = (data_vector_map * 1000.0).array().square() * multiplier;
00039 Eigen::VectorXf noise = rms_noise.array().square();
00040
00041
00042 for (int i = 0; i < width * height; ++i) {
00043 if (InRange(data_vector_map[i])) {
00044 data_vector_map[i] +=
00045 this->dist(this->gen) * std::min(((float)noise(i)), max_stdev);
00046 } else {
00047 data_vector_map[i] = this->bad_point;
00048 }
00049 }
00050 }
00051
00052 void KinectDepthNoiseModel::ApplyNoise(const uint32_t width,
00053 const uint32_t height, float *data) {
00054 if (data == nullptr) {
00055 return;
00056 }
00057
00058
00059
00060
00061
00062 Eigen::Map<Eigen::VectorXf> data_vector_map(data, width * height);
00063 Eigen::VectorXf var_noise = 0.0012f + 0.0019f * (data_vector_map.array() - 0.4f).array().square();
00064
00065
00066 for (int i = 0; i < width * height; ++i) {
00067 if (InRange(data_vector_map[i])) {
00068 data_vector_map[i] += this->dist(this->gen) * var_noise(i);
00069 } else {
00070 data_vector_map[i] = this->bad_point;
00071 }
00072 }
00073 }
rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43