depth_noise_model.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2018 Michael Pantic, ASL, ETH Zurich, Switzerland
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
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   // Formula taken from the Intel Whitepaper:
00036   // "Best-Known-Methods for Tuning Intel RealSenseā„¢ D400 Depth Cameras for Best Performance".
00037   // We are using the theoretical RMS model formula.
00038   Eigen::VectorXf rms_noise = (data_vector_map * 1000.0).array().square() * multiplier;
00039   Eigen::VectorXf noise = rms_noise.array().square();
00040 
00041   // Sample noise for each pixel and transform variance according to error at this depth.
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   // Axial noise model from
00059   // https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=6375037,
00060   // Nguyen, Izadi & Lovell: "Modeling Kinect Sensor Noise for Improved 3D Reconstrucion and Tracking", 3DIM/3DPVT, 2012.
00061   // We are using the 10-60 Degree model as an approximation.
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   // Sample noise for each pixel and transform variance according to error at this depth.
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