From 425f9f53f39e5d22857d911e8435cb6361362cc0 Mon Sep 17 00:00:00 2001 From: Ahmed Abdelmawgoud Date: Mon, 23 Jul 2018 14:32:52 -0500 Subject: [PATCH 1/3] intial commit for noise model, not working --- include/gl_depth_sim/interfaces/noise_model.h | 17 ++++++++++++ src/camera_ros_example.cpp | 2 ++ src/interfaces/noise_model.cpp | 26 +++++++++++++++++++ 3 files changed, 45 insertions(+) create mode 100644 include/gl_depth_sim/interfaces/noise_model.h create mode 100644 src/interfaces/noise_model.cpp diff --git a/include/gl_depth_sim/interfaces/noise_model.h b/include/gl_depth_sim/interfaces/noise_model.h new file mode 100644 index 0000000..e4745fb --- /dev/null +++ b/include/gl_depth_sim/interfaces/noise_model.h @@ -0,0 +1,17 @@ +#ifndef NOISE_MODEL_H +#define NOISE_MODEL_H + +#include +#include +#include +#include +#include +#include +#include + +namespace gl_depth_sim +{ +std::vector noise(std::vector distance); + +} +#endif // NOISE_MODEL_H diff --git a/src/camera_ros_example.cpp b/src/camera_ros_example.cpp index 32580d7..bc12982 100644 --- a/src/camera_ros_example.cpp +++ b/src/camera_ros_example.cpp @@ -99,6 +99,8 @@ int main(int argc, char** argv) const auto depth_img = sim.render(pose); + depth_img= noise(depth_img); + frame_counter++; if (frame_counter % 100 == 0) diff --git a/src/interfaces/noise_model.cpp b/src/interfaces/noise_model.cpp new file mode 100644 index 0000000..8f87281 --- /dev/null +++ b/src/interfaces/noise_model.cpp @@ -0,0 +1,26 @@ +#include "gl_depth_sim/interfaces/noise_model.h" + +gl_depth_sim::vector noise(gl_depth_sim::vector distance) +{ + float segma; + std::seed_seq seed2{r(), r(), r(), r(), r(), r(), r(), r()}; + std::mt19937 e2(seed2); + + for (int i = 0 ; i < distance.size() ;++i){ + // Seed with a real random value, if available + //std::random_device r; + segma= 0.0012+0.0019* pow(distance[i]-0.4,2); + float r = distance[i]; + // Choose a random mean between 1 and 6 + //std::default_random_engine e1(r()); + //std::uniform_int_distribution uniform_dist(r-segma, r+segma); + //int mean = uniform_dist(e1); + + // Generate a normal distribution around that mean + std::normal_distribution<> normal_dist(r, segma); + + float new_value = r + normal_dist(e2); + + } + return distance; +} From 1e0e5dcc752b7239a542a900ccc18b973a642301 Mon Sep 17 00:00:00 2001 From: Ahmed Abdelmawgoud Date: Mon, 23 Jul 2018 17:00:21 -0500 Subject: [PATCH 2/3] CMakelist for my intial commit --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8617311..a0fe853 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,6 +57,7 @@ target_link_libraries(${PROJECT_NAME} add_library(${PROJECT_NAME}_interfaces src/interfaces/pcl_interface.cpp src/interfaces/opencv_interface.cpp + src/interfaces/noise_model.cpp ) add_dependencies(${PROJECT_NAME}_interfaces ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) From 3e157f1c6be8a3d96dc18b55592adb6863922b7b Mon Sep 17 00:00:00 2001 From: Ahmed Abdelmawgoud Date: Wed, 25 Jul 2018 10:43:20 -0500 Subject: [PATCH 3/3] working model with noise reary for review --- include/gl_depth_sim/camera_properties.h | 8 ++++++-- include/gl_depth_sim/interfaces/noise_model.h | 2 +- src/camera_ros_example.cpp | 6 +++--- src/interfaces/noise_model.cpp | 19 ++++++------------- 4 files changed, 16 insertions(+), 19 deletions(-) diff --git a/include/gl_depth_sim/camera_properties.h b/include/gl_depth_sim/camera_properties.h index 8635ac2..867c7da 100644 --- a/include/gl_depth_sim/camera_properties.h +++ b/include/gl_depth_sim/camera_properties.h @@ -1,8 +1,12 @@ #ifndef GL_DEPTH_SIM_CAMERA_PROPERTIES_H #define GL_DEPTH_SIM_CAMERA_PROPERTIES_H - +#include #include - +#include +#include +#include +#include +#include namespace gl_depth_sim { diff --git a/include/gl_depth_sim/interfaces/noise_model.h b/include/gl_depth_sim/interfaces/noise_model.h index e4745fb..674ec0a 100644 --- a/include/gl_depth_sim/interfaces/noise_model.h +++ b/include/gl_depth_sim/interfaces/noise_model.h @@ -11,7 +11,7 @@ namespace gl_depth_sim { -std::vector noise(std::vector distance); +std::vector noise(std::vector distance); } #endif // NOISE_MODEL_H diff --git a/src/camera_ros_example.cpp b/src/camera_ros_example.cpp index bc12982..dcfc8fc 100644 --- a/src/camera_ros_example.cpp +++ b/src/camera_ros_example.cpp @@ -1,7 +1,7 @@ #include "gl_depth_sim/sim_depth_camera.h" #include "gl_depth_sim/mesh_loader.h" #include "gl_depth_sim/interfaces/pcl_interface.h" - +#include "gl_depth_sim/interfaces/noise_model.h" #include #include #include @@ -97,9 +97,9 @@ int main(int argc, char** argv) const auto pose = lookat(camera_pos, look_at, Eigen::Vector3d(0,0,1)); - const auto depth_img = sim.render(pose); + auto depth_img = sim.render(pose); - depth_img= noise(depth_img); + depth_img.data = gl_depth_sim::noise(depth_img.data); frame_counter++; diff --git a/src/interfaces/noise_model.cpp b/src/interfaces/noise_model.cpp index 8f87281..f170264 100644 --- a/src/interfaces/noise_model.cpp +++ b/src/interfaces/noise_model.cpp @@ -1,26 +1,19 @@ #include "gl_depth_sim/interfaces/noise_model.h" -gl_depth_sim::vector noise(gl_depth_sim::vector distance) +std::vector gl_depth_sim::noise(std::vector distance) { float segma; + std::random_device r; std::seed_seq seed2{r(), r(), r(), r(), r(), r(), r(), r()}; std::mt19937 e2(seed2); for (int i = 0 ; i < distance.size() ;++i){ // Seed with a real random value, if available - //std::random_device r; - segma= 0.0012+0.0019* pow(distance[i]-0.4,2); + segma= 0.0012+0.0019* pow(distance[i]-0.4,2); float r = distance[i]; - // Choose a random mean between 1 and 6 - //std::default_random_engine e1(r()); - //std::uniform_int_distribution uniform_dist(r-segma, r+segma); - //int mean = uniform_dist(e1); - - // Generate a normal distribution around that mean - std::normal_distribution<> normal_dist(r, segma); - - float new_value = r + normal_dist(e2); - + // Generate a normal distribution around that mean + std::normal_distribution<> normal_dist(r, segma); + distance[i] = normal_dist(e2); } return distance; }