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}) 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 new file mode 100644 index 0000000..674ec0a --- /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..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,7 +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.data = gl_depth_sim::noise(depth_img.data); frame_counter++; diff --git a/src/interfaces/noise_model.cpp b/src/interfaces/noise_model.cpp new file mode 100644 index 0000000..f170264 --- /dev/null +++ b/src/interfaces/noise_model.cpp @@ -0,0 +1,19 @@ +#include "gl_depth_sim/interfaces/noise_model.h" + +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 + segma= 0.0012+0.0019* pow(distance[i]-0.4,2); + float r = distance[i]; + // Generate a normal distribution around that mean + std::normal_distribution<> normal_dist(r, segma); + distance[i] = normal_dist(e2); + } + return distance; +}