├── .gitignore ├── 01_Screenshots ├── BlenderAddon_BSLAMSIM_BlenderUI.png ├── BlenderAddon_BSLAMSIM_Part1.jpg └── BlenderAddon_BSLAMSIM_Part2.jpg ├── 02_Utilities ├── BlenderAddon │ ├── addon_vslam_groundtruth_Blender279.py │ ├── addon_vslam_groundtruth_Blender280.py │ ├── addon_vslam_groundtruth_Blender300.py │ └── addon_vslam_groundtruth_Blender351.py └── FusionLinearKalmanFilter │ ├── 01_LinearKalmanFilter_allEvaluations.blend │ ├── 01_LinearKalmanFilter_allEvaluations.py │ ├── BlenderPlot.py │ ├── Blender_DSO_Destructed Loop 1m Only DSO updates_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_DSO_Linear Constant Velocity 1m Only DSO updates_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_DSO_SceneCity Loop 1m Only DSO updates_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_DSO_Venice Loop 1m Only DSO updates_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Position Manually scaled_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Position Manually scaled_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Position Manually scaled_4.84254_-72.6998_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Position raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Position raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Position raw_4.84254_-72.6998_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Velocity raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Velocity raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 10m GPS Velocity raw_4.84254_-72.6998_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Position Manually scaled_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Position Manually scaled_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Position Manually scaled_4.84254_-72.6998_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Position raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Position raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Position raw_4.84254_-72.6998_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Velocity raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Velocity raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Destructed Loop 1m GPS Velocity raw_4.84254_-72.6998_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 10m GPS Position Manually scaled_-30.0_-9.3_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 10m GPS Position Manually scaled_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 10m GPS Position Manually scaled_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 10m GPS Position raw_-30.0_-9.3_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 10m GPS Position raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 10m GPS Position raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 10m GPS Velocity raw_-30.0_-9.3_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 10m GPS Velocity raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Position Manually scaled_-30.0_-9.3_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Position Manually scaled_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Position Manually scaled_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Position raw_-30.0_-9.3_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Position raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Position raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Velocity raw_-30.0_-9.3_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Velocity raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Linear Constant Velocity 1m GPS Velocity raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 10m GPS Position Manually scaled_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 10m GPS Position Manually scaled_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 10m GPS Position Manually scaled_1.02_-4_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 10m GPS Position raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 10m GPS Velocity raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 10m GPS Velocity raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 10m GPS Velocity raw_1.02_-4_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Position Manually scaled_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Position Manually scaled_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Position Manually scaled_1.02_-4_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Position raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Position raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Position raw_1.02_-4_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Velocity raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Velocity raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCity Loop 1m GPS Velocity raw_1.02_-4_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCityLoop 10m GPS Position raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_SceneCityLoop 10m GPS Position raw_1.02_-4_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Position Manually scaled_-992.412_362.021_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Position Manually scaled_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Position Manually scaled_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Position raw_-992.412_362.021_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Position raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Position raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Velocity raw_-992.412_362.021_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Velocity raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 10m GPS Velocity raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Position Manually scaled_-992.412_362.021_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Position Manually scaled_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Position Manually scaled_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Position raw_-992.412_362.021_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Position raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Position raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Velocity raw_-992.412_362.021_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Velocity raw_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS+DSO_Venice Loop 1m GPS Velocity raw_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_Destructed Loop 1m Only GPS Updates, every 50th frame, initialized_4.84254_-72.6998_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_Destructed Loop 1m Only GPS Updates, every 50th frame_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_Destructed Loop 1m Only GPS Updates, every frame_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_Linear Constant Velocity 1m Only GPS Updates, every 50th frame, initialized_-30.0_-9.3_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_Linear Constant Velocity 1m Only GPS Updates, every 50th frame_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_Linear Constant Velocity 1m Only GPS Updates, every frame_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_SceneCity Loop 1m Only GPS Updates, every 50th frame, initialized_1.02_-4_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_SceneCity Loop 1m Only GPS Updates, every 50th frame_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_SceneCity Loop 1m Only GPS Updates, every frame_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_Venice Loop 1m Only GPS Updates, every 50th frame, initialized_-992.412_362.021_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ ├── Blender_GPS_Venice Loop 1m Only GPS Updates, every 50th frame_0.0_0.0_0.0_50_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt │ └── Blender_GPS_Venice Loop 1m Only GPS Updates, every frame_0.0_0.0_0.0_1_[1.0, 1.0]_[5.0, 5.0, 5.0]_0.02_0.4_0.2_0.2.txt ├── 03_Application ├── dso │ ├── CMakeLists.txt │ └── src │ │ ├── FullSystem │ │ ├── CoarseInitializer.cpp │ │ ├── CoarseInitializer.h │ │ ├── CoarseTracker.cpp │ │ ├── CoarseTracker.h │ │ ├── FullSystem.cpp │ │ ├── FullSystem.h │ │ ├── FullSystemDebugStuff.cpp │ │ ├── FullSystemMarginalize.cpp │ │ ├── FullSystemOptPoint.cpp │ │ ├── FullSystemOptimize.cpp │ │ ├── HessianBlocks.cpp │ │ ├── HessianBlocks.h │ │ ├── ImmaturePoint.cpp │ │ ├── ImmaturePoint.h │ │ ├── PixelSelector.h │ │ ├── PixelSelector2.cpp │ │ ├── PixelSelector2.h │ │ ├── ResidualProjections.h │ │ ├── Residuals.cpp │ │ └── Residuals.h │ │ ├── IOWrapper │ │ ├── ImageDisplay.h │ │ ├── ImageDisplay_dummy.cpp │ │ ├── ImageRW.h │ │ ├── ImageRW_dummy.cpp │ │ ├── OpenCV │ │ │ ├── ImageDisplay_OpenCV.cpp │ │ │ └── ImageRW_OpenCV.cpp │ │ ├── Output3DWrapper.h │ │ ├── OutputWrapper │ │ │ └── SampleOutputWrapper.h │ │ └── Pangolin │ │ │ ├── KeyFrameDisplay.cpp │ │ │ ├── KeyFrameDisplay.h │ │ │ ├── PangolinDSOViewer.cpp │ │ │ └── PangolinDSOViewer.h │ │ ├── OptimizationBackend │ │ ├── AccumulatedSCHessian.cpp │ │ ├── AccumulatedSCHessian.h │ │ ├── AccumulatedTopHessian.cpp │ │ ├── AccumulatedTopHessian.h │ │ ├── EnergyFunctional.cpp │ │ ├── EnergyFunctional.h │ │ ├── EnergyFunctionalStructs.cpp │ │ ├── EnergyFunctionalStructs.h │ │ ├── MatrixAccumulators.h │ │ └── RawResidualJacobian.h │ │ ├── main_dso_pangolin.cpp │ │ └── util │ │ ├── DatasetReader.h │ │ ├── FrameShell.h │ │ ├── HTTPPOSTRequest.h │ │ ├── ImageAndExposure.h │ │ ├── IndexThreadReduce.h │ │ ├── KalmanFilter.cpp │ │ ├── KalmanFilter.h │ │ ├── MinimalImage.h │ │ ├── NumType.h │ │ ├── UDPServer.cpp │ │ ├── UDPServer.h │ │ ├── Undistort.cpp │ │ ├── Undistort.h │ │ ├── globalCalib.cpp │ │ ├── globalCalib.h │ │ ├── globalFuncs.h │ │ ├── nanoflann.h │ │ ├── settings.cpp │ │ └── settings.h ├── dso_ros │ ├── .gitignore │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── dsoCameraZED_720P.txt │ ├── dsoCameraZED_FullHD.txt │ ├── package.xml │ ├── runDSOOffline.sh │ ├── runDSOOnline.sh │ ├── runDemo.sh │ └── src │ │ └── main.cpp ├── median_filter │ └── median_filter.py └── video2bag │ └── img2bag.py ├── 06_Data └── Logfiles │ ├── BlenderSceneCity_ConsoleLogFile.txt │ └── DSO_Pipeline_rough.sh ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | ## Core latex/pdflatex auxiliary files: 2 | *.aux 3 | *.lof 4 | *.log 5 | *.lot 6 | *.fls 7 | *.out 8 | *.toc 9 | *.fmt 10 | *.fot 11 | *.cb 12 | *.cb2 13 | 14 | ## Intermediate documents: 15 | *.dvi 16 | *-converted-to.* 17 | # these rules might exclude image files for figures etc. 18 | # *.ps 19 | # *.eps 20 | # *.pdf 21 | 22 | ## Generated if empty string is given at "Please type another file name for output:" 23 | .pdf 24 | 25 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 26 | *.bbl 27 | *.bcf 28 | *.blg 29 | *-blx.aux 30 | *-blx.bib 31 | *.run.xml 32 | 33 | ## Build tool auxiliary files: 34 | *.fdb_latexmk 35 | *.synctex 36 | *.synctex(busy) 37 | *.synctex.gz 38 | *.synctex.gz(busy) 39 | *.pdfsync 40 | 41 | ## Auxiliary and intermediate files from other packages: 42 | # algorithms 43 | *.alg 44 | *.loa 45 | 46 | # achemso 47 | acs-*.bib 48 | 49 | # amsthm 50 | *.thm 51 | 52 | # beamer 53 | *.nav 54 | *.pre 55 | *.snm 56 | *.vrb 57 | 58 | # changes 59 | *.soc 60 | 61 | # cprotect 62 | *.cpt 63 | 64 | # elsarticle (documentclass of Elsevier journals) 65 | *.spl 66 | 67 | # endnotes 68 | *.ent 69 | 70 | # fixme 71 | *.lox 72 | 73 | # feynmf/feynmp 74 | *.mf 75 | *.mp 76 | *.t[1-9] 77 | *.t[1-9][0-9] 78 | *.tfm 79 | 80 | #(r)(e)ledmac/(r)(e)ledpar 81 | *.end 82 | *.?end 83 | *.[1-9] 84 | *.[1-9][0-9] 85 | *.[1-9][0-9][0-9] 86 | *.[1-9]R 87 | *.[1-9][0-9]R 88 | *.[1-9][0-9][0-9]R 89 | *.eledsec[1-9] 90 | *.eledsec[1-9]R 91 | *.eledsec[1-9][0-9] 92 | *.eledsec[1-9][0-9]R 93 | *.eledsec[1-9][0-9][0-9] 94 | *.eledsec[1-9][0-9][0-9]R 95 | 96 | # glossaries 97 | *.acn 98 | *.acr 99 | *.glg 100 | *.glo 101 | *.gls 102 | *.glsdefs 103 | 104 | # gnuplottex 105 | *-gnuplottex-* 106 | 107 | # gregoriotex 108 | *.gaux 109 | *.gtex 110 | 111 | # hyperref 112 | *.brf 113 | 114 | # knitr 115 | *-concordance.tex 116 | # TODO Comment the next line if you want to keep your tikz graphics files 117 | *.tikz 118 | *-tikzDictionary 119 | 120 | # listings 121 | *.lol 122 | 123 | # makeidx 124 | *.idx 125 | *.ilg 126 | *.ind 127 | *.ist 128 | 129 | # minitoc 130 | *.maf 131 | *.mlf 132 | *.mlt 133 | *.mtc[0-9]* 134 | *.slf[0-9]* 135 | *.slt[0-9]* 136 | *.stc[0-9]* 137 | 138 | # minted 139 | _minted* 140 | *.pyg 141 | 142 | # morewrites 143 | *.mw 144 | 145 | # nomencl 146 | *.nlo 147 | 148 | # pax 149 | *.pax 150 | 151 | # pdfpcnotes 152 | *.pdfpc 153 | 154 | # sagetex 155 | *.sagetex.sage 156 | *.sagetex.py 157 | *.sagetex.scmd 158 | 159 | # scrwfile 160 | *.wrt 161 | 162 | # sympy 163 | *.sout 164 | *.sympy 165 | sympy-plots-for-*.tex/ 166 | 167 | # pdfcomment 168 | *.upa 169 | *.upb 170 | 171 | # pythontex 172 | *.pytxcode 173 | pythontex-files-*/ 174 | 175 | # thmtools 176 | *.loe 177 | 178 | # TikZ & PGF 179 | *.dpth 180 | *.md5 181 | *.auxlock 182 | 183 | # todonotes 184 | *.tdo 185 | 186 | # easy-todo 187 | *.lod 188 | 189 | # xindy 190 | *.xdy 191 | 192 | # xypic precompiled matrices 193 | *.xyc 194 | 195 | # endfloat 196 | *.ttt 197 | *.fff 198 | 199 | # Latexian 200 | TSWLatexianTemp* 201 | 202 | ## Editors: 203 | # WinEdt 204 | *.bak 205 | *.sav 206 | 207 | # Texpad 208 | .texpadtmp 209 | 210 | # Kile 211 | *.backup 212 | 213 | # KBibTeX 214 | *~[0-9]* 215 | 216 | # auto folder when using emacs and auctex 217 | /auto/* 218 | 219 | # expex forward references with \gathertags 220 | *-tags.tex 221 | -------------------------------------------------------------------------------- /01_Screenshots/BlenderAddon_BSLAMSIM_BlenderUI.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GSORF/Visual-GPS-SLAM/338a63c2aaef6678de231e4323c60c22b6860248/01_Screenshots/BlenderAddon_BSLAMSIM_BlenderUI.png -------------------------------------------------------------------------------- /01_Screenshots/BlenderAddon_BSLAMSIM_Part1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GSORF/Visual-GPS-SLAM/338a63c2aaef6678de231e4323c60c22b6860248/01_Screenshots/BlenderAddon_BSLAMSIM_Part1.jpg -------------------------------------------------------------------------------- /01_Screenshots/BlenderAddon_BSLAMSIM_Part2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GSORF/Visual-GPS-SLAM/338a63c2aaef6678de231e4323c60c22b6860248/01_Screenshots/BlenderAddon_BSLAMSIM_Part2.jpg -------------------------------------------------------------------------------- /02_Utilities/FusionLinearKalmanFilter/01_LinearKalmanFilter_allEvaluations.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GSORF/Visual-GPS-SLAM/338a63c2aaef6678de231e4323c60c22b6860248/02_Utilities/FusionLinearKalmanFilter/01_LinearKalmanFilter_allEvaluations.blend -------------------------------------------------------------------------------- /03_Application/dso/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(PROJECT_NAME DSO) 2 | 3 | PROJECT(${PROJECT_NAME}) 4 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 5 | #set(CMAKE_VERBOSE_MAKEFILE ON) 6 | 7 | 8 | #set(BUILD_TYPE Release) 9 | set(BUILD_TYPE RelWithDebInfo) 10 | 11 | set(EXECUTABLE_OUTPUT_PATH bin) 12 | set(LIBRARY_OUTPUT_PATH lib) 13 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 14 | 15 | # required libraries 16 | find_package(SuiteParse REQUIRED) 17 | find_package(Eigen3 REQUIRED) 18 | find_package(Boost COMPONENTS system thread) 19 | 20 | # optional libraries 21 | find_package(LibZip QUIET) 22 | find_package(Pangolin 0.2 QUIET) 23 | find_package(OpenCV QUIET) 24 | 25 | # flags 26 | add_definitions("-DENABLE_SSE") 27 | set(CMAKE_CXX_FLAGS 28 | "${SSE_FLAGS} -O3 -g -std=c++0x -march=native" 29 | # "${SSE_FLAGS} -O3 -g -std=c++0x -fno-omit-frame-pointer" 30 | ) 31 | 32 | if (MSVC) 33 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc") 34 | endif (MSVC) 35 | 36 | # Sources files 37 | set(dso_SOURCE_FILES 38 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystem.cpp 39 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemOptimize.cpp 40 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemOptPoint.cpp 41 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemDebugStuff.cpp 42 | ${PROJECT_SOURCE_DIR}/src/FullSystem/FullSystemMarginalize.cpp 43 | ${PROJECT_SOURCE_DIR}/src/FullSystem/Residuals.cpp 44 | ${PROJECT_SOURCE_DIR}/src/FullSystem/CoarseTracker.cpp 45 | ${PROJECT_SOURCE_DIR}/src/FullSystem/CoarseInitializer.cpp 46 | ${PROJECT_SOURCE_DIR}/src/FullSystem/ImmaturePoint.cpp 47 | ${PROJECT_SOURCE_DIR}/src/FullSystem/HessianBlocks.cpp 48 | ${PROJECT_SOURCE_DIR}/src/FullSystem/PixelSelector2.cpp 49 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/EnergyFunctional.cpp 50 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/AccumulatedTopHessian.cpp 51 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/AccumulatedSCHessian.cpp 52 | ${PROJECT_SOURCE_DIR}/src/OptimizationBackend/EnergyFunctionalStructs.cpp 53 | ${PROJECT_SOURCE_DIR}/src/util/settings.cpp 54 | ${PROJECT_SOURCE_DIR}/src/util/Undistort.cpp 55 | ${PROJECT_SOURCE_DIR}/src/util/globalCalib.cpp 56 | ${PROJECT_SOURCE_DIR}/src/util/KalmanFilter.cpp 57 | ${PROJECT_SOURCE_DIR}/src/util/UDPServer.cpp 58 | ) 59 | 60 | 61 | 62 | include_directories( 63 | ${PROJECT_SOURCE_DIR}/src 64 | ${PROJECT_SOURCE_DIR}/thirdparty/Sophus 65 | ${PROJECT_SOURCE_DIR}/thirdparty/sse2neon 66 | ${EIGEN3_INCLUDE_DIR} 67 | ) 68 | 69 | 70 | # decide if we have pangolin 71 | if (Pangolin_FOUND) 72 | message("--- found PANGOLIN, compiling dso_pangolin library.") 73 | include_directories( ${Pangolin_INCLUDE_DIRS} ) 74 | set(dso_pangolin_SOURCE_FILES 75 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/Pangolin/KeyFrameDisplay.cpp 76 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/Pangolin/PangolinDSOViewer.cpp) 77 | set(HAS_PANGOLIN 1) 78 | else () 79 | message("--- could not find PANGOLIN, not compiling dso_pangolin library.") 80 | message(" this means there will be no 3D display / GUI available for dso_dataset.") 81 | set(dso_pangolin_SOURCE_FILES ) 82 | set(HAS_PANGOLIN 0) 83 | endif () 84 | 85 | # decide if we have openCV 86 | if (OpenCV_FOUND) 87 | message("--- found OpenCV, compiling dso_opencv library.") 88 | include_directories( ${OpenCV_INCLUDE_DIRS} ) 89 | set(dso_opencv_SOURCE_FILES 90 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp 91 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp) 92 | set(HAS_OPENCV 1) 93 | else () 94 | message("--- could not find OpenCV, not compiling dso_opencv library.") 95 | message(" this means there will be no image display, and image read / load functionality.") 96 | set(dso_opencv_SOURCE_FILES 97 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/ImageDisplay_dummy.cpp 98 | ${PROJECT_SOURCE_DIR}/src/IOWrapper/ImageRW_dummy.cpp) 99 | set(HAS_OPENCV 0) 100 | endif () 101 | 102 | # decide if we have ziplib. 103 | if (LIBZIP_LIBRARY) 104 | message("--- found ziplib (${LIBZIP_VERSION}), compiling with zip capability.") 105 | add_definitions(-DHAS_ZIPLIB=1) 106 | include_directories( ${LIBZIP_INCLUDE_DIR_ZIP} ${LIBZIP_INCLUDE_DIR_ZIPCONF} ) 107 | else() 108 | message("--- not found ziplib (${LIBZIP_LIBRARY}), compiling without zip capability.") 109 | set(LIBZIP_LIBRARY "") 110 | endif() 111 | 112 | 113 | # compile main library. 114 | include_directories( ${CSPARSE_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR}) 115 | add_library(dso ${dso_SOURCE_FILES} ${dso_opencv_SOURCE_FILES} ${dso_pangolin_SOURCE_FILES}) 116 | 117 | #set_property( TARGET dso APPEND_STRING PROPERTY COMPILE_FLAGS -Wall ) 118 | 119 | 120 | if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # OSX 121 | set(BOOST_THREAD_LIBRARY boost_thread-mt) 122 | else() 123 | set(BOOST_THREAD_LIBRARY boost_thread) 124 | endif() 125 | 126 | # build main executable (only if we have both OpenCV and Pangolin) 127 | if (OpenCV_FOUND AND Pangolin_FOUND) 128 | message("--- compiling dso_dataset.") 129 | add_executable(dso_dataset ${PROJECT_SOURCE_DIR}/src/main_dso_pangolin.cpp ) 130 | target_link_libraries(dso_dataset dso boost_system cxsparse ${BOOST_THREAD_LIBRARY} ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ${OpenCV_LIBS}) 131 | else() 132 | message("--- not building dso_dataset, since either don't have openCV or Pangolin.") 133 | endif() 134 | 135 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/CoarseInitializer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | #include "OptimizationBackend/MatrixAccumulators.h" 29 | #include "IOWrapper/Output3DWrapper.h" 30 | #include "util/settings.h" 31 | #include "vector" 32 | #include 33 | 34 | 35 | 36 | 37 | namespace dso 38 | { 39 | struct CalibHessian; 40 | struct FrameHessian; 41 | 42 | 43 | struct Pnt 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 | // index in jacobian. never changes (actually, there is no reason why). 48 | float u,v; 49 | 50 | // idepth / isgood / energy during optimization. 51 | float idepth; 52 | bool isGood; 53 | Vec2f energy; // (UenergyPhotometric, energyRegularizer) 54 | bool isGood_new; 55 | float idepth_new; 56 | Vec2f energy_new; 57 | 58 | float iR; 59 | float iRSumNum; 60 | 61 | float lastHessian; 62 | float lastHessian_new; 63 | 64 | // max stepsize for idepth (corresponding to max. movement in pixel-space). 65 | float maxstep; 66 | 67 | // idx (x+y*w) of closest point one pyramid level above. 68 | int parent; 69 | float parentDist; 70 | 71 | // idx (x+y*w) of up to 10 nearest points in pixel space. 72 | int neighbours[10]; 73 | float neighboursDist[10]; 74 | 75 | float my_type; 76 | float outlierTH; 77 | }; 78 | 79 | class CoarseInitializer { 80 | public: 81 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 82 | CoarseInitializer(int w, int h); 83 | ~CoarseInitializer(); 84 | 85 | 86 | void setFirst( CalibHessian* HCalib, FrameHessian* newFrameHessian); 87 | bool trackFrame(FrameHessian* newFrameHessian, std::vector &wraps); 88 | void calcTGrads(FrameHessian* newFrameHessian); 89 | 90 | int frameID; 91 | bool fixAffine; 92 | bool printDebug; 93 | 94 | Pnt* points[PYR_LEVELS]; 95 | int numPoints[PYR_LEVELS]; 96 | AffLight thisToNext_aff; 97 | SE3 thisToNext; 98 | 99 | 100 | FrameHessian* firstFrame; 101 | FrameHessian* newFrame; 102 | private: 103 | Mat33 K[PYR_LEVELS]; 104 | Mat33 Ki[PYR_LEVELS]; 105 | double fx[PYR_LEVELS]; 106 | double fy[PYR_LEVELS]; 107 | double fxi[PYR_LEVELS]; 108 | double fyi[PYR_LEVELS]; 109 | double cx[PYR_LEVELS]; 110 | double cy[PYR_LEVELS]; 111 | double cxi[PYR_LEVELS]; 112 | double cyi[PYR_LEVELS]; 113 | int w[PYR_LEVELS]; 114 | int h[PYR_LEVELS]; 115 | void makeK(CalibHessian* HCalib); 116 | 117 | bool snapped; 118 | int snappedAt; 119 | 120 | // pyramid images & levels on all levels 121 | Eigen::Vector3f* dINew[PYR_LEVELS]; 122 | Eigen::Vector3f* dIFist[PYR_LEVELS]; 123 | 124 | Eigen::DiagonalMatrix wM; 125 | 126 | // temporary buffers for H and b. 127 | Vec10f* JbBuffer; // 0-7: sum(dd * dp). 8: sum(res*dd). 9: 1/(1+sum(dd*dd))=inverse hessian entry. 128 | Vec10f* JbBuffer_new; 129 | 130 | Accumulator9 acc9; 131 | Accumulator9 acc9SC; 132 | 133 | 134 | Vec3f dGrads[PYR_LEVELS]; 135 | 136 | float alphaK; 137 | float alphaW; 138 | float regWeight; 139 | float couplingWeight; 140 | 141 | Vec3f calcResAndGS( 142 | int lvl, 143 | Mat88f &H_out, Vec8f &b_out, 144 | Mat88f &H_out_sc, Vec8f &b_out_sc, 145 | const SE3 &refToNew, AffLight refToNew_aff, 146 | bool plot); 147 | Vec3f calcEC(int lvl); // returns OLD NERGY, NEW ENERGY, NUM TERMS. 148 | void optReg(int lvl); 149 | 150 | void propagateUp(int srcLvl); 151 | void propagateDown(int srcLvl); 152 | float rescale(); 153 | 154 | void resetPoints(int lvl); 155 | void doStep(int lvl, float lambda, Vec8f inc); 156 | void applyStep(int lvl); 157 | 158 | void makeGradients(Eigen::Vector3f** data); 159 | 160 | void debugPlot(int lvl, std::vector &wraps); 161 | void makeNN(); 162 | }; 163 | 164 | 165 | 166 | 167 | struct FLANNPointcloud 168 | { 169 | inline FLANNPointcloud() {num=0; points=0;} 170 | inline FLANNPointcloud(int n, Pnt* p) : num(n), points(p) {} 171 | int num; 172 | Pnt* points; 173 | inline size_t kdtree_get_point_count() const { return num; } 174 | inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t /*size*/) const 175 | { 176 | const float d0=p1[0]-points[idx_p2].u; 177 | const float d1=p1[1]-points[idx_p2].v; 178 | return d0*d0+d1*d1; 179 | } 180 | 181 | inline float kdtree_get_pt(const size_t idx, int dim) const 182 | { 183 | if (dim==0) return points[idx].u; 184 | else return points[idx].v; 185 | } 186 | template 187 | bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } 188 | }; 189 | 190 | } 191 | 192 | 193 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/CoarseTracker.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | #include "vector" 30 | #include 31 | #include "util/settings.h" 32 | #include "OptimizationBackend/MatrixAccumulators.h" 33 | #include "IOWrapper/Output3DWrapper.h" 34 | 35 | 36 | 37 | 38 | namespace dso 39 | { 40 | struct CalibHessian; 41 | struct FrameHessian; 42 | struct PointFrameResidual; 43 | 44 | class CoarseTracker { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 | 48 | CoarseTracker(int w, int h); 49 | ~CoarseTracker(); 50 | 51 | bool trackNewestCoarse( 52 | FrameHessian* newFrameHessian, 53 | SE3 &lastToNew_out, AffLight &aff_g2l_out, 54 | int coarsestLvl, Vec5 minResForAbort, 55 | IOWrap::Output3DWrapper* wrap=0); 56 | 57 | void setCoarseTrackingRef( 58 | std::vector frameHessians); 59 | 60 | void makeK( 61 | CalibHessian* HCalib); 62 | 63 | bool debugPrint, debugPlot; 64 | 65 | Mat33f K[PYR_LEVELS]; 66 | Mat33f Ki[PYR_LEVELS]; 67 | float fx[PYR_LEVELS]; 68 | float fy[PYR_LEVELS]; 69 | float fxi[PYR_LEVELS]; 70 | float fyi[PYR_LEVELS]; 71 | float cx[PYR_LEVELS]; 72 | float cy[PYR_LEVELS]; 73 | float cxi[PYR_LEVELS]; 74 | float cyi[PYR_LEVELS]; 75 | int w[PYR_LEVELS]; 76 | int h[PYR_LEVELS]; 77 | 78 | void debugPlotIDepthMap(float* minID, float* maxID, std::vector &wraps); 79 | void debugPlotIDepthMapFloat(std::vector &wraps); 80 | 81 | FrameHessian* lastRef; 82 | AffLight lastRef_aff_g2l; 83 | FrameHessian* newFrame; 84 | int refFrameID; 85 | 86 | // act as pure ouptut 87 | Vec5 lastResiduals; 88 | Vec3 lastFlowIndicators; 89 | double firstCoarseRMSE; 90 | private: 91 | 92 | 93 | void makeCoarseDepthL0(std::vector frameHessians); 94 | float* idepth[PYR_LEVELS]; 95 | float* weightSums[PYR_LEVELS]; 96 | float* weightSums_bak[PYR_LEVELS]; 97 | 98 | 99 | Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); 100 | Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH); 101 | void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); 102 | void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l); 103 | 104 | // pc buffers 105 | float* pc_u[PYR_LEVELS]; 106 | float* pc_v[PYR_LEVELS]; 107 | float* pc_idepth[PYR_LEVELS]; 108 | float* pc_color[PYR_LEVELS]; 109 | int pc_n[PYR_LEVELS]; 110 | 111 | // warped buffers 112 | float* buf_warped_idepth; 113 | float* buf_warped_u; 114 | float* buf_warped_v; 115 | float* buf_warped_dx; 116 | float* buf_warped_dy; 117 | float* buf_warped_residual; 118 | float* buf_warped_weight; 119 | float* buf_warped_refColor; 120 | int buf_warped_n; 121 | 122 | 123 | std::vector ptrToDelete; 124 | 125 | 126 | Accumulator9 acc; 127 | }; 128 | 129 | 130 | class CoarseDistanceMap { 131 | public: 132 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 133 | 134 | CoarseDistanceMap(int w, int h); 135 | ~CoarseDistanceMap(); 136 | 137 | void makeDistanceMap( 138 | std::vector frameHessians, 139 | FrameHessian* frame); 140 | 141 | void makeInlierVotes( 142 | std::vector frameHessians); 143 | 144 | void makeK( CalibHessian* HCalib); 145 | 146 | 147 | float* fwdWarpedIDDistFinal; 148 | 149 | Mat33f K[PYR_LEVELS]; 150 | Mat33f Ki[PYR_LEVELS]; 151 | float fx[PYR_LEVELS]; 152 | float fy[PYR_LEVELS]; 153 | float fxi[PYR_LEVELS]; 154 | float fyi[PYR_LEVELS]; 155 | float cx[PYR_LEVELS]; 156 | float cy[PYR_LEVELS]; 157 | float cxi[PYR_LEVELS]; 158 | float cyi[PYR_LEVELS]; 159 | int w[PYR_LEVELS]; 160 | int h[PYR_LEVELS]; 161 | 162 | void addIntoDistFinal(int u, int v); 163 | 164 | 165 | private: 166 | 167 | PointFrameResidual** coarseProjectionGrid; 168 | int* coarseProjectionGridNum; 169 | Eigen::Vector2i* bfsList1; 170 | Eigen::Vector2i* bfsList2; 171 | 172 | void growDistBFS(int bfsNum); 173 | }; 174 | 175 | } 176 | 177 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/FullSystemMarginalize.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | /* 26 | * KFBuffer.cpp 27 | * 28 | * Created on: Jan 7, 2014 29 | * Author: engelj 30 | */ 31 | 32 | #include "FullSystem/FullSystem.h" 33 | 34 | #include "stdio.h" 35 | #include "util/globalFuncs.h" 36 | #include 37 | #include 38 | #include "IOWrapper/ImageDisplay.h" 39 | #include "util/globalCalib.h" 40 | 41 | #include 42 | #include 43 | #include "FullSystem/ResidualProjections.h" 44 | #include "FullSystem/ImmaturePoint.h" 45 | 46 | #include "OptimizationBackend/EnergyFunctional.h" 47 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 48 | 49 | #include "IOWrapper/Output3DWrapper.h" 50 | 51 | #include "FullSystem/CoarseTracker.h" 52 | 53 | namespace dso 54 | { 55 | 56 | 57 | 58 | void FullSystem::flagFramesForMarginalization(FrameHessian* newFH) 59 | { 60 | if(setting_minFrameAge > setting_maxFrames) 61 | { 62 | for(int i=setting_maxFrames;i<(int)frameHessians.size();i++) 63 | { 64 | FrameHessian* fh = frameHessians[i-setting_maxFrames]; 65 | fh->flaggedForMarginalization = true; 66 | } 67 | return; 68 | } 69 | 70 | 71 | int flagged = 0; 72 | // marginalize all frames that have not enough points. 73 | for(int i=0;i<(int)frameHessians.size();i++) 74 | { 75 | FrameHessian* fh = frameHessians[i]; 76 | int in = fh->pointHessians.size() + fh->immaturePoints.size(); 77 | int out = fh->pointHessiansMarginalized.size() + fh->pointHessiansOut.size(); 78 | 79 | 80 | Vec2 refToFh=AffLight::fromToVecExposure(frameHessians.back()->ab_exposure, fh->ab_exposure, 81 | frameHessians.back()->aff_g2l(), fh->aff_g2l()); 82 | 83 | 84 | if( (in < setting_minPointsRemaining *(in+out) || fabs(logf((float)refToFh[0])) > setting_maxLogAffFacInWindow) 85 | && ((int)frameHessians.size())-flagged > setting_minFrames) 86 | { 87 | // printf("MARGINALIZE frame %d, as only %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n", 88 | // fh->frameID, in, in+out, 89 | // (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(), 90 | // (int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(), 91 | // visInLast, outInLast, 92 | // fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame); 93 | fh->flaggedForMarginalization = true; 94 | flagged++; 95 | } 96 | else 97 | { 98 | // printf("May Keep frame %d, as %'d/%'d points remaining (%'d %'d %'d %'d). VisInLast %'d / %'d. traces %d, activated %d!\n", 99 | // fh->frameID, in, in+out, 100 | // (int)fh->pointHessians.size(), (int)fh->immaturePoints.size(), 101 | // (int)fh->pointHessiansMarginalized.size(), (int)fh->pointHessiansOut.size(), 102 | // visInLast, outInLast, 103 | // fh->statistics_tracesCreatedForThisFrame, fh->statistics_pointsActivatedForThisFrame); 104 | } 105 | } 106 | 107 | // marginalize one. 108 | if((int)frameHessians.size()-flagged >= setting_maxFrames) 109 | { 110 | double smallestScore = 1; 111 | FrameHessian* toMarginalize=0; 112 | FrameHessian* latest = frameHessians.back(); 113 | 114 | 115 | for(FrameHessian* fh : frameHessians) 116 | { 117 | if(fh->frameID > latest->frameID-setting_minFrameAge || fh->frameID == 0) continue; 118 | //if(fh==frameHessians.front() == 0) continue; 119 | 120 | double distScore = 0; 121 | for(FrameFramePrecalc &ffh : fh->targetPrecalc) 122 | { 123 | if(ffh.target->frameID > latest->frameID-setting_minFrameAge+1 || ffh.target == ffh.host) continue; 124 | distScore += 1/(1e-5+ffh.distanceLL); 125 | 126 | } 127 | distScore *= -sqrtf(fh->targetPrecalc.back().distanceLL); 128 | 129 | 130 | if(distScore < smallestScore) 131 | { 132 | smallestScore = distScore; 133 | toMarginalize = fh; 134 | } 135 | } 136 | 137 | // printf("MARGINALIZE frame %d, as it is the closest (score %.2f)!\n", 138 | // toMarginalize->frameID, smallestScore); 139 | toMarginalize->flaggedForMarginalization = true; 140 | flagged++; 141 | } 142 | 143 | // printf("FRAMES LEFT: "); 144 | // for(FrameHessian* fh : frameHessians) 145 | // printf("%d ", fh->frameID); 146 | // printf("\n"); 147 | } 148 | 149 | 150 | 151 | 152 | void FullSystem::marginalizeFrame(FrameHessian* frame) 153 | { 154 | // marginalize or remove all this frames points. 155 | 156 | assert((int)frame->pointHessians.size()==0); 157 | 158 | 159 | ef->marginalizeFrame(frame->efFrame); 160 | 161 | // drop all observations of existing points in that frame. 162 | 163 | for(FrameHessian* fh : frameHessians) 164 | { 165 | if(fh==frame) continue; 166 | 167 | for(PointHessian* ph : fh->pointHessians) 168 | { 169 | for(unsigned int i=0;iresiduals.size();i++) 170 | { 171 | PointFrameResidual* r = ph->residuals[i]; 172 | if(r->target == frame) 173 | { 174 | if(ph->lastResiduals[0].first == r) 175 | ph->lastResiduals[0].first=0; 176 | else if(ph->lastResiduals[1].first == r) 177 | ph->lastResiduals[1].first=0; 178 | 179 | 180 | if(r->host->frameID < r->target->frameID) 181 | statistics_numForceDroppedResFwd++; 182 | else 183 | statistics_numForceDroppedResBwd++; 184 | 185 | ef->dropResidual(r->efResidual); 186 | deleteOut(ph->residuals,i); 187 | break; 188 | } 189 | } 190 | } 191 | } 192 | 193 | 194 | 195 | { 196 | std::vector v; 197 | v.push_back(frame); 198 | for(IOWrap::Output3DWrapper* ow : outputWrapper) 199 | ow->publishKeyframes(v, true, &Hcalib); 200 | } 201 | 202 | 203 | frame->shell->marginalizedAt = frameHessians.back()->shell->id; 204 | frame->shell->movedByOpt = frame->w2c_leftEps().norm(); 205 | 206 | deleteOutOrder(frameHessians, frame); 207 | for(unsigned int i=0;iidx = i; 209 | 210 | 211 | 212 | 213 | setPrecalcValues(); 214 | ef->setAdjointsF(&Hcalib); 215 | } 216 | 217 | 218 | 219 | 220 | } 221 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/FullSystemOptPoint.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | /* 26 | * KFBuffer.cpp 27 | * 28 | * Created on: Jan 7, 2014 29 | * Author: engelj 30 | */ 31 | 32 | #include "FullSystem/FullSystem.h" 33 | 34 | #include "stdio.h" 35 | #include "util/globalFuncs.h" 36 | #include 37 | #include 38 | #include "IOWrapper/ImageDisplay.h" 39 | #include "util/globalCalib.h" 40 | 41 | #include 42 | #include 43 | #include "FullSystem/ImmaturePoint.h" 44 | #include "math.h" 45 | 46 | namespace dso 47 | { 48 | 49 | 50 | 51 | PointHessian* FullSystem::optimizeImmaturePoint( 52 | ImmaturePoint* point, int minObs, 53 | ImmaturePointTemporaryResidual* residuals) 54 | { 55 | int nres = 0; 56 | for(FrameHessian* fh : frameHessians) 57 | { 58 | if(fh != point->host) 59 | { 60 | residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0; 61 | residuals[nres].state_NewState = ResState::OUTLIER; 62 | residuals[nres].state_state = ResState::IN; 63 | residuals[nres].target = fh; 64 | nres++; 65 | } 66 | } 67 | assert(nres == ((int)frameHessians.size())-1); 68 | 69 | bool print = false;//rand()%50==0; 70 | 71 | float lastEnergy = 0; 72 | float lastHdd=0; 73 | float lastbd=0; 74 | float currentIdepth=(point->idepth_max+point->idepth_min)*0.5f; 75 | 76 | 77 | 78 | 79 | 80 | 81 | for(int i=0;ilinearizeResidual(&Hcalib, 1000, residuals+i,lastHdd, lastbd, currentIdepth); 84 | residuals[i].state_state = residuals[i].state_NewState; 85 | residuals[i].state_energy = residuals[i].state_NewEnergy; 86 | } 87 | 88 | if(!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act) 89 | { 90 | if(print) 91 | printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n", 92 | nres, lastHdd, lastEnergy); 93 | return 0; 94 | } 95 | 96 | if(print) printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial Id=%f\n" , 97 | nres, lastHdd,lastEnergy,currentIdepth); 98 | 99 | float lambda = 0.1; 100 | for(int iteration=0;iterationlinearizeResidual(&Hcalib, 1, residuals+i,newHdd, newbd, newIdepth); 110 | 111 | if(!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act) 112 | { 113 | if(print) printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n", 114 | nres, 115 | newHdd, 116 | lastEnergy); 117 | return 0; 118 | } 119 | 120 | if(print) printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n", 121 | (true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT", 122 | iteration, 123 | log10(lambda), 124 | "", 125 | lastEnergy, newEnergy, newIdepth); 126 | 127 | if(newEnergy < lastEnergy) 128 | { 129 | currentIdepth = newIdepth; 130 | lastHdd = newHdd; 131 | lastbd = newbd; 132 | lastEnergy = newEnergy; 133 | for(int i=0;ienergyTH)) {delete p; return (PointHessian*)((long)(-1));} 171 | 172 | p->lastResiduals[0].first = 0; 173 | p->lastResiduals[0].second = ResState::OOB; 174 | p->lastResiduals[1].first = 0; 175 | p->lastResiduals[1].second = ResState::OOB; 176 | p->setIdepthZero(currentIdepth); 177 | p->setIdepth(currentIdepth); 178 | p->setPointStatus(PointHessian::ACTIVE); 179 | 180 | for(int i=0;ihost, residuals[i].target); 184 | r->state_NewEnergy = r->state_energy = 0; 185 | r->state_NewState = ResState::OUTLIER; 186 | r->setState(ResState::IN); 187 | p->residuals.push_back(r); 188 | 189 | if(r->target == frameHessians.back()) 190 | { 191 | p->lastResiduals[0].first = r; 192 | p->lastResiduals[0].second = ResState::IN; 193 | } 194 | else if(r->target == (frameHessians.size()<2 ? 0 : frameHessians[frameHessians.size()-2])) 195 | { 196 | p->lastResiduals[1].first = r; 197 | p->lastResiduals[1].second = ResState::IN; 198 | } 199 | } 200 | 201 | if(print) printf("point activated!\n"); 202 | 203 | statistics_numActivatedPoints++; 204 | return p; 205 | } 206 | 207 | 208 | 209 | } 210 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/HessianBlocks.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #include "FullSystem/HessianBlocks.h" 27 | #include "util/FrameShell.h" 28 | #include "FullSystem/ImmaturePoint.h" 29 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 30 | 31 | namespace dso 32 | { 33 | 34 | 35 | PointHessian::PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib) 36 | { 37 | instanceCounter++; 38 | host = rawPoint->host; 39 | hasDepthPrior=false; 40 | 41 | idepth_hessian=0; 42 | maxRelBaseline=0; 43 | numGoodResiduals=0; 44 | 45 | // set static values & initialization. 46 | u = rawPoint->u; 47 | v = rawPoint->v; 48 | assert(std::isfinite(rawPoint->idepth_max)); 49 | //idepth_init = rawPoint->idepth_GT; 50 | 51 | my_type = rawPoint->my_type; 52 | 53 | setIdepthScaled((rawPoint->idepth_max + rawPoint->idepth_min)*0.5); 54 | setPointStatus(PointHessian::INACTIVE); 55 | 56 | int n = patternNum; 57 | memcpy(color, rawPoint->color, sizeof(float)*n); 58 | memcpy(weights, rawPoint->weights, sizeof(float)*n); 59 | energyTH = rawPoint->energyTH; 60 | 61 | efPoint=0; 62 | 63 | 64 | } 65 | 66 | 67 | void PointHessian::release() 68 | { 69 | for(unsigned int i=0;i().squaredNorm() < 1e-20); 77 | 78 | this->state_zero = state_zero; 79 | 80 | 81 | for(int i=0;i<6;i++) 82 | { 83 | Vec6 eps; eps.setZero(); eps[i] = 1e-3; 84 | SE3 EepsP = Sophus::SE3::exp(eps); 85 | SE3 EepsM = Sophus::SE3::exp(-eps); 86 | SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT() * EepsP) * get_worldToCam_evalPT().inverse(); 87 | SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT() * EepsM) * get_worldToCam_evalPT().inverse(); 88 | nullspaces_pose.col(i) = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); 89 | } 90 | //nullspaces_pose.topRows<3>() *= SCALE_XI_TRANS_INVERSE; 91 | //nullspaces_pose.bottomRows<3>() *= SCALE_XI_ROT_INVERSE; 92 | 93 | // scale change 94 | SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT()); 95 | w2c_leftEps_P_x0.translation() *= 1.00001; 96 | w2c_leftEps_P_x0 = w2c_leftEps_P_x0 * get_worldToCam_evalPT().inverse(); 97 | SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT()); 98 | w2c_leftEps_M_x0.translation() /= 1.00001; 99 | w2c_leftEps_M_x0 = w2c_leftEps_M_x0 * get_worldToCam_evalPT().inverse(); 100 | nullspaces_scale = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); 101 | 102 | 103 | nullspaces_affine.setZero(); 104 | nullspaces_affine.topLeftCorner<2,1>() = Vec2(1,0); 105 | assert(ab_exposure > 0); 106 | nullspaces_affine.topRightCorner<2,1>() = Vec2(0, expf(aff_g2l_0().a)*ab_exposure); 107 | }; 108 | 109 | 110 | 111 | void FrameHessian::release() 112 | { 113 | // DELETE POINT 114 | // DELETE RESIDUAL 115 | for(unsigned int i=0;i0) 152 | { 153 | int lvlm1 = lvl-1; 154 | int wlm1 = wG[lvlm1]; 155 | Eigen::Vector3f* dI_lm = dIp[lvlm1]; 156 | 157 | 158 | 159 | for(int y=0;ygetBGradOnly((float)(dI_l[idx][0])); 187 | dabs_l[idx] *= gw*gw; // convert to gradient of original color space (before removing response). 188 | } 189 | } 190 | } 191 | } 192 | 193 | void FrameFramePrecalc::set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib ) 194 | { 195 | this->host = host; 196 | this->target = target; 197 | 198 | SE3 leftToLeft_0 = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse(); 199 | PRE_RTll_0 = (leftToLeft_0.rotationMatrix()).cast(); 200 | PRE_tTll_0 = (leftToLeft_0.translation()).cast(); 201 | 202 | 203 | 204 | SE3 leftToLeft = target->PRE_worldToCam * host->PRE_camToWorld; 205 | PRE_RTll = (leftToLeft.rotationMatrix()).cast(); 206 | PRE_tTll = (leftToLeft.translation()).cast(); 207 | distanceLL = leftToLeft.translation().norm(); 208 | 209 | 210 | Mat33f K = Mat33f::Zero(); 211 | K(0,0) = HCalib->fxl(); 212 | K(1,1) = HCalib->fyl(); 213 | K(0,2) = HCalib->cxl(); 214 | K(1,2) = HCalib->cyl(); 215 | K(2,2) = 1; 216 | PRE_KRKiTll = K * PRE_RTll * K.inverse(); 217 | PRE_RKiTll = PRE_RTll * K.inverse(); 218 | PRE_KtTll = K * PRE_tTll; 219 | 220 | 221 | PRE_aff_mode = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l(), target->aff_g2l()).cast(); 222 | PRE_b0_mode = host->aff_g2l_0().b; 223 | } 224 | 225 | } -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/ImmaturePoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | 30 | #include "FullSystem/HessianBlocks.h" 31 | namespace dso 32 | { 33 | 34 | 35 | struct ImmaturePointTemporaryResidual 36 | { 37 | public: 38 | ResState state_state; 39 | double state_energy; 40 | ResState state_NewState; 41 | double state_NewEnergy; 42 | FrameHessian* target; 43 | }; 44 | 45 | 46 | enum ImmaturePointStatus { 47 | IPS_GOOD=0, // traced well and good 48 | IPS_OOB, // OOB: end tracking & marginalize! 49 | IPS_OUTLIER, // energy too high: if happens again: outlier! 50 | IPS_SKIPPED, // traced well and good (but not actually traced). 51 | IPS_BADCONDITION, // not traced because of bad condition. 52 | IPS_UNINITIALIZED}; // not even traced once. 53 | 54 | 55 | class ImmaturePoint 56 | { 57 | public: 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 59 | // static values 60 | float color[MAX_RES_PER_POINT]; 61 | float weights[MAX_RES_PER_POINT]; 62 | 63 | 64 | 65 | 66 | 67 | Mat22f gradH; 68 | Vec2f gradH_ev; 69 | Mat22f gradH_eig; 70 | float energyTH; 71 | float u,v; 72 | FrameHessian* host; 73 | int idxInImmaturePoints; 74 | 75 | float quality; 76 | 77 | float my_type; 78 | 79 | float idepth_min; 80 | float idepth_max; 81 | ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib); 82 | ~ImmaturePoint(); 83 | 84 | ImmaturePointStatus traceOn(FrameHessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false); 85 | 86 | ImmaturePointStatus lastTraceStatus; 87 | Vec2f lastTraceUV; 88 | float lastTracePixelInterval; 89 | 90 | float idepth_GT; 91 | 92 | double linearizeResidual( 93 | CalibHessian * HCalib, const float outlierTHSlack, 94 | ImmaturePointTemporaryResidual* tmpRes, 95 | float &Hdd, float &bd, 96 | float idepth); 97 | float getdPixdd( 98 | CalibHessian * HCalib, 99 | ImmaturePointTemporaryResidual* tmpRes, 100 | float idepth); 101 | 102 | float calcResidual( 103 | CalibHessian * HCalib, const float outlierTHSlack, 104 | ImmaturePointTemporaryResidual* tmpRes, 105 | float idepth); 106 | 107 | private: 108 | }; 109 | 110 | } 111 | 112 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/PixelSelector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | 30 | 31 | 32 | 33 | namespace dso 34 | { 35 | 36 | 37 | const float minUseGrad_pixsel = 10; 38 | 39 | 40 | template 41 | inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, float THFac) 42 | { 43 | 44 | memset(map_out, 0, sizeof(bool)*w*h); 45 | 46 | int numGood = 0; 47 | for(int y=1;y().squaredNorm(); 65 | float TH = THFac*minUseGrad_pixsel * (0.75f); 66 | 67 | if(sqgd > TH*TH) 68 | { 69 | float agx = fabs((float)g[1]); 70 | if(agx > bestXX) {bestXX=agx; bestXXID=idx;} 71 | 72 | float agy = fabs((float)g[2]); 73 | if(agy > bestYY) {bestYY=agy; bestYYID=idx;} 74 | 75 | float gxpy = fabs((float)(g[1]-g[2])); 76 | if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;} 77 | 78 | float gxmy = fabs((float)(g[1]+g[2])); 79 | if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;} 80 | } 81 | } 82 | 83 | bool* map0 = map_out+x+y*w; 84 | 85 | if(bestXXID>=0) 86 | { 87 | if(!map0[bestXXID]) 88 | numGood++; 89 | map0[bestXXID] = true; 90 | 91 | } 92 | if(bestYYID>=0) 93 | { 94 | if(!map0[bestYYID]) 95 | numGood++; 96 | map0[bestYYID] = true; 97 | 98 | } 99 | if(bestXYID>=0) 100 | { 101 | if(!map0[bestXYID]) 102 | numGood++; 103 | map0[bestXYID] = true; 104 | 105 | } 106 | if(bestYXID>=0) 107 | { 108 | if(!map0[bestYXID]) 109 | numGood++; 110 | map0[bestYXID] = true; 111 | 112 | } 113 | } 114 | } 115 | 116 | return numGood; 117 | } 118 | 119 | 120 | inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, int pot, float THFac) 121 | { 122 | 123 | memset(map_out, 0, sizeof(bool)*w*h); 124 | 125 | int numGood = 0; 126 | for(int y=1;y().squaredNorm(); 144 | float TH = THFac*minUseGrad_pixsel * (0.75f); 145 | 146 | if(sqgd > TH*TH) 147 | { 148 | float agx = fabs((float)g[1]); 149 | if(agx > bestXX) {bestXX=agx; bestXXID=idx;} 150 | 151 | float agy = fabs((float)g[2]); 152 | if(agy > bestYY) {bestYY=agy; bestYYID=idx;} 153 | 154 | float gxpy = fabs((float)(g[1]-g[2])); 155 | if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;} 156 | 157 | float gxmy = fabs((float)(g[1]+g[2])); 158 | if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;} 159 | } 160 | } 161 | 162 | bool* map0 = map_out+x+y*w; 163 | 164 | if(bestXXID>=0) 165 | { 166 | if(!map0[bestXXID]) 167 | numGood++; 168 | map0[bestXXID] = true; 169 | 170 | } 171 | if(bestYYID>=0) 172 | { 173 | if(!map0[bestYYID]) 174 | numGood++; 175 | map0[bestYYID] = true; 176 | 177 | } 178 | if(bestXYID>=0) 179 | { 180 | if(!map0[bestXYID]) 181 | numGood++; 182 | map0[bestXYID] = true; 183 | 184 | } 185 | if(bestYXID>=0) 186 | { 187 | if(!map0[bestYXID]) 188 | numGood++; 189 | map0[bestYXID] = true; 190 | 191 | } 192 | } 193 | } 194 | 195 | return numGood; 196 | } 197 | 198 | 199 | inline int makePixelStatus(Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1) 200 | { 201 | if(sparsityFactor < 1) sparsityFactor = 1; 202 | 203 | int numGoodPoints; 204 | 205 | 206 | if(sparsityFactor==1) numGoodPoints = gridMaxSelection<1>(grads, map, w, h, THFac); 207 | else if(sparsityFactor==2) numGoodPoints = gridMaxSelection<2>(grads, map, w, h, THFac); 208 | else if(sparsityFactor==3) numGoodPoints = gridMaxSelection<3>(grads, map, w, h, THFac); 209 | else if(sparsityFactor==4) numGoodPoints = gridMaxSelection<4>(grads, map, w, h, THFac); 210 | else if(sparsityFactor==5) numGoodPoints = gridMaxSelection<5>(grads, map, w, h, THFac); 211 | else if(sparsityFactor==6) numGoodPoints = gridMaxSelection<6>(grads, map, w, h, THFac); 212 | else if(sparsityFactor==7) numGoodPoints = gridMaxSelection<7>(grads, map, w, h, THFac); 213 | else if(sparsityFactor==8) numGoodPoints = gridMaxSelection<8>(grads, map, w, h, THFac); 214 | else if(sparsityFactor==9) numGoodPoints = gridMaxSelection<9>(grads, map, w, h, THFac); 215 | else if(sparsityFactor==10) numGoodPoints = gridMaxSelection<10>(grads, map, w, h, THFac); 216 | else if(sparsityFactor==11) numGoodPoints = gridMaxSelection<11>(grads, map, w, h, THFac); 217 | else numGoodPoints = gridMaxSelection(grads, map, w, h, sparsityFactor, THFac); 218 | 219 | 220 | /* 221 | * #points is approximately proportional to sparsityFactor^2. 222 | */ 223 | 224 | float quotia = numGoodPoints / (float)(desiredDensity); 225 | 226 | int newSparsity = (sparsityFactor * sqrtf(quotia))+0.7f; 227 | 228 | 229 | if(newSparsity < 1) newSparsity=1; 230 | 231 | 232 | float oldTHFac = THFac; 233 | if(newSparsity==1 && sparsityFactor==1) THFac = 0.5; 234 | 235 | 236 | if((abs(newSparsity-sparsityFactor) < 1 && THFac==oldTHFac) || 237 | ( quotia > 0.8 && 1.0f / quotia > 0.8) || 238 | recsLeft == 0) 239 | { 240 | 241 | // printf(" \n"); 242 | //all good 243 | sparsityFactor = newSparsity; 244 | return numGoodPoints; 245 | } 246 | else 247 | { 248 | // printf(" -> re-evaluate! \n"); 249 | // re-evaluate. 250 | sparsityFactor = newSparsity; 251 | return makePixelStatus(grads, map, w,h, desiredDensity, recsLeft-1, THFac); 252 | } 253 | } 254 | 255 | } 256 | 257 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/PixelSelector2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | 29 | namespace dso 30 | { 31 | 32 | enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3}; 33 | 34 | 35 | class FrameHessian; 36 | 37 | class PixelSelector 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 41 | int makeMaps( 42 | const FrameHessian* const fh, 43 | float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1); 44 | 45 | PixelSelector(int w, int h); 46 | ~PixelSelector(); 47 | int currentPotential; 48 | 49 | 50 | bool allowFast; 51 | void makeHists(const FrameHessian* const fh); 52 | private: 53 | 54 | Eigen::Vector3i select(const FrameHessian* const fh, 55 | float* map_out, int pot, float thFactor=1); 56 | 57 | 58 | unsigned char* randomPattern; 59 | 60 | 61 | int* gradHist; 62 | float* ths; 63 | float* thsSmoothed; 64 | int thsStep; 65 | const FrameHessian* gradHistFrame; 66 | }; 67 | 68 | 69 | 70 | 71 | } 72 | 73 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/ResidualProjections.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | #include "FullSystem/FullSystem.h" 29 | #include "FullSystem/HessianBlocks.h" 30 | #include "util/settings.h" 31 | 32 | namespace dso 33 | { 34 | 35 | 36 | EIGEN_STRONG_INLINE float derive_idepth( 37 | const Vec3f &t, const float &u, const float &v, 38 | const int &dx, const int &dy, const float &dxInterp, 39 | const float &dyInterp, const float &drescale) 40 | { 41 | return (dxInterp*drescale * (t[0]-t[2]*u) 42 | + dyInterp*drescale * (t[1]-t[2]*v))*SCALE_IDEPTH; 43 | } 44 | 45 | 46 | 47 | EIGEN_STRONG_INLINE bool projectPoint( 48 | const float &u_pt,const float &v_pt, 49 | const float &idepth, 50 | const Mat33f &KRKi, const Vec3f &Kt, 51 | float &Ku, float &Kv) 52 | { 53 | Vec3f ptp = KRKi * Vec3f(u_pt,v_pt, 1) + Kt*idepth; 54 | Ku = ptp[0] / ptp[2]; 55 | Kv = ptp[1] / ptp[2]; 56 | return Ku>1.1f && Kv>1.1f && Kucxl())*HCalib->fxli(), 72 | (v_pt+dy-HCalib->cyl())*HCalib->fyli(), 73 | 1); 74 | 75 | Vec3f ptp = R * KliP + t*idepth; 76 | drescale = 1.0f/ptp[2]; 77 | new_idepth = idepth*drescale; 78 | 79 | if(!(drescale>0)) return false; 80 | 81 | u = ptp[0] * drescale; 82 | v = ptp[1] * drescale; 83 | Ku = u*HCalib->fxl() + HCalib->cxl(); 84 | Kv = v*HCalib->fyl() + HCalib->cyl(); 85 | 86 | return Ku>1.1f && Kv>1.1f && Ku, 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | /* 26 | * KFBuffer.cpp 27 | * 28 | * Created on: Jan 7, 2014 29 | * Author: engelj 30 | */ 31 | 32 | #include "FullSystem/FullSystem.h" 33 | 34 | #include "stdio.h" 35 | #include "util/globalFuncs.h" 36 | #include 37 | #include 38 | #include "IOWrapper/ImageDisplay.h" 39 | #include "util/globalCalib.h" 40 | #include 41 | #include 42 | 43 | #include "FullSystem/ResidualProjections.h" 44 | #include "OptimizationBackend/EnergyFunctional.h" 45 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 46 | 47 | #include "FullSystem/HessianBlocks.h" 48 | 49 | namespace dso 50 | { 51 | int PointFrameResidual::instanceCounter = 0; 52 | 53 | 54 | long runningResID=0; 55 | 56 | 57 | PointFrameResidual::PointFrameResidual(){assert(false); instanceCounter++;} 58 | 59 | PointFrameResidual::~PointFrameResidual(){assert(efResidual==0); instanceCounter--; delete J;} 60 | 61 | PointFrameResidual::PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_) : 62 | point(point_), 63 | host(host_), 64 | target(target_) 65 | { 66 | efResidual=0; 67 | instanceCounter++; 68 | resetOOB(); 69 | J = new RawResidualJacobian(); 70 | assert(((long)J)%16==0); 71 | 72 | isNew=true; 73 | } 74 | 75 | 76 | 77 | 78 | double PointFrameResidual::linearize(CalibHessian* HCalib) 79 | { 80 | state_NewEnergyWithOutlier=-1; 81 | 82 | if(state_state == ResState::OOB) 83 | { state_NewState = ResState::OOB; return state_energy; } 84 | 85 | FrameFramePrecalc* precalc = &(host->targetPrecalc[target->idx]); 86 | float energyLeft=0; 87 | const Eigen::Vector3f* dIl = target->dI; 88 | //const float* const Il = target->I; 89 | const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll; 90 | const Vec3f &PRE_KtTll = precalc->PRE_KtTll; 91 | const Mat33f &PRE_RTll_0 = precalc->PRE_RTll_0; 92 | const Vec3f &PRE_tTll_0 = precalc->PRE_tTll_0; 93 | const float * const color = point->color; 94 | const float * const weights = point->weights; 95 | 96 | Vec2f affLL = precalc->PRE_aff_mode; 97 | float b0 = precalc->PRE_b0_mode; 98 | 99 | 100 | Vec6f d_xi_x, d_xi_y; 101 | Vec4f d_C_x, d_C_y; 102 | float d_d_x, d_d_y; 103 | { 104 | float drescale, u, v, new_idepth; 105 | float Ku, Kv; 106 | Vec3f KliP; 107 | 108 | if(!projectPoint(point->u, point->v, point->idepth_zero_scaled, 0, 0,HCalib, 109 | PRE_RTll_0,PRE_tTll_0, drescale, u, v, Ku, Kv, KliP, new_idepth)) 110 | { state_NewState = ResState::OOB; return state_energy; } 111 | 112 | centerProjectedTo = Vec3f(Ku, Kv, new_idepth); 113 | 114 | 115 | // diff d_idepth 116 | d_d_x = drescale * (PRE_tTll_0[0]-PRE_tTll_0[2]*u)*SCALE_IDEPTH*HCalib->fxl(); 117 | d_d_y = drescale * (PRE_tTll_0[1]-PRE_tTll_0[2]*v)*SCALE_IDEPTH*HCalib->fyl(); 118 | 119 | 120 | 121 | 122 | // diff calib 123 | d_C_x[2] = drescale*(PRE_RTll_0(2,0)*u-PRE_RTll_0(0,0)); 124 | d_C_x[3] = HCalib->fxl() * drescale*(PRE_RTll_0(2,1)*u-PRE_RTll_0(0,1)) * HCalib->fyli(); 125 | d_C_x[0] = KliP[0]*d_C_x[2]; 126 | d_C_x[1] = KliP[1]*d_C_x[3]; 127 | 128 | d_C_y[2] = HCalib->fyl() * drescale*(PRE_RTll_0(2,0)*v-PRE_RTll_0(1,0)) * HCalib->fxli(); 129 | d_C_y[3] = drescale*(PRE_RTll_0(2,1)*v-PRE_RTll_0(1,1)); 130 | d_C_y[0] = KliP[0]*d_C_y[2]; 131 | d_C_y[1] = KliP[1]*d_C_y[3]; 132 | 133 | d_C_x[0] = (d_C_x[0]+u)*SCALE_F; 134 | d_C_x[1] *= SCALE_F; 135 | d_C_x[2] = (d_C_x[2]+1)*SCALE_C; 136 | d_C_x[3] *= SCALE_C; 137 | 138 | d_C_y[0] *= SCALE_F; 139 | d_C_y[1] = (d_C_y[1]+v)*SCALE_F; 140 | d_C_y[2] *= SCALE_C; 141 | d_C_y[3] = (d_C_y[3]+1)*SCALE_C; 142 | 143 | 144 | d_xi_x[0] = new_idepth*HCalib->fxl(); 145 | d_xi_x[1] = 0; 146 | d_xi_x[2] = -new_idepth*u*HCalib->fxl(); 147 | d_xi_x[3] = -u*v*HCalib->fxl(); 148 | d_xi_x[4] = (1+u*u)*HCalib->fxl(); 149 | d_xi_x[5] = -v*HCalib->fxl(); 150 | 151 | d_xi_y[0] = 0; 152 | d_xi_y[1] = new_idepth*HCalib->fyl(); 153 | d_xi_y[2] = -new_idepth*v*HCalib->fyl(); 154 | d_xi_y[3] = -(1+v*v)*HCalib->fyl(); 155 | d_xi_y[4] = u*v*HCalib->fyl(); 156 | d_xi_y[5] = u*HCalib->fyl(); 157 | } 158 | 159 | 160 | { 161 | J->Jpdxi[0] = d_xi_x; 162 | J->Jpdxi[1] = d_xi_y; 163 | 164 | J->Jpdc[0] = d_C_x; 165 | J->Jpdc[1] = d_C_y; 166 | 167 | J->Jpdd[0] = d_d_x; 168 | J->Jpdd[1] = d_d_y; 169 | 170 | } 171 | 172 | 173 | 174 | 175 | 176 | 177 | float JIdxJIdx_00=0, JIdxJIdx_11=0, JIdxJIdx_10=0; 178 | float JabJIdx_00=0, JabJIdx_01=0, JabJIdx_10=0, JabJIdx_11=0; 179 | float JabJab_00=0, JabJab_01=0, JabJab_11=0; 180 | 181 | float wJI2_sum = 0; 182 | 183 | for(int idx=0;idxu+patternP[idx][0], point->v+patternP[idx][1], point->idepth_scaled, PRE_KRKiTll, PRE_KtTll, Ku, Kv)) 187 | { state_NewState = ResState::OOB; return state_energy; } 188 | 189 | projectedTo[idx][0] = Ku; 190 | projectedTo[idx][1] = Kv; 191 | 192 | 193 | Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0])); 194 | float residual = hitColor[0] - (float)(affLL[0] * color[idx] + affLL[1]); 195 | 196 | 197 | 198 | float drdA = (color[idx]-b0); 199 | if(!std::isfinite((float)hitColor[0])) 200 | { state_NewState = ResState::OOB; return state_energy; } 201 | 202 | 203 | float w = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + hitColor.tail<2>().squaredNorm())); 204 | w = 0.5f*(w + weights[idx]); 205 | 206 | 207 | 208 | float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual); 209 | energyLeft += w*w*hw *residual*residual*(2-hw); 210 | 211 | { 212 | if(hw < 1) hw = sqrtf(hw); 213 | hw = hw*w; 214 | 215 | hitColor[1]*=hw; 216 | hitColor[2]*=hw; 217 | 218 | J->resF[idx] = residual*hw; 219 | 220 | J->JIdx[0][idx] = hitColor[1]; 221 | J->JIdx[1][idx] = hitColor[2]; 222 | J->JabF[0][idx] = drdA*hw; 223 | J->JabF[1][idx] = hw; 224 | 225 | JIdxJIdx_00+=hitColor[1]*hitColor[1]; 226 | JIdxJIdx_11+=hitColor[2]*hitColor[2]; 227 | JIdxJIdx_10+=hitColor[1]*hitColor[2]; 228 | 229 | JabJIdx_00+= drdA*hw * hitColor[1]; 230 | JabJIdx_01+= drdA*hw * hitColor[2]; 231 | JabJIdx_10+= hw * hitColor[1]; 232 | JabJIdx_11+= hw * hitColor[2]; 233 | 234 | JabJab_00+= drdA*drdA*hw*hw; 235 | JabJab_01+= drdA*hw*hw; 236 | JabJab_11+= hw*hw; 237 | 238 | 239 | wJI2_sum += hw*hw*(hitColor[1]*hitColor[1]+hitColor[2]*hitColor[2]); 240 | 241 | if(setting_affineOptModeA < 0) J->JabF[0][idx]=0; 242 | if(setting_affineOptModeB < 0) J->JabF[1][idx]=0; 243 | 244 | } 245 | } 246 | 247 | J->JIdx2(0,0) = JIdxJIdx_00; 248 | J->JIdx2(0,1) = JIdxJIdx_10; 249 | J->JIdx2(1,0) = JIdxJIdx_10; 250 | J->JIdx2(1,1) = JIdxJIdx_11; 251 | J->JabJIdx(0,0) = JabJIdx_00; 252 | J->JabJIdx(0,1) = JabJIdx_01; 253 | J->JabJIdx(1,0) = JabJIdx_10; 254 | J->JabJIdx(1,1) = JabJIdx_11; 255 | J->Jab2(0,0) = JabJab_00; 256 | J->Jab2(0,1) = JabJab_01; 257 | J->Jab2(1,0) = JabJab_01; 258 | J->Jab2(1,1) = JabJab_11; 259 | 260 | state_NewEnergyWithOutlier = energyLeft; 261 | 262 | if(energyLeft > std::max(host->frameEnergyTH, target->frameEnergyTH) || wJI2_sum < 2) 263 | { 264 | energyLeft = std::max(host->frameEnergyTH, target->frameEnergyTH); 265 | state_NewState = ResState::OUTLIER; 266 | } 267 | else 268 | { 269 | state_NewState = ResState::IN; 270 | } 271 | 272 | state_NewEnergy = energyLeft; 273 | return energyLeft; 274 | } 275 | 276 | 277 | 278 | void PointFrameResidual::debugPlot() 279 | { 280 | if(state_state==ResState::OOB) return; 281 | Vec3b cT = Vec3b(0,0,0); 282 | 283 | if(freeDebugParam5==0) 284 | { 285 | float rT = 20*sqrt(state_energy/9); 286 | if(rT<0) rT=0; if(rT>255)rT=255; 287 | cT = Vec3b(0,255-rT,rT); 288 | } 289 | else 290 | { 291 | if(state_state == ResState::IN) cT = Vec3b(255,0,0); 292 | else if(state_state == ResState::OOB) cT = Vec3b(255,255,0); 293 | else if(state_state == ResState::OUTLIER) cT = Vec3b(0,0,255); 294 | else cT = Vec3b(255,255,255); 295 | } 296 | 297 | for(int i=0;i 2 && projectedTo[i][1] > 2 && projectedTo[i][0] < wG[0]-3 && projectedTo[i][1] < hG[0]-3 )) 300 | target->debugImage->setPixel1((float)projectedTo[i][0], (float)projectedTo[i][1],cT); 301 | } 302 | } 303 | 304 | 305 | 306 | void PointFrameResidual::applyRes(bool copyJacobians) 307 | { 308 | if(copyJacobians) 309 | { 310 | if(state_state == ResState::OOB) 311 | { 312 | assert(!efResidual->isActiveAndIsGoodNEW); 313 | return; // can never go back from OOB 314 | } 315 | if(state_NewState == ResState::IN)// && ) 316 | { 317 | efResidual->isActiveAndIsGoodNEW=true; 318 | efResidual->takeDataF(); 319 | } 320 | else 321 | { 322 | efResidual->isActiveAndIsGoodNEW=false; 323 | } 324 | } 325 | 326 | setState(state_NewState); 327 | state_energy = state_NewEnergy; 328 | } 329 | } 330 | -------------------------------------------------------------------------------- /03_Application/dso/src/FullSystem/Residuals.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/globalCalib.h" 29 | #include "vector" 30 | 31 | #include "util/NumType.h" 32 | #include 33 | #include 34 | #include "util/globalFuncs.h" 35 | #include "OptimizationBackend/RawResidualJacobian.h" 36 | 37 | namespace dso 38 | { 39 | class PointHessian; 40 | class FrameHessian; 41 | class CalibHessian; 42 | 43 | class EFResidual; 44 | 45 | 46 | enum ResLocation {ACTIVE=0, LINEARIZED, MARGINALIZED, NONE}; 47 | enum ResState {IN=0, OOB, OUTLIER}; 48 | 49 | struct FullJacRowT 50 | { 51 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 52 | }; 53 | 54 | class PointFrameResidual 55 | { 56 | public: 57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 58 | 59 | EFResidual* efResidual; 60 | 61 | static int instanceCounter; 62 | 63 | 64 | ResState state_state; 65 | double state_energy; 66 | ResState state_NewState; 67 | double state_NewEnergy; 68 | double state_NewEnergyWithOutlier; 69 | 70 | 71 | void setState(ResState s) {state_state = s;} 72 | 73 | 74 | PointHessian* point; 75 | FrameHessian* host; 76 | FrameHessian* target; 77 | RawResidualJacobian* J; 78 | 79 | 80 | bool isNew; 81 | 82 | 83 | Eigen::Vector2f projectedTo[MAX_RES_PER_POINT]; 84 | Vec3f centerProjectedTo; 85 | 86 | ~PointFrameResidual(); 87 | PointFrameResidual(); 88 | PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_); 89 | double linearize(CalibHessian* HCalib); 90 | 91 | 92 | void resetOOB() 93 | { 94 | state_NewEnergy = state_energy = 0; 95 | state_NewState = ResState::OUTLIER; 96 | 97 | setState(ResState::IN); 98 | }; 99 | void applyRes( bool copyJacobians); 100 | 101 | void debugPlot(); 102 | 103 | void printRows(std::vector &v, VecX &r, int nFrames, int nPoints, int M, int res); 104 | }; 105 | } 106 | 107 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/ImageDisplay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #pragma once 27 | #include 28 | #include "util/NumType.h" 29 | #include "util/MinimalImage.h" 30 | 31 | 32 | namespace dso 33 | { 34 | 35 | namespace IOWrap 36 | { 37 | 38 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize = false); 39 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize = false); 40 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize = false); 41 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize = false); 42 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize = false); 43 | 44 | 45 | void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); 46 | void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); 47 | void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); 48 | void displayImageStitch(const char* windowName, const std::vector images, int cc=0, int rc=0); 49 | 50 | int waitKey(int milliseconds); 51 | void closeAllWindows(); 52 | 53 | } 54 | 55 | } 56 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/ImageDisplay_dummy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #include "IOWrapper/ImageDisplay.h" 27 | 28 | namespace dso 29 | { 30 | 31 | 32 | namespace IOWrap 33 | { 34 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize) {}; 35 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize) {}; 36 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize) {}; 37 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize) {}; 38 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize) {}; 39 | 40 | 41 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; 42 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; 43 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; 44 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) {}; 45 | 46 | int waitKey(int milliseconds) {return 0;}; 47 | void closeAllWindows() {}; 48 | } 49 | 50 | } 51 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/ImageRW.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #pragma once 27 | #include "util/NumType.h" 28 | #include "util/MinimalImage.h" 29 | 30 | namespace dso 31 | { 32 | namespace IOWrap 33 | { 34 | 35 | MinimalImageB* readImageBW_8U(std::string filename); 36 | MinimalImageB3* readImageRGB_8U(std::string filename); 37 | MinimalImage* readImageBW_16U(std::string filename); 38 | 39 | 40 | MinimalImageB* readStreamBW_8U(char* data, int numBytes); 41 | 42 | void writeImage(std::string filename, MinimalImageB* img); 43 | void writeImage(std::string filename, MinimalImageB3* img); 44 | void writeImage(std::string filename, MinimalImageF* img); 45 | void writeImage(std::string filename, MinimalImageF3* img); 46 | 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/ImageRW_dummy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #include "IOWrapper/ImageRW.h" 27 | 28 | namespace dso 29 | { 30 | 31 | 32 | namespace IOWrap 33 | { 34 | 35 | MinimalImageB* readImageBW_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; 36 | MinimalImageB3* readImageRGB_8U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; 37 | MinimalImage* readImageBW_16U(std::string filename) {printf("not implemented. bye!\n"); return 0;}; 38 | MinimalImageB* readStreamBW_8U(char* data, int numBytes) {printf("not implemented. bye!\n"); return 0;}; 39 | void writeImage(std::string filename, MinimalImageB* img) {}; 40 | void writeImage(std::string filename, MinimalImageB3* img) {}; 41 | void writeImage(std::string filename, MinimalImageF* img) {}; 42 | void writeImage(std::string filename, MinimalImageF3* img) {}; 43 | 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/OpenCV/ImageDisplay_OpenCV.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #include "IOWrapper/ImageDisplay.h" 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include "util/settings.h" 36 | 37 | namespace dso 38 | { 39 | 40 | 41 | namespace IOWrap 42 | { 43 | 44 | std::unordered_set openWindows; 45 | boost::mutex openCVdisplayMutex; 46 | 47 | 48 | 49 | void displayImage(const char* windowName, const cv::Mat& image, bool autoSize) 50 | { 51 | if(disableAllDisplay) return; 52 | 53 | boost::unique_lock lock(openCVdisplayMutex); 54 | if(!autoSize) 55 | { 56 | if(openWindows.find(windowName) == openWindows.end()) 57 | { 58 | cv::namedWindow(windowName, cv::WINDOW_NORMAL); 59 | cv::resizeWindow(windowName, image.cols, image.rows); 60 | openWindows.insert(windowName); 61 | } 62 | } 63 | cv::imshow(windowName, image); 64 | } 65 | 66 | 67 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 68 | { 69 | if(disableAllDisplay) return; 70 | if(images.size() == 0) return; 71 | 72 | // get dimensions. 73 | int w = images[0]->cols; 74 | int h = images[0]->rows; 75 | 76 | int num = std::max((int)setting_maxFrames, (int)images.size()); 77 | 78 | // get optimal dimensions. 79 | int bestCC = 0; 80 | float bestLoss = 1e10; 81 | for(int cc=1;cc<10;cc++) 82 | { 83 | int ww = w * cc; 84 | int hh = h * ((num+cc-1)/cc); 85 | 86 | 87 | float wLoss = ww/16.0f; 88 | float hLoss = hh/10.0f; 89 | float loss = std::max(wLoss, hLoss); 90 | 91 | if(loss < bestLoss) 92 | { 93 | bestLoss = loss; 94 | bestCC = cc; 95 | } 96 | } 97 | 98 | int bestRC = ((num+bestCC-1)/bestCC); 99 | if(cc != 0) 100 | { 101 | bestCC = cc; 102 | bestRC= rc; 103 | } 104 | cv::Mat stitch = cv::Mat(bestRC*h, bestCC*w, images[0]->type()); 105 | stitch.setTo(0); 106 | for(int i=0;i<(int)images.size() && i < bestCC*bestRC;i++) 107 | { 108 | int c = i%bestCC; 109 | int r = i/bestCC; 110 | 111 | cv::Mat roi = stitch(cv::Rect(c*w, r*h, w,h)); 112 | images[i]->copyTo(roi); 113 | } 114 | displayImage(windowName, stitch, false); 115 | } 116 | 117 | 118 | 119 | void displayImage(const char* windowName, const MinimalImageB* img, bool autoSize) 120 | { 121 | displayImage(windowName, cv::Mat(img->h, img->w, CV_8U, img->data), autoSize); 122 | } 123 | void displayImage(const char* windowName, const MinimalImageB3* img, bool autoSize) 124 | { 125 | displayImage(windowName, cv::Mat(img->h, img->w, CV_8UC3, img->data), autoSize); 126 | } 127 | void displayImage(const char* windowName, const MinimalImageF* img, bool autoSize) 128 | { 129 | displayImage(windowName, cv::Mat(img->h, img->w, CV_32F, img->data)*(1/254.0f), autoSize); 130 | } 131 | void displayImage(const char* windowName, const MinimalImageF3* img, bool autoSize) 132 | { 133 | displayImage(windowName, cv::Mat(img->h, img->w, CV_32FC3, img->data)*(1/254.0f), autoSize); 134 | } 135 | void displayImage(const char* windowName, const MinimalImageB16* img, bool autoSize) 136 | { 137 | displayImage(windowName, cv::Mat(img->h, img->w, CV_16U, img->data), autoSize); 138 | } 139 | 140 | 141 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 142 | { 143 | std::vector imagesCV; 144 | for(size_t i=0; i < images.size();i++) 145 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8U, images[i]->data)); 146 | displayImageStitch(windowName, imagesCV, cc, rc); 147 | for(size_t i=0; i < images.size();i++) 148 | delete imagesCV[i]; 149 | } 150 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 151 | { 152 | std::vector imagesCV; 153 | for(size_t i=0; i < images.size();i++) 154 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_8UC3, images[i]->data)); 155 | displayImageStitch(windowName, imagesCV, cc, rc); 156 | for(size_t i=0; i < images.size();i++) 157 | delete imagesCV[i]; 158 | } 159 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 160 | { 161 | std::vector imagesCV; 162 | for(size_t i=0; i < images.size();i++) 163 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32F, images[i]->data)); 164 | displayImageStitch(windowName, imagesCV, cc, rc); 165 | for(size_t i=0; i < images.size();i++) 166 | delete imagesCV[i]; 167 | } 168 | void displayImageStitch(const char* windowName, const std::vector images, int cc, int rc) 169 | { 170 | std::vector imagesCV; 171 | for(size_t i=0; i < images.size();i++) 172 | imagesCV.push_back(new cv::Mat(images[i]->h, images[i]->w, CV_32FC3, images[i]->data)); 173 | displayImageStitch(windowName, imagesCV, cc, rc); 174 | for(size_t i=0; i < images.size();i++) 175 | delete imagesCV[i]; 176 | } 177 | 178 | 179 | 180 | int waitKey(int milliseconds) 181 | { 182 | if(disableAllDisplay) return 0; 183 | 184 | boost::unique_lock lock(openCVdisplayMutex); 185 | return cv::waitKey(milliseconds); 186 | } 187 | 188 | void closeAllWindows() 189 | { 190 | if(disableAllDisplay) return; 191 | boost::unique_lock lock(openCVdisplayMutex); 192 | cv::destroyAllWindows(); 193 | openWindows.clear(); 194 | } 195 | } 196 | 197 | } 198 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/OpenCV/ImageRW_OpenCV.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #include "IOWrapper/ImageRW.h" 27 | #include 28 | 29 | 30 | namespace dso 31 | { 32 | 33 | namespace IOWrap 34 | { 35 | MinimalImageB* readImageBW_8U(std::string filename) 36 | { 37 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE); 38 | if(m.rows*m.cols==0) 39 | { 40 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 41 | return 0; 42 | } 43 | if(m.type() != CV_8U) 44 | { 45 | printf("cv::imread did something strange! this may segfault. \n"); 46 | return 0; 47 | } 48 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 49 | memcpy(img->data, m.data, m.rows*m.cols); 50 | return img; 51 | } 52 | 53 | MinimalImageB3* readImageRGB_8U(std::string filename) 54 | { 55 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_COLOR); 56 | if(m.rows*m.cols==0) 57 | { 58 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 59 | return 0; 60 | } 61 | if(m.type() != CV_8UC3) 62 | { 63 | printf("cv::imread did something strange! this may segfault. \n"); 64 | return 0; 65 | } 66 | MinimalImageB3* img = new MinimalImageB3(m.cols, m.rows); 67 | memcpy(img->data, m.data, 3*m.rows*m.cols); 68 | return img; 69 | } 70 | 71 | MinimalImage* readImageBW_16U(std::string filename) 72 | { 73 | cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_UNCHANGED); 74 | if(m.rows*m.cols==0) 75 | { 76 | printf("cv::imread could not read image %s! this may segfault. \n", filename.c_str()); 77 | return 0; 78 | } 79 | if(m.type() != CV_16U) 80 | { 81 | printf("readImageBW_16U called on image that is not a 16bit grayscale image. this may segfault. \n"); 82 | return 0; 83 | } 84 | MinimalImage* img = new MinimalImage(m.cols, m.rows); 85 | memcpy(img->data, m.data, 2*m.rows*m.cols); 86 | return img; 87 | } 88 | 89 | MinimalImageB* readStreamBW_8U(char* data, int numBytes) 90 | { 91 | cv::Mat m = cv::imdecode(cv::Mat(numBytes,1,CV_8U, data), CV_LOAD_IMAGE_GRAYSCALE); 92 | if(m.rows*m.cols==0) 93 | { 94 | printf("cv::imdecode could not read stream (%d bytes)! this may segfault. \n", numBytes); 95 | return 0; 96 | } 97 | if(m.type() != CV_8U) 98 | { 99 | printf("cv::imdecode did something strange! this may segfault. \n"); 100 | return 0; 101 | } 102 | MinimalImageB* img = new MinimalImageB(m.cols, m.rows); 103 | memcpy(img->data, m.data, m.rows*m.cols); 104 | return img; 105 | } 106 | 107 | 108 | 109 | void writeImage(std::string filename, MinimalImageB* img) 110 | { 111 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8U, img->data)); 112 | } 113 | void writeImage(std::string filename, MinimalImageB3* img) 114 | { 115 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_8UC3, img->data)); 116 | } 117 | void writeImage(std::string filename, MinimalImageF* img) 118 | { 119 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32F, img->data)); 120 | } 121 | void writeImage(std::string filename, MinimalImageF3* img) 122 | { 123 | cv::imwrite(filename, cv::Mat(img->h, img->w, CV_32FC3, img->data)); 124 | } 125 | 126 | } 127 | 128 | } 129 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/Output3DWrapper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #pragma once 27 | #include 28 | #include 29 | 30 | #include "util/NumType.h" 31 | #include "util/MinimalImage.h" 32 | #include "map" 33 | 34 | namespace cv { 35 | class Mat; 36 | } 37 | 38 | namespace dso 39 | { 40 | 41 | class FrameHessian; 42 | class CalibHessian; 43 | class FrameShell; 44 | 45 | namespace IOWrap 46 | { 47 | 48 | /* ======================= Some typical usecases: =============== 49 | * 50 | * (1) always get the pose of the most recent frame: 51 | * -> Implement [publishCamPose]. 52 | * 53 | * (2) always get the depthmap of the most recent keyframe 54 | * -> Implement [pushDepthImageFloat] (use inverse depth in [image], and pose / frame information from [KF]). 55 | * 56 | * (3) accumulate final model 57 | * -> Implement [publishKeyframes] (skip for final!=false), and accumulate frames. 58 | * 59 | * (4) get evolving model in real-time 60 | * -> Implement [publishKeyframes] (update all frames for final==false). 61 | * 62 | * 63 | * 64 | * 65 | * ==================== How to use the structs: =================== 66 | * [FrameShell]: minimal struct kept for each frame ever tracked. 67 | * ->camToWorld = camera to world transformation 68 | * ->poseValid = false if [camToWorld] is invalid (only happens for frames during initialization). 69 | * ->trackingRef = Shell of the frame this frame was tracked on. 70 | * ->id = ID of that frame, starting with 0 for the very first frame. 71 | * 72 | * ->incoming_id = ID passed into [addActiveFrame( ImageAndExposure* image, int id )]. 73 | * ->timestamp = timestamp passed into [addActiveFrame( ImageAndExposure* image, int id )] as image.timestamp. 74 | * 75 | * [FrameHessian] 76 | * ->immaturePoints: contains points that have not been "activated" (they do however have a depth initialization). 77 | * ->pointHessians: contains active points. 78 | * ->pointHessiansMarginalized: contains marginalized points. 79 | * ->pointHessiansOut: contains outlier points. 80 | * 81 | * ->frameID: incremental ID for keyframes only. 82 | * ->shell: corresponding [FrameShell] struct. 83 | * 84 | * 85 | * [CalibHessian] 86 | * ->fxl(), fyl(), cxl(), cyl(): get optimized, most recent (pinhole) camera intrinsics. 87 | * 88 | * 89 | * [PointHessian] 90 | * ->u,v: pixel-coordinates of point. 91 | * ->idepth_scaled: inverse depth of point. 92 | * DO NOT USE [idepth], since it may be scaled with [SCALE_IDEPTH] ... however that is currently set to 1 so never mind. 93 | * ->host: pointer to host-frame of point. 94 | * ->status: current status of point (ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED) 95 | * ->numGoodResiduals: number of non-outlier residuals supporting this point (approximate). 96 | * ->maxRelBaseline: value roughly proportional to the relative baseline this point was observed with (0 = no baseline). 97 | * points for which this value is low are badly contrained. 98 | * ->idepth_hessian: hessian value (inverse variance) of inverse depth. 99 | * 100 | * [ImmaturePoint] 101 | * ->u,v: pixel-coordinates of point. 102 | * ->idepth_min, idepth_max: the initialization sais that the inverse depth of this point is very likely 103 | * between these two thresholds (their mean being the best guess) 104 | * ->host: pointer to host-frame of point. 105 | */ 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | class Output3DWrapper 114 | { 115 | public: 116 | Output3DWrapper() {} 117 | virtual ~Output3DWrapper() {} 118 | 119 | 120 | /* Usage: 121 | * Called once after each new Keyframe is inserted & optimized. 122 | * [connectivity] contains for each frame-frame pair the number of [0] active residuals in between them, 123 | * and [1] the number of marginalized reisduals between them. 124 | * frame-frame pairs are encoded as HASH_IDX = [(int)hostFrameKFID << 32 + (int)targetFrameKFID]. 125 | * the [***frameKFID] used for hashing correspond to the [FrameHessian]->frameID. 126 | * 127 | * Calling: 128 | * Always called, no overhead if not used. 129 | */ 130 | virtual void publishGraph(const std::map, Eigen::aligned_allocator > > &connectivity) {} 131 | 132 | 133 | 134 | 135 | 136 | /* Usage: 137 | * Called after each new Keyframe is inserted & optimized, with all keyframes that were part of the active window during 138 | * that optimization in [frames] (with final=false). Use to access the new frame pose and points. 139 | * Also called just before a frame is marginalized (with final=true) with only that frame in [frames]; at that point, 140 | * no further updates will ever occur to it's optimized values (pose & idepth values of it's points). 141 | * 142 | * If you want always all most recent values for all frames, just use the [final=false] calls. 143 | * If you only want to get the final model, but don't care about it being delay-free, only use the 144 | * [final=true] calls, to save compute. 145 | * 146 | * Calling: 147 | * Always called, negligible overhead if not used. 148 | */ 149 | virtual void publishKeyframes(std::vector &frames, bool final, CalibHessian* HCalib) {} 150 | 151 | 152 | 153 | 154 | 155 | /* Usage: 156 | * Called once for each tracked frame, with the real-time, low-delay frame pose. 157 | * 158 | * Calling: 159 | * Always called, no overhead if not used. 160 | */ 161 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) {} 162 | 163 | 164 | 165 | 166 | 167 | /* Usage: 168 | * Called once for each new frame, before it is tracked (i.e., it doesn't have a pose yet). 169 | * 170 | * Calling: 171 | * Always called, no overhead if not used. 172 | */ 173 | virtual void pushLiveFrame(FrameHessian* image) {} 174 | 175 | 176 | 177 | 178 | /* called once after a new keyframe is created, with the color-coded, forward-warped inverse depthmap for that keyframe, 179 | * which is used for initial alignment of future frames. Meant for visualization. 180 | * 181 | * Calling: 182 | * Needs to prepare the depth image, so it is only called if [needPushDepthImage()] returned true. 183 | */ 184 | virtual void pushDepthImage(MinimalImageB3* image) {} 185 | virtual bool needPushDepthImage() {return false;} 186 | 187 | 188 | 189 | /* Usage: 190 | * called once after a new keyframe is created, with the forward-warped inverse depthmap for that keyframe. 191 | * (<= 0 for pixels without inv. depth value, >0 for pixels with inv. depth value) 192 | * 193 | * Calling: 194 | * Always called, almost no overhead if not used. 195 | */ 196 | virtual void pushDepthImageFloat(MinimalImageF* image, FrameHessian* KF ) {} 197 | 198 | 199 | 200 | /* call on finish */ 201 | virtual void join() {} 202 | 203 | /* call on reset */ 204 | virtual void reset() {} 205 | 206 | }; 207 | 208 | } 209 | } 210 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/Pangolin/KeyFrameDisplay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #undef Success 28 | #include 29 | #include "util/NumType.h" 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace dso 36 | { 37 | class CalibHessian; 38 | class FrameHessian; 39 | class FrameShell; 40 | 41 | namespace IOWrap 42 | { 43 | 44 | template 45 | struct InputPointSparse 46 | { 47 | float u; 48 | float v; 49 | float idpeth; 50 | float idepth_hessian; 51 | float relObsBaseline; 52 | int numGoodRes; 53 | unsigned char color[ppp]; 54 | unsigned char status; 55 | }; 56 | 57 | struct MyVertex 58 | { 59 | float point[3]; 60 | unsigned char color[4]; 61 | }; 62 | 63 | // stores a pointcloud associated to a Keyframe. 64 | class KeyFrameDisplay 65 | { 66 | 67 | public: 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 69 | KeyFrameDisplay(); 70 | ~KeyFrameDisplay(); 71 | 72 | // copies points from KF over to internal buffer, 73 | // keeping some additional information so we can render it differently. 74 | void setFromKF(FrameHessian* fh, CalibHessian* HCalib); 75 | 76 | // copies points from KF over to internal buffer, 77 | // keeping some additional information so we can render it differently. 78 | void setFromF(FrameShell* fs, CalibHessian* HCalib); 79 | 80 | // copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing. 81 | bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity); 82 | 83 | // renders cam & pointcloud. 84 | void drawCam(float lineWidth = 1, float* color = 0, float sizeFactor=1, bool drawAxes=false); 85 | void drawPC(float pointSize); 86 | 87 | int id; 88 | bool active; 89 | SE3 camToWorld; 90 | SE3 camToWorld_predicted; // ADAM: Added to display kalman Filter 91 | bool hasPrediction; // ADAM: Added to display kalman Filter 92 | SE3 camToWorld_measured; // ADAM: Added to display GPS 93 | bool hasMeasurement; // ADAM: Added to display GPS 94 | 95 | 96 | 97 | 98 | inline bool operator < (const KeyFrameDisplay& other) const 99 | { 100 | return (id < other.id); 101 | } 102 | 103 | 104 | private: 105 | float fx,fy,cx,cy; 106 | float fxi,fyi,cxi,cyi; 107 | int width, height; 108 | 109 | float my_scaledTH, my_absTH, my_scale; 110 | int my_sparsifyFactor; 111 | int my_displayMode; 112 | float my_minRelBS; 113 | bool needRefresh; 114 | 115 | 116 | int numSparsePoints; 117 | int numSparseBufferSize; 118 | InputPointSparse* originalInputSparse; 119 | 120 | 121 | bool bufferValid; 122 | int numGLBufferPoints; 123 | int numGLBufferGoodPoints; 124 | pangolin::GlBuffer vertexBuffer; 125 | pangolin::GlBuffer colorBuffer; 126 | }; 127 | 128 | } 129 | } 130 | 131 | -------------------------------------------------------------------------------- /03_Application/dso/src/IOWrapper/Pangolin/PangolinDSOViewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | #include 27 | #include "boost/thread.hpp" 28 | #include "util/MinimalImage.h" 29 | #include "IOWrapper/Output3DWrapper.h" 30 | #include 31 | #include 32 | 33 | 34 | namespace dso 35 | { 36 | 37 | class FrameHessian; 38 | class CalibHessian; 39 | class FrameShell; 40 | 41 | 42 | namespace IOWrap 43 | { 44 | 45 | class KeyFrameDisplay; 46 | 47 | struct GraphConnection 48 | { 49 | KeyFrameDisplay* from; 50 | KeyFrameDisplay* to; 51 | int fwdMarg, bwdMarg, fwdAct, bwdAct; 52 | }; 53 | 54 | 55 | class PangolinDSOViewer : public Output3DWrapper 56 | { 57 | public: 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 59 | PangolinDSOViewer(int w, int h, bool startRunThread=true); 60 | virtual ~PangolinDSOViewer(); 61 | 62 | void run(); 63 | void close(); 64 | 65 | void addImageToDisplay(std::string name, MinimalImageB3* image); 66 | void clearAllImagesToDisplay(); 67 | 68 | 69 | // ==================== Output3DWrapper Functionality ====================== 70 | virtual void publishGraph(const std::map &connectivity); 71 | virtual void publishKeyframes( std::vector &frames, bool final, CalibHessian* HCalib); 72 | virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib); 73 | 74 | 75 | virtual void pushLiveFrame(FrameHessian* image); 76 | virtual void pushDepthImage(MinimalImageB3* image); 77 | virtual bool needPushDepthImage(); 78 | 79 | virtual void join(); 80 | 81 | virtual void reset(); 82 | private: 83 | 84 | bool needReset; 85 | void reset_internal(); 86 | void drawConstraints(); 87 | 88 | boost::thread runThread; 89 | bool running; 90 | int w,h; 91 | 92 | 93 | 94 | // images rendering 95 | boost::mutex openImagesMutex; 96 | MinimalImageB3* internalVideoImg; 97 | MinimalImageB3* internalKFImg; 98 | MinimalImageB3* internalResImg; 99 | bool videoImgChanged, kfImgChanged, resImgChanged; 100 | 101 | 102 | 103 | // 3D model rendering 104 | boost::mutex model3DMutex; 105 | KeyFrameDisplay* currentCam; 106 | std::vector keyframes; 107 | std::vector> allFramePoses; 108 | std::map keyframesByKFID; 109 | std::vector> connections; 110 | 111 | 112 | 113 | // render settings 114 | bool settings_showKFCameras; 115 | bool settings_showCurrentCamera; 116 | bool settings_showTrajectory; 117 | bool settings_showFullTrajectory; 118 | bool settings_showActiveConstraints; 119 | bool settings_showAllConstraints; 120 | /* ADAM: UI controls to show worldGrid and Coordinate Systems */ 121 | bool settings_showWorldGrid; 122 | bool settings_showAxes; 123 | 124 | float settings_scaledVarTH; 125 | float settings_absVarTH; 126 | int settings_pointCloudMode; 127 | float settings_minRelBS; 128 | int settings_sparsity; 129 | 130 | 131 | // timings 132 | struct timeval last_track; 133 | struct timeval last_map; 134 | 135 | 136 | std::deque lastNTrackingMs; 137 | std::deque lastNMappingMs; 138 | }; 139 | 140 | 141 | 142 | } 143 | 144 | 145 | 146 | } 147 | -------------------------------------------------------------------------------- /03_Application/dso/src/OptimizationBackend/AccumulatedSCHessian.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #include "OptimizationBackend/AccumulatedSCHessian.h" 26 | #include "OptimizationBackend/EnergyFunctional.h" 27 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 28 | 29 | #include "FullSystem/HessianBlocks.h" 30 | 31 | namespace dso 32 | { 33 | 34 | void AccumulatedSCHessianSSE::addPoint(EFPoint* p, bool shiftPriorToZero, int tid) 35 | { 36 | int ngoodres = 0; 37 | for(EFResidual* r : p->residualsAll) if(r->isActive()) ngoodres++; 38 | if(ngoodres==0) 39 | { 40 | p->HdiF=0; 41 | p->bdSumF=0; 42 | p->data->idepth_hessian=0; 43 | p->data->maxRelBaseline=0; 44 | return; 45 | } 46 | 47 | float H = p->Hdd_accAF+p->Hdd_accLF+p->priorF; 48 | if(H < 1e-10) H = 1e-10; 49 | 50 | p->data->idepth_hessian=H; 51 | 52 | p->HdiF = 1.0 / H; 53 | p->bdSumF = p->bd_accAF + p->bd_accLF; 54 | if(shiftPriorToZero) p->bdSumF += p->priorF*p->deltaF; 55 | VecCf Hcd = p->Hcd_accAF + p->Hcd_accLF; 56 | accHcc[tid].update(Hcd,Hcd,p->HdiF); 57 | accbc[tid].update(Hcd, p->bdSumF * p->HdiF); 58 | 59 | assert(std::isfinite((float)(p->HdiF))); 60 | 61 | int nFrames2 = nframes[tid]*nframes[tid]; 62 | for(EFResidual* r1 : p->residualsAll) 63 | { 64 | if(!r1->isActive()) continue; 65 | int r1ht = r1->hostIDX + r1->targetIDX*nframes[tid]; 66 | 67 | for(EFResidual* r2 : p->residualsAll) 68 | { 69 | if(!r2->isActive()) continue; 70 | 71 | accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF); 72 | } 73 | 74 | accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF); 75 | accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF); 76 | } 77 | } 78 | void AccumulatedSCHessianSSE::stitchDoubleInternal( 79 | MatXX* H, VecX* b, EnergyFunctional const * const EF, 80 | int min, int max, Vec10* stats, int tid) 81 | { 82 | int toAggregate = NUM_THREADS; 83 | if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate. 84 | if(min==max) return; 85 | 86 | 87 | int nf = nframes[0]; 88 | int nframes2 = nf*nf; 89 | 90 | for(int k=min;k(); 107 | bp += accEB[tid2][ijIdx].A1m.cast(); 108 | } 109 | 110 | H[tid].block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * Hpc; 111 | H[tid].block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * Hpc; 112 | b[tid].segment<8>(iIdx) += EF->adHost[ijIdx] * bp; 113 | b[tid].segment<8>(jIdx) += EF->adTarget[ijIdx] * bp; 114 | 115 | 116 | 117 | for(int k=0;k(); 130 | } 131 | 132 | H[tid].block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 133 | H[tid].block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 134 | H[tid].block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 135 | H[tid].block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 136 | } 137 | } 138 | 139 | if(min==0) 140 | { 141 | for(int tid2=0;tid2 < toAggregate;tid2++) 142 | { 143 | accHcc[tid2].finish(); 144 | accbc[tid2].finish(); 145 | H[tid].topLeftCorner() += accHcc[tid2].A1m.cast(); 146 | b[tid].head() += accbc[tid2].A1m.cast(); 147 | } 148 | } 149 | 150 | 151 | // // ----- new: copy transposed parts for calibration only. 152 | // for(int h=0;h(0,hIdx).noalias() = H.block<8,4>(hIdx,0).transpose(); 156 | // } 157 | } 158 | 159 | void AccumulatedSCHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, int tid) 160 | { 161 | 162 | int nf = nframes[0]; 163 | int nframes2 = nf*nf; 164 | 165 | H = MatXX::Zero(nf*8+CPARS, nf*8+CPARS); 166 | b = VecX::Zero(nf*8+CPARS); 167 | 168 | 169 | for(int i=0;i(); 180 | Vec8 accEBV = accEB[tid][ijIdx].A1m.cast(); 181 | 182 | H.block<8,CPARS>(iIdx,0) += EF->adHost[ijIdx] * accEM; 183 | H.block<8,CPARS>(jIdx,0) += EF->adTarget[ijIdx] * accEM; 184 | 185 | b.segment<8>(iIdx) += EF->adHost[ijIdx] * accEBV; 186 | b.segment<8>(jIdx) += EF->adTarget[ijIdx] * accEBV; 187 | 188 | for(int k=0;k(); 197 | 198 | H.block<8,8>(iIdx, iIdx) += EF->adHost[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 199 | 200 | H.block<8,8>(jIdx, kIdx) += EF->adTarget[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 201 | 202 | H.block<8,8>(jIdx, iIdx) += EF->adTarget[ijIdx] * accDM * EF->adHost[ikIdx].transpose(); 203 | 204 | H.block<8,8>(iIdx, kIdx) += EF->adHost[ijIdx] * accDM * EF->adTarget[ikIdx].transpose(); 205 | } 206 | } 207 | 208 | accHcc[tid].finish(); 209 | accbc[tid].finish(); 210 | H.topLeftCorner() = accHcc[tid].A1m.cast(); 211 | b.head() = accbc[tid].A1m.cast(); 212 | 213 | // ----- new: copy transposed parts for calibration only. 214 | for(int h=0;h(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 218 | } 219 | } 220 | 221 | } 222 | -------------------------------------------------------------------------------- /03_Application/dso/src/OptimizationBackend/AccumulatedSCHessian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | #include "util/IndexThreadReduce.h" 30 | #include "OptimizationBackend/MatrixAccumulators.h" 31 | #include "vector" 32 | #include 33 | 34 | namespace dso 35 | { 36 | 37 | class EFPoint; 38 | class EnergyFunctional; 39 | 40 | 41 | class AccumulatedSCHessianSSE 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 45 | inline AccumulatedSCHessianSSE() 46 | { 47 | for(int i=0;i[n*n]; 73 | accEB[tid] = new AccumulatorX<8>[n*n]; 74 | accD[tid] = new AccumulatorXX<8,8>[n*n*n]; 75 | } 76 | accbc[tid].initialize(); 77 | accHcc[tid].initialize(); 78 | 79 | for(int i=0;i* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool MT) 94 | { 95 | // sum up, splitting by bock in square. 96 | if(MT) 97 | { 98 | MatXX Hs[NUM_THREADS]; 99 | VecX bs[NUM_THREADS]; 100 | for(int i=0;ireduce(boost::bind(&AccumulatedSCHessianSSE::stitchDoubleInternal, 108 | this,Hs, bs, EF, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); 109 | 110 | // sum up results 111 | H = Hs[0]; 112 | b = bs[0]; 113 | 114 | for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 132 | } 133 | } 134 | 135 | 136 | AccumulatorXX<8,CPARS>* accE[NUM_THREADS]; 137 | AccumulatorX<8>* accEB[NUM_THREADS]; 138 | AccumulatorXX<8,8>* accD[NUM_THREADS]; 139 | AccumulatorXX accHcc[NUM_THREADS]; 140 | AccumulatorX accbc[NUM_THREADS]; 141 | int nframes[NUM_THREADS]; 142 | 143 | 144 | void addPointsInternal( 145 | std::vector* points, bool shiftPriorToZero, 146 | int min=0, int max=1, Vec10* stats=0, int tid=0) 147 | { 148 | for(int i=min;i, 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #include "OptimizationBackend/AccumulatedTopHessian.h" 26 | #include "OptimizationBackend/EnergyFunctional.h" 27 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 28 | #include 29 | 30 | #if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) 31 | #include "SSE2NEON.h" 32 | #endif 33 | 34 | namespace dso 35 | { 36 | 37 | 38 | 39 | template 40 | void AccumulatedTopHessianSSE::addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid) // 0 = active, 1 = linearized, 2=marginalize 41 | { 42 | 43 | 44 | assert(mode==0 || mode==1 || mode==2); 45 | 46 | VecCf dc = ef->cDeltaF; 47 | float dd = p->deltaF; 48 | 49 | float bd_acc=0; 50 | float Hdd_acc=0; 51 | VecCf Hcd_acc = VecCf::Zero(); 52 | 53 | for(EFResidual* r : p->residualsAll) 54 | { 55 | if(mode==0) 56 | { 57 | if(r->isLinearized || !r->isActive()) continue; 58 | } 59 | if(mode==1) 60 | { 61 | if(!r->isLinearized || !r->isActive()) continue; 62 | } 63 | if(mode==2) 64 | { 65 | if(!r->isActive()) continue; 66 | assert(r->isLinearized); 67 | } 68 | 69 | 70 | RawResidualJacobian* rJ = r->J; 71 | int htIDX = r->hostIDX + r->targetIDX*nframes[tid]; 72 | Mat18f dp = ef->adHTdeltaF[htIDX]; 73 | 74 | 75 | 76 | VecNRf resApprox; 77 | if(mode==0) 78 | resApprox = rJ->resF; 79 | if(mode==2) 80 | resApprox = r->res_toZeroF; 81 | if(mode==1) 82 | { 83 | // compute Jp*delta 84 | __m128 Jp_delta_x = _mm_set1_ps(rJ->Jpdxi[0].dot(dp.head<6>())+rJ->Jpdc[0].dot(dc)+rJ->Jpdd[0]*dd); 85 | __m128 Jp_delta_y = _mm_set1_ps(rJ->Jpdxi[1].dot(dp.head<6>())+rJ->Jpdc[1].dot(dc)+rJ->Jpdd[1]*dd); 86 | __m128 delta_a = _mm_set1_ps((float)(dp[6])); 87 | __m128 delta_b = _mm_set1_ps((float)(dp[7])); 88 | 89 | for(int i=0;ires_toZeroF)+i); 93 | rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x)); 94 | rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y)); 95 | rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a)); 96 | rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b)); 97 | _mm_store_ps(((float*)&resApprox)+i, rtz); 98 | } 99 | } 100 | 101 | // need to compute JI^T * r, and Jab^T * r. (both are 2-vectors). 102 | Vec2f JI_r(0,0); 103 | Vec2f Jab_r(0,0); 104 | float rr=0; 105 | for(int i=0;iJIdx[0][i]; 108 | JI_r[1] += resApprox[i] *rJ->JIdx[1][i]; 109 | Jab_r[0] += resApprox[i] *rJ->JabF[0][i]; 110 | Jab_r[1] += resApprox[i] *rJ->JabF[1][i]; 111 | rr += resApprox[i]*resApprox[i]; 112 | } 113 | 114 | 115 | acc[tid][htIDX].update( 116 | rJ->Jpdc[0].data(), rJ->Jpdxi[0].data(), 117 | rJ->Jpdc[1].data(), rJ->Jpdxi[1].data(), 118 | rJ->JIdx2(0,0),rJ->JIdx2(0,1),rJ->JIdx2(1,1)); 119 | 120 | acc[tid][htIDX].updateBotRight( 121 | rJ->Jab2(0,0), rJ->Jab2(0,1), Jab_r[0], 122 | rJ->Jab2(1,1), Jab_r[1],rr); 123 | 124 | acc[tid][htIDX].updateTopRight( 125 | rJ->Jpdc[0].data(), rJ->Jpdxi[0].data(), 126 | rJ->Jpdc[1].data(), rJ->Jpdxi[1].data(), 127 | rJ->JabJIdx(0,0), rJ->JabJIdx(0,1), 128 | rJ->JabJIdx(1,0), rJ->JabJIdx(1,1), 129 | JI_r[0], JI_r[1]); 130 | 131 | 132 | Vec2f Ji2_Jpdd = rJ->JIdx2 * rJ->Jpdd; 133 | bd_acc += JI_r[0]*rJ->Jpdd[0] + JI_r[1]*rJ->Jpdd[1]; 134 | Hdd_acc += Ji2_Jpdd.dot(rJ->Jpdd); 135 | Hcd_acc += rJ->Jpdc[0]*Ji2_Jpdd[0] + rJ->Jpdc[1]*Ji2_Jpdd[1]; 136 | 137 | nres[tid]++; 138 | } 139 | 140 | if(mode==0) 141 | { 142 | p->Hdd_accAF = Hdd_acc; 143 | p->bd_accAF = bd_acc; 144 | p->Hcd_accAF = Hcd_acc; 145 | } 146 | if(mode==1 || mode==2) 147 | { 148 | p->Hdd_accLF = Hdd_acc; 149 | p->bd_accLF = bd_acc; 150 | p->Hcd_accLF = Hcd_acc; 151 | } 152 | if(mode==2) 153 | { 154 | p->Hcd_accAF.setZero(); 155 | p->Hdd_accAF = 0; 156 | p->bd_accAF = 0; 157 | } 158 | 159 | } 160 | template void AccumulatedTopHessianSSE::addPoint<0>(EFPoint* p, EnergyFunctional const * const ef, int tid); 161 | template void AccumulatedTopHessianSSE::addPoint<1>(EFPoint* p, EnergyFunctional const * const ef, int tid); 162 | template void AccumulatedTopHessianSSE::addPoint<2>(EFPoint* p, EnergyFunctional const * const ef, int tid); 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | void AccumulatedTopHessianSSE::stitchDouble(MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool useDelta, int tid) 172 | { 173 | H = MatXX::Zero(nframes[tid]*8+CPARS, nframes[tid]*8+CPARS); 174 | b = VecX::Zero(nframes[tid]*8+CPARS); 175 | 176 | 177 | for(int h=0;h(); 190 | 191 | 192 | H.block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose(); 193 | 194 | H.block<8,8>(tIdx, tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); 195 | 196 | H.block<8,8>(hIdx, tIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); 197 | 198 | H.block<8,CPARS>(hIdx,0).noalias() += EF->adHost[aidx] * accH.block<8,CPARS>(CPARS,0); 199 | 200 | H.block<8,CPARS>(tIdx,0).noalias() += EF->adTarget[aidx] * accH.block<8,CPARS>(CPARS,0); 201 | 202 | H.topLeftCorner().noalias() += accH.block(0,0); 203 | 204 | b.segment<8>(hIdx).noalias() += EF->adHost[aidx] * accH.block<8,1>(CPARS,8+CPARS); 205 | 206 | b.segment<8>(tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,1>(CPARS,8+CPARS); 207 | 208 | b.head().noalias() += accH.block(0,8+CPARS); 209 | } 210 | 211 | 212 | // ----- new: copy transposed parts. 213 | for(int h=0;h(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 217 | 218 | for(int t=h+1;t(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose(); 222 | H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose(); 223 | } 224 | } 225 | 226 | 227 | if(usePrior) 228 | { 229 | assert(useDelta); 230 | H.diagonal().head() += EF->cPrior; 231 | b.head() += EF->cPrior.cwiseProduct(EF->cDeltaF.cast()); 232 | for(int h=0;h(CPARS+h*8) += EF->frames[h]->prior; 235 | b.segment<8>(CPARS+h*8) += EF->frames[h]->prior.cwiseProduct(EF->frames[h]->delta_prior); 236 | } 237 | } 238 | } 239 | 240 | 241 | void AccumulatedTopHessianSSE::stitchDoubleInternal( 242 | MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior, 243 | int min, int max, Vec10* stats, int tid) 244 | { 245 | int toAggregate = NUM_THREADS; 246 | if(tid == -1) { toAggregate = 1; tid = 0; } // special case: if we dont do multithreading, dont aggregate. 247 | if(min==max) return; 248 | 249 | 250 | for(int k=min;k(); 268 | } 269 | 270 | H[tid].block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose(); 271 | 272 | H[tid].block<8,8>(tIdx, tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); 273 | 274 | H[tid].block<8,8>(hIdx, tIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adTarget[aidx].transpose(); 275 | 276 | H[tid].block<8,CPARS>(hIdx,0).noalias() += EF->adHost[aidx] * accH.block<8,CPARS>(CPARS,0); 277 | 278 | H[tid].block<8,CPARS>(tIdx,0).noalias() += EF->adTarget[aidx] * accH.block<8,CPARS>(CPARS,0); 279 | 280 | H[tid].topLeftCorner().noalias() += accH.block(0,0); 281 | 282 | b[tid].segment<8>(hIdx).noalias() += EF->adHost[aidx] * accH.block<8,1>(CPARS,CPARS+8); 283 | 284 | b[tid].segment<8>(tIdx).noalias() += EF->adTarget[aidx] * accH.block<8,1>(CPARS,CPARS+8); 285 | 286 | b[tid].head().noalias() += accH.block(0,CPARS+8); 287 | 288 | } 289 | 290 | 291 | // only do this on one thread. 292 | if(min==0 && usePrior) 293 | { 294 | H[tid].diagonal().head() += EF->cPrior; 295 | b[tid].head() += EF->cPrior.cwiseProduct(EF->cDeltaF.cast()); 296 | for(int h=0;h(CPARS+h*8) += EF->frames[h]->prior; 299 | b[tid].segment<8>(CPARS+h*8) += EF->frames[h]->prior.cwiseProduct(EF->frames[h]->delta_prior); 300 | 301 | } 302 | } 303 | } 304 | 305 | 306 | 307 | } 308 | 309 | 310 | -------------------------------------------------------------------------------- /03_Application/dso/src/OptimizationBackend/AccumulatedTopHessian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | #include "OptimizationBackend/MatrixAccumulators.h" 30 | #include "vector" 31 | #include 32 | #include "util/IndexThreadReduce.h" 33 | 34 | 35 | namespace dso 36 | { 37 | 38 | class EFPoint; 39 | class EnergyFunctional; 40 | 41 | 42 | 43 | class AccumulatedTopHessianSSE 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 47 | inline AccumulatedTopHessianSSE() 48 | { 49 | for(int tid=0;tid < NUM_THREADS; tid++) 50 | { 51 | nres[tid]=0; 52 | acc[tid]=0; 53 | nframes[tid]=0; 54 | } 55 | 56 | }; 57 | inline ~AccumulatedTopHessianSSE() 58 | { 59 | for(int tid=0;tid < NUM_THREADS; tid++) 60 | { 61 | if(acc[tid] != 0) delete[] acc[tid]; 62 | } 63 | }; 64 | 65 | inline void setZero(int nFrames, int min=0, int max=1, Vec10* stats=0, int tid=0) 66 | { 67 | 68 | if(nFrames != nframes[tid]) 69 | { 70 | if(acc[tid] != 0) delete[] acc[tid]; 71 | #if USE_XI_MODEL 72 | acc[tid] = new Accumulator14[nFrames*nFrames]; 73 | #else 74 | acc[tid] = new AccumulatorApprox[nFrames*nFrames]; 75 | #endif 76 | } 77 | 78 | for(int i=0;i void addPoint(EFPoint* p, EnergyFunctional const * const ef, int tid=0); 88 | 89 | 90 | 91 | void stitchDoubleMT(IndexThreadReduce* red, MatXX &H, VecX &b, EnergyFunctional const * const EF, bool usePrior, bool MT) 92 | { 93 | // sum up, splitting by bock in square. 94 | if(MT) 95 | { 96 | MatXX Hs[NUM_THREADS]; 97 | VecX bs[NUM_THREADS]; 98 | for(int i=0;ireduce(boost::bind(&AccumulatedTopHessianSSE::stitchDoubleInternal, 106 | this,Hs, bs, EF, usePrior, _1, _2, _3, _4), 0, nframes[0]*nframes[0], 0); 107 | 108 | // sum up results 109 | H = Hs[0]; 110 | b = bs[0]; 111 | 112 | for(int i=1;i(0,hIdx).noalias() = H.block<8,CPARS>(hIdx,0).transpose(); 131 | 132 | for(int t=h+1;t(hIdx, tIdx).noalias() += H.block<8,8>(tIdx, hIdx).transpose(); 136 | H.block<8,8>(tIdx, hIdx).noalias() = H.block<8,8>(hIdx, tIdx).transpose(); 137 | } 138 | } 139 | } 140 | 141 | 142 | 143 | 144 | int nframes[NUM_THREADS]; 145 | 146 | EIGEN_ALIGN16 AccumulatorApprox* acc[NUM_THREADS]; 147 | 148 | 149 | int nres[NUM_THREADS]; 150 | 151 | 152 | template void addPointsInternal( 153 | std::vector* points, EnergyFunctional const * const ef, 154 | int min=0, int max=1, Vec10* stats=0, int tid=0) 155 | { 156 | for(int i=min;i((*points)[i],ef,tid); 157 | } 158 | 159 | 160 | 161 | private: 162 | 163 | void stitchDoubleInternal( 164 | MatXX* H, VecX* b, EnergyFunctional const * const EF, bool usePrior, 165 | int min, int max, Vec10* stats, int tid); 166 | }; 167 | } 168 | 169 | -------------------------------------------------------------------------------- /03_Application/dso/src/OptimizationBackend/EnergyFunctional.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | #include "util/IndexThreadReduce.h" 30 | #include "vector" 31 | #include 32 | #include "map" 33 | 34 | 35 | namespace dso 36 | { 37 | 38 | class PointFrameResidual; 39 | class CalibHessian; 40 | class FrameHessian; 41 | class PointHessian; 42 | 43 | 44 | class EFResidual; 45 | class EFPoint; 46 | class EFFrame; 47 | class EnergyFunctional; 48 | class AccumulatedTopHessian; 49 | class AccumulatedTopHessianSSE; 50 | class AccumulatedSCHessian; 51 | class AccumulatedSCHessianSSE; 52 | 53 | 54 | extern bool EFAdjointsValid; 55 | extern bool EFIndicesValid; 56 | extern bool EFDeltaValid; 57 | 58 | 59 | 60 | class EnergyFunctional { 61 | public: 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 63 | friend class EFFrame; 64 | friend class EFPoint; 65 | friend class EFResidual; 66 | friend class AccumulatedTopHessian; 67 | friend class AccumulatedTopHessianSSE; 68 | friend class AccumulatedSCHessian; 69 | friend class AccumulatedSCHessianSSE; 70 | 71 | EnergyFunctional(); 72 | ~EnergyFunctional(); 73 | 74 | 75 | EFResidual* insertResidual(PointFrameResidual* r); 76 | EFFrame* insertFrame(FrameHessian* fh, CalibHessian* Hcalib); 77 | EFPoint* insertPoint(PointHessian* ph); 78 | 79 | void dropResidual(EFResidual* r); 80 | void marginalizeFrame(EFFrame* fh); 81 | void removePoint(EFPoint* ph); 82 | 83 | 84 | 85 | void marginalizePointsF(); 86 | void dropPointsF(); 87 | void solveSystemF(int iteration, double lambda, CalibHessian* HCalib); 88 | double calcMEnergyF(); 89 | double calcLEnergyF_MT(); 90 | 91 | 92 | void makeIDX(); 93 | 94 | void setDeltaF(CalibHessian* HCalib); 95 | 96 | void setAdjointsF(CalibHessian* Hcalib); 97 | 98 | std::vector frames; 99 | int nPoints, nFrames, nResiduals; 100 | 101 | MatXX HM; 102 | VecX bM; 103 | 104 | int resInA, resInL, resInM; 105 | MatXX lastHS; 106 | VecX lastbS; 107 | VecX lastX; 108 | std::vector lastNullspaces_forLogging; 109 | std::vector lastNullspaces_pose; 110 | std::vector lastNullspaces_scale; 111 | std::vector lastNullspaces_affA; 112 | std::vector lastNullspaces_affB; 113 | 114 | IndexThreadReduce* red; 115 | 116 | 117 | std::map, 120 | Eigen::aligned_allocator> 121 | > connectivityMap; 122 | 123 | private: 124 | 125 | VecX getStitchedDeltaF() const; 126 | 127 | void resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT); 128 | void resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid); 129 | 130 | void accumulateAF_MT(MatXX &H, VecX &b, bool MT); 131 | void accumulateLF_MT(MatXX &H, VecX &b, bool MT); 132 | void accumulateSCF_MT(MatXX &H, VecX &b, bool MT); 133 | 134 | void calcLEnergyPt(int min, int max, Vec10* stats, int tid); 135 | 136 | void orthogonalize(VecX* b, MatXX* H); 137 | Mat18f* adHTdeltaF; 138 | 139 | Mat88* adHost; 140 | Mat88* adTarget; 141 | 142 | Mat88f* adHostF; 143 | Mat88f* adTargetF; 144 | 145 | 146 | VecC cPrior; 147 | VecCf cDeltaF; 148 | VecCf cPriorF; 149 | 150 | AccumulatedTopHessianSSE* accSSE_top_L; 151 | AccumulatedTopHessianSSE* accSSE_top_A; 152 | 153 | 154 | AccumulatedSCHessianSSE* accSSE_bot; 155 | 156 | std::vector allPoints; 157 | std::vector allPointsToMarg; 158 | 159 | float currentLambda; 160 | }; 161 | } 162 | 163 | -------------------------------------------------------------------------------- /03_Application/dso/src/OptimizationBackend/EnergyFunctionalStructs.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #include "OptimizationBackend/EnergyFunctionalStructs.h" 26 | #include "OptimizationBackend/EnergyFunctional.h" 27 | #include "FullSystem/FullSystem.h" 28 | #include "FullSystem/HessianBlocks.h" 29 | #include "FullSystem/Residuals.h" 30 | 31 | #if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) 32 | #include "SSE2NEON.h" 33 | #endif 34 | 35 | namespace dso 36 | { 37 | 38 | 39 | void EFResidual::takeDataF() 40 | { 41 | std::swap(J, data->J); 42 | 43 | Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd; 44 | 45 | for(int i=0;i<6;i++) 46 | JpJdF[i] = J->Jpdxi[0][i]*JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1]; 47 | 48 | JpJdF.segment<2>(6) = J->JabJIdx*J->Jpdd; 49 | } 50 | 51 | 52 | void EFFrame::takeData() 53 | { 54 | prior = data->getPrior().head<8>(); 55 | delta = data->get_state_minus_stateZero().head<8>(); 56 | delta_prior = (data->get_state() - data->getPriorZero()).head<8>(); 57 | 58 | 59 | 60 | // Vec10 state_zero = data->get_state_zero(); 61 | // state_zero.segment<3>(0) = SCALE_XI_TRANS * state_zero.segment<3>(0); 62 | // state_zero.segment<3>(3) = SCALE_XI_ROT * state_zero.segment<3>(3); 63 | // state_zero[6] = SCALE_A * state_zero[6]; 64 | // state_zero[7] = SCALE_B * state_zero[7]; 65 | // state_zero[8] = SCALE_A * state_zero[8]; 66 | // state_zero[9] = SCALE_B * state_zero[9]; 67 | // 68 | // std::cout << "state_zero: " << state_zero.transpose() << "\n"; 69 | 70 | 71 | assert(data->frameID != -1); 72 | 73 | frameID = data->frameID; 74 | } 75 | 76 | 77 | 78 | 79 | void EFPoint::takeData() 80 | { 81 | priorF = data->hasDepthPrior ? setting_idepthFixPrior*SCALE_IDEPTH*SCALE_IDEPTH : 0; 82 | if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) priorF=0; 83 | 84 | deltaF = data->idepth-data->idepth_zero; 85 | } 86 | 87 | 88 | void EFResidual::fixLinearizationF(EnergyFunctional* ef) 89 | { 90 | Vec8f dp = ef->adHTdeltaF[hostIDX+ef->nFrames*targetIDX]; 91 | 92 | // compute Jp*delta 93 | __m128 Jp_delta_x = _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>()) 94 | +J->Jpdc[0].dot(ef->cDeltaF) 95 | +J->Jpdd[0]*point->deltaF); 96 | __m128 Jp_delta_y = _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>()) 97 | +J->Jpdc[1].dot(ef->cDeltaF) 98 | +J->Jpdd[1]*point->deltaF); 99 | __m128 delta_a = _mm_set1_ps((float)(dp[6])); 100 | __m128 delta_b = _mm_set1_ps((float)(dp[7])); 101 | 102 | for(int i=0;iresF)+i); 106 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x)); 107 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y)); 108 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a)); 109 | rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b)); 110 | _mm_store_ps(((float*)&res_toZeroF)+i, rtz); 111 | } 112 | 113 | isLinearized = true; 114 | } 115 | 116 | } 117 | -------------------------------------------------------------------------------- /03_Application/dso/src/OptimizationBackend/EnergyFunctionalStructs.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | #include "vector" 30 | #include 31 | #include "OptimizationBackend/RawResidualJacobian.h" 32 | 33 | namespace dso 34 | { 35 | 36 | class PointFrameResidual; 37 | class CalibHessian; 38 | class FrameHessian; 39 | class PointHessian; 40 | 41 | class EFResidual; 42 | class EFPoint; 43 | class EFFrame; 44 | class EnergyFunctional; 45 | 46 | 47 | 48 | 49 | 50 | 51 | class EFResidual 52 | { 53 | public: 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 55 | 56 | inline EFResidual(PointFrameResidual* org, EFPoint* point_, EFFrame* host_, EFFrame* target_) : 57 | data(org), point(point_), host(host_), target(target_) 58 | { 59 | isLinearized=false; 60 | isActiveAndIsGoodNEW=false; 61 | J = new RawResidualJacobian(); 62 | assert(((long)this)%16==0); 63 | assert(((long)J)%16==0); 64 | } 65 | inline ~EFResidual() 66 | { 67 | delete J; 68 | } 69 | 70 | 71 | void takeDataF(); 72 | 73 | 74 | void fixLinearizationF(EnergyFunctional* ef); 75 | 76 | 77 | // structural pointers 78 | PointFrameResidual* data; 79 | int hostIDX, targetIDX; 80 | EFPoint* point; 81 | EFFrame* host; 82 | EFFrame* target; 83 | int idxInAll; 84 | 85 | RawResidualJacobian* J; 86 | 87 | VecNRf res_toZeroF; 88 | Vec8f JpJdF; 89 | 90 | 91 | // status. 92 | bool isLinearized; 93 | 94 | // if residual is not OOB & not OUTLIER & should be used during accumulations 95 | bool isActiveAndIsGoodNEW; 96 | inline const bool &isActive() const {return isActiveAndIsGoodNEW;} 97 | }; 98 | 99 | 100 | enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP}; 101 | 102 | class EFPoint 103 | { 104 | public: 105 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 106 | EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_) 107 | { 108 | takeData(); 109 | stateFlag=EFPointStatus::PS_GOOD; 110 | } 111 | void takeData(); 112 | 113 | PointHessian* data; 114 | 115 | 116 | 117 | float priorF; 118 | float deltaF; 119 | 120 | 121 | // constant info (never changes in-between). 122 | int idxInPoints; 123 | EFFrame* host; 124 | 125 | // contains all residuals. 126 | std::vector residualsAll; 127 | 128 | float bdSumF; 129 | float HdiF; 130 | float Hdd_accLF; 131 | VecCf Hcd_accLF; 132 | float bd_accLF; 133 | float Hdd_accAF; 134 | VecCf Hcd_accAF; 135 | float bd_accAF; 136 | 137 | 138 | EFPointStatus stateFlag; 139 | }; 140 | 141 | 142 | 143 | class EFFrame 144 | { 145 | public: 146 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 147 | EFFrame(FrameHessian* d) : data(d) 148 | { 149 | takeData(); 150 | } 151 | void takeData(); 152 | 153 | 154 | Vec8 prior; // prior hessian (diagonal) 155 | Vec8 delta_prior; // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior) 156 | Vec8 delta; // state - state_zero. 157 | 158 | 159 | 160 | std::vector points; 161 | FrameHessian* data; 162 | int idx; // idx in frames. 163 | 164 | int frameID; 165 | }; 166 | 167 | } 168 | 169 | -------------------------------------------------------------------------------- /03_Application/dso/src/OptimizationBackend/RawResidualJacobian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | 28 | #include "util/NumType.h" 29 | 30 | namespace dso 31 | { 32 | struct RawResidualJacobian 33 | { 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 35 | // ================== new structure: save independently =============. 36 | VecNRf resF; 37 | 38 | // the two rows of d[x,y]/d[xi]. 39 | Vec6f Jpdxi[2]; // 2x6 40 | 41 | // the two rows of d[x,y]/d[C]. 42 | VecCf Jpdc[2]; // 2x4 43 | 44 | // the two rows of d[x,y]/d[idepth]. 45 | Vec2f Jpdd; // 2x1 46 | 47 | // the two columns of d[r]/d[x,y]. 48 | VecNRf JIdx[2]; // 9x2 49 | 50 | // = the two columns of d[r] / d[ab] 51 | VecNRf JabF[2]; // 9x2 52 | 53 | 54 | // = JIdx^T * JIdx (inner product). Only as a shorthand. 55 | Mat22f JIdx2; // 2x2 56 | // = Jab^T * JIdx (inner product). Only as a shorthand. 57 | Mat22f JabJIdx; // 2x2 58 | // = Jab^T * Jab (inner product). Only as a shorthand. 59 | Mat22f Jab2; // 2x2 60 | 61 | }; 62 | } 63 | 64 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/FrameShell.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | #include "algorithm" 29 | 30 | namespace dso 31 | { 32 | 33 | 34 | class FrameShell 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 38 | int id; // INTERNAL ID, starting at zero. 39 | int incoming_id; // ID passed into DSO 40 | double timestamp; // timestamp passed into DSO. 41 | 42 | // set once after tracking 43 | SE3 camToTrackingRef; 44 | FrameShell* trackingRef; 45 | 46 | // constantly adapted. 47 | SE3 camToWorld; // Write: TRACKING, while frame is still fresh; MAPPING: only when locked [shellPoseMutex]. 48 | SE3 camToWorld_predicted; // this is used for storing the (Kalman) filtered camera pose 49 | bool hasPrediction; // this is used for checking if a (Kalman) filtered camera pose is available 50 | SE3 camToWorld_measured; // this is used for integrating other Measurements, e.g. GPS 51 | bool hasMeasurement; // this is used for checking if other measurements are available 52 | AffLight aff_g2l; 53 | bool poseValid; 54 | 55 | // statisitcs 56 | int statistics_outlierResOnThis; 57 | int statistics_goodResOnThis; 58 | int marginalizedAt; 59 | double movedByOpt; 60 | 61 | 62 | inline FrameShell() 63 | { 64 | id=0; 65 | poseValid=true; 66 | camToWorld = SE3(); 67 | camToWorld_predicted = SE3(); 68 | hasPrediction=false; 69 | camToWorld_measured = SE3(); 70 | hasMeasurement=false; 71 | timestamp=0; 72 | marginalizedAt=-1; 73 | movedByOpt=0; 74 | statistics_outlierResOnThis=statistics_goodResOnThis=0; 75 | trackingRef=0; 76 | camToTrackingRef = SE3(); 77 | } 78 | }; 79 | 80 | 81 | } 82 | 83 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/ImageAndExposure.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | #include 27 | #include 28 | 29 | 30 | namespace dso 31 | { 32 | 33 | 34 | class ImageAndExposure 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 38 | float* image; // irradiance. between 0 and 256 39 | int w,h; // width and height; 40 | double timestamp; 41 | float exposure_time; // exposure time in ms. 42 | inline ImageAndExposure(int w_, int h_, double timestamp_=0) : w(w_), h(h_), timestamp(timestamp_) 43 | { 44 | image = new float[w*h]; 45 | exposure_time=1; 46 | } 47 | inline ~ImageAndExposure() 48 | { 49 | delete[] image; 50 | } 51 | 52 | inline void copyMetaTo(ImageAndExposure &other) 53 | { 54 | other.exposure_time = exposure_time; 55 | } 56 | 57 | inline ImageAndExposure* getDeepCopy() 58 | { 59 | ImageAndExposure* img = new ImageAndExposure(w,h,timestamp); 60 | img->exposure_time = exposure_time; 61 | memcpy(img->image, image, w*h*sizeof(float)); 62 | return img; 63 | } 64 | }; 65 | 66 | 67 | } 68 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/IndexThreadReduce.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #pragma once 27 | #include "util/settings.h" 28 | #include "boost/thread.hpp" 29 | #include 30 | #include 31 | 32 | 33 | 34 | namespace dso 35 | { 36 | 37 | template 38 | class IndexThreadReduce 39 | { 40 | 41 | public: 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 43 | 44 | inline IndexThreadReduce() 45 | { 46 | nextIndex = 0; 47 | maxIndex = 0; 48 | stepSize = 1; 49 | callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); 50 | 51 | running = true; 52 | for(int i=0;i callPerIndex, int first, int end, int stepSize = 0) 77 | { 78 | 79 | memset(&stats, 0, sizeof(Running)); 80 | 81 | // if(!multiThreading) 82 | // { 83 | // callPerIndex(first, end, &stats, 0); 84 | // return; 85 | // } 86 | 87 | 88 | 89 | if(stepSize == 0) 90 | stepSize = ((end-first)+NUM_THREADS-1)/NUM_THREADS; 91 | 92 | 93 | //printf("reduce called\n"); 94 | 95 | boost::unique_lock lock(exMutex); 96 | 97 | // save 98 | this->callPerIndex = callPerIndex; 99 | nextIndex = first; 100 | maxIndex = end; 101 | this->stepSize = stepSize; 102 | 103 | // go worker threads! 104 | for(int i=0;icallPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3, _4); 135 | 136 | //printf("reduce done (all threads finished)\n"); 137 | } 138 | 139 | Running stats; 140 | 141 | private: 142 | boost::thread workerThreads[NUM_THREADS]; 143 | bool isDone[NUM_THREADS]; 144 | bool gotOne[NUM_THREADS]; 145 | 146 | boost::mutex exMutex; 147 | boost::condition_variable todo_signal; 148 | boost::condition_variable done_signal; 149 | 150 | int nextIndex; 151 | int maxIndex; 152 | int stepSize; 153 | 154 | bool running; 155 | 156 | boost::function callPerIndex; 157 | 158 | void callPerIndexDefault(int i, int j,Running* k, int tid) 159 | { 160 | printf("ERROR: should never be called....\n"); 161 | assert(false); 162 | } 163 | 164 | void workerLoop(int idx) 165 | { 166 | boost::unique_lock lock(exMutex); 167 | 168 | while(running) 169 | { 170 | // try to get something to do. 171 | int todo = 0; 172 | bool gotSomething = false; 173 | if(nextIndex < maxIndex) 174 | { 175 | // got something! 176 | todo = nextIndex; 177 | nextIndex+=stepSize; 178 | gotSomething = true; 179 | } 180 | 181 | // if got something: do it (unlock in the meantime) 182 | if(gotSomething) 183 | { 184 | lock.unlock(); 185 | 186 | assert(callPerIndex != 0); 187 | 188 | Running s; memset(&s, 0, sizeof(Running)); 189 | callPerIndex(todo, std::min(todo+stepSize, maxIndex), &s, idx); 190 | gotOne[idx] = true; 191 | lock.lock(); 192 | stats += s; 193 | } 194 | 195 | // otherwise wait on signal, releasing lock in the meantime. 196 | else 197 | { 198 | if(!gotOne[idx]) 199 | { 200 | lock.unlock(); 201 | assert(callPerIndex != 0); 202 | Running s; memset(&s, 0, sizeof(Running)); 203 | callPerIndex(0, 0, &s, idx); 204 | gotOne[idx] = true; 205 | lock.lock(); 206 | stats += s; 207 | } 208 | isDone[idx] = true; 209 | //printf("worker %d waiting..\n", idx); 210 | done_signal.notify_all(); 211 | todo_signal.wait(lock); 212 | } 213 | } 214 | } 215 | }; 216 | } 217 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/KalmanFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this license header, choose License Headers in Project Properties. 3 | * To change this template file, choose Tools | Templates 4 | * and open the template in the editor. 5 | */ 6 | 7 | /* 8 | * File: KalmanFilter.h 9 | * Author: akp 10 | * 11 | * Created on 4. April 2018, 02:40 12 | */ 13 | 14 | #pragma once 15 | 16 | #ifndef KALMANFILTER_H 17 | #define KALMANFILTER_H 18 | 19 | #include "util/FrameShell.h" 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | namespace dso 26 | { 27 | 28 | /* 29 | 30 | This file implements a (linear) Kalman Filter, which estimates the 31 | optimal camera trajectory 32 | 33 | */ 34 | 35 | class KalmanFilter 36 | { 37 | public: 38 | KalmanFilter(); 39 | 40 | 41 | void init(float initx = 0.0, float inity = 0.0, float initz = 0.0, 42 | float initvx = 0.0, float initvy = 0.0, float initvz = 0.0, 43 | float sigmax = 10.0, float sigmay = 10.0, float sigmaz = 10.0, 44 | float sigmavx = 1.0, float sigmavy = 1.0, float sigmavz = 1.0, 45 | float qx = 0.10, float qy = 0.10, float qz = 0.10, 46 | float qvx = 0.01, float qvy = 0.01, float qvz = 0.01, 47 | float rx = 10.0f, float ry = 10.0f, float rz = 10.0f, 48 | float rvx = 1.0f, float rvy = 1.0f, float rvz = 1.0f); 49 | 50 | bool initialized; 51 | bool isInitialized(); 52 | 53 | float initx, inity, initz; 54 | float initvx, initvy, initvz; 55 | float sigmax, sigmay, sigmaz; 56 | float sigmavx, sigmavy, sigmavz; 57 | float qx, qy, qz; 58 | float qvx, qvy, qvz; 59 | float rx, ry, rz; 60 | float rvx, rvy, rvz; 61 | 62 | 63 | Eigen::Matrix state; // State vector: x, y, z, velocityX, velocityY, velocityZ 64 | Eigen::Matrix sigma; // "P"-Matrix (state covariance / uncertainty of state) 65 | 66 | Eigen::Matrix Q; // Possible noise in the predicted new state (process noise) 67 | Eigen::Matrix R_GPS; // Possible noise in the measured new GPS state (observation noise) 68 | Eigen::Matrix R_DSO; // Possible noise in the measured new DSO state (observation noise) 69 | 70 | unsigned long timestampLast; 71 | Eigen::Vector3f dsoTranslationPrevious; 72 | Eigen::Quaternionf dsoOrientation; 73 | 74 | // Next timestep x_{t+1} = g(x_t,u) 75 | // and update uncertainty 76 | void predictionStep(const Eigen::Matrix& currentState); 77 | void predict(float timestep); 78 | 79 | // compare expected and measured values, update state and uncertainty 80 | void correctionStep(const Eigen::Vector3f& measurement); 81 | void updateGPS(const Eigen::Vector3f& measurement); // GPS x-y-z position 82 | void updateDSO(const Eigen::Vector3f& measurement, const Eigen::Quaternionf orientation, float timestep); // Velocities 83 | 84 | /* 85 | Helper Functions 86 | */ 87 | unsigned long getCurrentTimestamp(); 88 | unsigned long getLastTimestamp(); 89 | void setLastTimestampToCurrent(); 90 | double getDeltaTimeInSeconds(); 91 | 92 | 93 | const double a = 6378137.0; //WGS-84 semi-major axis 94 | //const double a = 0.0006378137; //TODO: WRONG (for visualization only) 95 | const double e2 = 6.6943799901377997e-3; //WGS-84 first eccentricity squared 96 | const double a1 = 4.2697672707157535e+4; //a1 = a*e2 97 | const double a2 = 1.8230912546075455e+9; //a2 = a1*a1 98 | const double a3 = 1.4291722289812413e+2; //a3 = a1*e2/2 99 | const double a4 = 4.5577281365188637e+9; //a4 = 2.5*a2 100 | const double a5 = 4.2840589930055659e+4; //a5 = a1+a3 101 | const double a6 = 9.9330562000986220e-1; //a6 = 1-e2 102 | 103 | void ecef_to_geo( double &ecef_x, double &ecef_y, double &ecef_z, double &geo_lat, double &geo_lon, double &geo_alt ); 104 | void geo_to_ecef(double &geo_lat, double &geo_lon, double &geo_alt, double &ecef_x, double &ecef_y, double &ecef_z); 105 | 106 | SE3 getStateAsSE3(); 107 | 108 | protected: 109 | 110 | 111 | }; 112 | 113 | } 114 | 115 | 116 | #endif /* KALMANFILTER_H */ 117 | 118 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/MinimalImage.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "util/NumType.h" 28 | #include "algorithm" 29 | 30 | namespace dso 31 | { 32 | 33 | template 34 | class MinimalImage 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | int w; 39 | int h; 40 | T* data; 41 | 42 | /* 43 | * creates minimal image with own memory 44 | */ 45 | inline MinimalImage(int w_, int h_) : w(w_), h(h_) 46 | { 47 | data = new T[w*h]; 48 | ownData=true; 49 | } 50 | 51 | /* 52 | * creates minimal image wrapping around existing memory 53 | */ 54 | inline MinimalImage(int w_, int h_, T* data_) : w(w_), h(h_) 55 | { 56 | data = data_; 57 | ownData=false; 58 | } 59 | 60 | inline ~MinimalImage() 61 | { 62 | if(ownData) delete [] data; 63 | } 64 | 65 | inline MinimalImage* getClone() 66 | { 67 | MinimalImage* clone = new MinimalImage(w,h); 68 | memcpy(clone->data, data, sizeof(T)*w*h); 69 | return clone; 70 | } 71 | 72 | 73 | inline T& at(int x, int y) {return data[(int)x+((int)y)*w];} 74 | inline T& at(int i) {return data[i];} 75 | 76 | inline void setBlack() 77 | { 78 | memset(data, 0, sizeof(T)*w*h); 79 | } 80 | 81 | inline void setConst(T val) 82 | { 83 | for(int i=0;i Vec3b; 160 | typedef MinimalImage MinimalImageF; 161 | typedef MinimalImage MinimalImageF3; 162 | typedef MinimalImage MinimalImageB; 163 | typedef MinimalImage MinimalImageB3; 164 | typedef MinimalImage MinimalImageB16; 165 | 166 | } 167 | 168 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/NumType.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "Eigen/Core" 28 | #include "sophus/sim3.hpp" 29 | #include "sophus/se3.hpp" 30 | 31 | 32 | namespace dso 33 | { 34 | 35 | // CAMERA MODEL TO USE 36 | 37 | 38 | #define SSEE(val,idx) (*(((float*)&val)+idx)) 39 | 40 | 41 | #define MAX_RES_PER_POINT 8 42 | #define NUM_THREADS 6 43 | 44 | 45 | #define todouble(x) (x).cast() 46 | 47 | 48 | typedef Sophus::SE3d SE3; 49 | typedef Sophus::Sim3d Sim3; 50 | typedef Sophus::SO3d SO3; 51 | 52 | 53 | 54 | #define CPARS 4 55 | 56 | 57 | typedef Eigen::Matrix MatXX; 58 | typedef Eigen::Matrix MatCC; 59 | #define MatToDynamic(x) MatXX(x) 60 | 61 | 62 | 63 | typedef Eigen::Matrix MatC10; 64 | typedef Eigen::Matrix Mat1010; 65 | typedef Eigen::Matrix Mat1313; 66 | 67 | typedef Eigen::Matrix Mat810; 68 | typedef Eigen::Matrix Mat83; 69 | typedef Eigen::Matrix Mat66; 70 | typedef Eigen::Matrix Mat53; 71 | typedef Eigen::Matrix Mat43; 72 | typedef Eigen::Matrix Mat42; 73 | typedef Eigen::Matrix Mat33; 74 | typedef Eigen::Matrix Mat22; 75 | typedef Eigen::Matrix Mat8C; 76 | typedef Eigen::Matrix MatC8; 77 | typedef Eigen::Matrix Mat8Cf; 78 | typedef Eigen::Matrix MatC8f; 79 | 80 | typedef Eigen::Matrix Mat88; 81 | typedef Eigen::Matrix Mat77; 82 | 83 | typedef Eigen::Matrix VecC; 84 | typedef Eigen::Matrix VecCf; 85 | typedef Eigen::Matrix Vec13; 86 | typedef Eigen::Matrix Vec10; 87 | typedef Eigen::Matrix Vec9; 88 | typedef Eigen::Matrix Vec8; 89 | typedef Eigen::Matrix Vec7; 90 | typedef Eigen::Matrix Vec6; 91 | typedef Eigen::Matrix Vec5; 92 | typedef Eigen::Matrix Vec4; 93 | typedef Eigen::Matrix Vec3; 94 | typedef Eigen::Matrix Vec2; 95 | typedef Eigen::Matrix VecX; 96 | 97 | typedef Eigen::Matrix Mat33f; 98 | typedef Eigen::Matrix Mat103f; 99 | typedef Eigen::Matrix Mat22f; 100 | typedef Eigen::Matrix Vec3f; 101 | typedef Eigen::Matrix Vec2f; 102 | typedef Eigen::Matrix Vec6f; 103 | 104 | 105 | 106 | typedef Eigen::Matrix Mat49; 107 | typedef Eigen::Matrix Mat89; 108 | 109 | typedef Eigen::Matrix Mat94; 110 | typedef Eigen::Matrix Mat98; 111 | 112 | typedef Eigen::Matrix Mat81; 113 | typedef Eigen::Matrix Mat18; 114 | typedef Eigen::Matrix Mat91; 115 | typedef Eigen::Matrix Mat19; 116 | 117 | 118 | typedef Eigen::Matrix Mat84; 119 | typedef Eigen::Matrix Mat48; 120 | typedef Eigen::Matrix Mat44; 121 | 122 | 123 | typedef Eigen::Matrix VecNRf; 124 | typedef Eigen::Matrix Vec12f; 125 | typedef Eigen::Matrix Mat18f; 126 | typedef Eigen::Matrix Mat66f; 127 | typedef Eigen::Matrix Mat88f; 128 | typedef Eigen::Matrix Mat84f; 129 | typedef Eigen::Matrix Vec8f; 130 | typedef Eigen::Matrix Vec10f; 131 | typedef Eigen::Matrix Mat66f; 132 | typedef Eigen::Matrix Vec4f; 133 | typedef Eigen::Matrix Mat44f; 134 | typedef Eigen::Matrix Mat1212f; 135 | typedef Eigen::Matrix Vec12f; 136 | typedef Eigen::Matrix Mat1313f; 137 | typedef Eigen::Matrix Mat1010f; 138 | typedef Eigen::Matrix Vec13f; 139 | typedef Eigen::Matrix Mat99f; 140 | typedef Eigen::Matrix Vec9f; 141 | 142 | typedef Eigen::Matrix Mat42f; 143 | typedef Eigen::Matrix Mat62f; 144 | typedef Eigen::Matrix Mat12f; 145 | 146 | typedef Eigen::Matrix VecXf; 147 | typedef Eigen::Matrix MatXXf; 148 | 149 | 150 | typedef Eigen::Matrix MatPCPC; 151 | typedef Eigen::Matrix MatPCPCf; 152 | typedef Eigen::Matrix VecPC; 153 | typedef Eigen::Matrix VecPCf; 154 | 155 | typedef Eigen::Matrix Mat1414f; 156 | typedef Eigen::Matrix Vec14f; 157 | typedef Eigen::Matrix Mat1414; 158 | typedef Eigen::Matrix Vec14; 159 | 160 | 161 | 162 | 163 | 164 | 165 | // transforms points from one frame to another. 166 | struct AffLight 167 | { 168 | AffLight(double a_, double b_) : a(a_), b(b_) {}; 169 | AffLight() : a(0), b(0) {}; 170 | 171 | // Affine Parameters: 172 | double a,b; // I_frame = exp(a)*I_global + b. // I_global = exp(-a)*(I_frame - b). 173 | 174 | static Vec2 fromToVecExposure(float exposureF, float exposureT, AffLight g2F, AffLight g2T) 175 | { 176 | if(exposureF==0 || exposureT==0) 177 | { 178 | exposureT = exposureF = 1; 179 | //printf("got exposure value of 0! please choose the correct model.\n"); 180 | //assert(setting_brightnessTransferFunc < 2); 181 | } 182 | 183 | double a = exp(g2T.a-g2F.a) * exposureT / exposureF; 184 | double b = g2T.b - a*g2F.b; 185 | return Vec2(a,b); 186 | } 187 | 188 | Vec2 vec() 189 | { 190 | return Vec2(a,b); 191 | } 192 | }; 193 | 194 | } 195 | 196 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/UDPServer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this license header, choose License Headers in Project Properties. 3 | * To change this template file, choose Tools | Templates 4 | * and open the template in the editor. 5 | */ 6 | 7 | /* 8 | * File: UDPServer.cpp 9 | * Author: akp 10 | * 11 | * Created on 8. Mai 2018, 00:17 12 | */ 13 | 14 | 15 | #include "UDPServer.h" 16 | 17 | namespace dso 18 | { 19 | 20 | 21 | 22 | 23 | UDPServer::~UDPServer() { 24 | printf("\n----->>>> UDP SERVER DESTROYED\n"); 25 | 26 | socket_.close(); 27 | } 28 | 29 | } -------------------------------------------------------------------------------- /03_Application/dso/src/util/UDPServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * To change this license header, choose License Headers in Project Properties. 3 | * To change this template file, choose Tools | Templates 4 | * and open the template in the editor. 5 | */ 6 | 7 | /* 8 | * File: UDPServer.h 9 | * Author: akp 10 | * 11 | * Created on 8. Mai 2018, 00:17 12 | */ 13 | 14 | #ifndef UDPSERVER_H 15 | #define UDPSERVER_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | using boost::asio::ip::udp; 32 | 33 | namespace dso 34 | { 35 | class UDPServer { 36 | public: 37 | 38 | // Constructor: Initialize to listen on UDP port 2016 39 | UDPServer(boost::asio::io_service &io_service): socket_(io_service, udp::endpoint(udp::v4(), 2016)) 40 | { 41 | 42 | printf("\n----->>>> UDP SERVER CREATED\n"); 43 | timestampLastUpdate_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); 44 | 45 | 46 | start_receive(); 47 | 48 | this->hasMeasurement = false; 49 | //start(); 50 | 51 | }; 52 | 53 | void start() 54 | { 55 | /* 56 | if(io_service.stopped()) 57 | { 58 | io_service.run(); 59 | } 60 | */ 61 | } 62 | void stop() 63 | { 64 | /* 65 | if(!io_service.stopped()) 66 | { 67 | io_service.stop(); 68 | } 69 | */ 70 | } 71 | 72 | bool getMeasurement(double &latitude, double &longitude, double &altitude, double &accuracy) 73 | { 74 | bool success = false; 75 | if (!this->hasMeasurement) return success; 76 | 77 | // Lock mutex to synchronize threads and avoid racing conditions 78 | mtx_.lock(); 79 | 80 | double timestampCurrent = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); 81 | 82 | std::cout << "bool getMeasurement(double &latitude, double &longitude, double &altitude, double &accuracy)" << std::endl; 83 | 84 | // Only return a measurement that has a better accuracy than 10 meters 85 | // TODO: Use statistical methods like e.g. Z-Score to perform proper outlier detection 86 | if( fabs( this->accuracy_ ) < 10.0 ) 87 | { 88 | // Check for age of measurement (in milliseconds). Set to 2 Seconds for now 89 | if( fabs(timestampLastUpdate_ - timestampCurrent) < 2000.0 ) 90 | { 91 | // If the measurement is not too old, return it back 92 | 93 | latitude = this->latitude_; 94 | longitude = this->longitude_; 95 | altitude = this->altitude_; 96 | accuracy = this->accuracy_; 97 | 98 | success = true; 99 | 100 | std::cout << "Got new GPS measurement" << std::endl; 101 | 102 | // Reset hasMeasurement flag to avoid same measurements are being taken multiple times 103 | this->hasMeasurement = false; 104 | } 105 | } 106 | 107 | // Unlock mutex to let go of thread 108 | mtx_.unlock(); 109 | 110 | 111 | 112 | 113 | if(success) 114 | return true; 115 | else 116 | return false; 117 | 118 | } 119 | 120 | 121 | virtual ~UDPServer(); 122 | private: 123 | 124 | std::string make_daytime_string() 125 | { 126 | using namespace std; // For time_t, time and ctime; 127 | time_t now = time(0); 128 | return ctime(&now); 129 | } 130 | 131 | void start_receive() 132 | { 133 | socket_.async_receive_from( 134 | boost::asio::buffer(recv_buffer_), remote_endpoint_, 135 | boost::bind(&UDPServer::handle_receive, this, 136 | boost::asio::placeholders::error, 137 | boost::asio::placeholders::bytes_transferred)); 138 | } 139 | 140 | void handle_receive(const boost::system::error_code& error, 141 | std::size_t bytes_transferred) 142 | { 143 | // Ignore anything that is larger than our buffer 144 | if (!error || error == boost::asio::error::message_size) 145 | { 146 | /* 147 | The datagram contains a message like: 148 | 1.) "VGPSSLAM_FindVGPSServer" 149 | 2.) "VGPSSLAM_VGPSServerIsHere"; 150 | 3.) "GPS#" + location.getLatitude() + "#" + location.getLongitude() + "#" + location.getAccuracy() 151 | */ 152 | 153 | // Complicated way to just get a string from boost... 154 | std::string received; 155 | std::copy(recv_buffer_.begin(), recv_buffer_.begin()+bytes_transferred, std::back_inserter(received)); 156 | std::cout << "Received from UDP: " << received << std::endl; 157 | 158 | 159 | // Internal UDP Logic - determine what to do with the received datagram 160 | if(received == msgFindServer) 161 | { 162 | // Broadcast message to get IP from GPS receiver was received (smartphone) 163 | boost::shared_ptr message( 164 | new std::string(msgServerIsHere)); 165 | 166 | socket_.async_send_to(boost::asio::buffer(*message), remote_endpoint_, 167 | boost::bind(&UDPServer::handle_send, this, message, 168 | boost::asio::placeholders::error, 169 | boost::asio::placeholders::bytes_transferred)); 170 | 171 | } 172 | else if(received == msgServerIsHere) 173 | { 174 | // We ignore messages that tell us where the server is, because we ARE the server ;) 175 | 176 | } 177 | else if(received.compare(0, msgPayloadStart.length(), msgPayloadStart) == 0) 178 | { 179 | // Lock mutex to synchronize threads and avoid racing conditions 180 | mtx_.lock(); 181 | 182 | // Parse String to get individual parts of gps data: 183 | std::vector string_parts; 184 | boost::split(string_parts, received, boost::is_any_of("#")); 185 | 186 | if(string_parts.size() >= 4) 187 | { 188 | // There are four parts: "GPS", "latitude", "longitude", "accuracy" 189 | // E.g.: GPS#49.4144637#11.1298899#20.903 190 | 191 | //EDIT May, 24th, 2018: Smartphone now publishes more data. Current UDP Packet contains: 192 | //"GPS", "latitude", "longitude", "accuracy", "bearing", "speed", "altitude", "provider (gps/wifi/network)" 193 | 194 | // Update timestamp for last update 195 | this->timestampLastUpdate_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); 196 | // Update gps data 197 | this->latitude_ = std::stod(string_parts.at(1)); 198 | this->longitude_ = std::stod(string_parts.at(2)); 199 | this->altitude_ = 0; 200 | this->accuracy_ = std::stod(string_parts.at(3)); 201 | 202 | //std::cout << "Received in handleReceive(): lat=" << latitude<< " lon=" << longitude << " acc=" << accuracy << std::endl; 203 | 204 | this->hasMeasurement = true; 205 | } 206 | 207 | // Unlock mutex to let go of thread 208 | mtx_.unlock(); 209 | 210 | } 211 | 212 | 213 | // Continue to receive UDP datagrams 214 | start_receive(); 215 | 216 | } 217 | } 218 | 219 | 220 | void handle_send(boost::shared_ptr /*message*/, 221 | const boost::system::error_code& /*error*/, 222 | std::size_t /*bytes_transferred*/) 223 | { 224 | //This function is invoked after the service request has been completed. 225 | } 226 | 227 | 228 | udp::socket socket_; 229 | udp::endpoint remote_endpoint_; 230 | boost::array recv_buffer_; 231 | 232 | double latitude_; 233 | double longitude_; 234 | double altitude_; 235 | double accuracy_; 236 | double timestampLastUpdate_; 237 | 238 | const std::string msgFindServer = "VGPSSLAM_FindVGPSServer"; 239 | const std::string msgServerIsHere = "VGPSSLAM_VGPSServerIsHere"; 240 | const std::string msgPayloadStart = "GPS"; 241 | 242 | bool hasMeasurement; 243 | 244 | boost::mutex mtx_; 245 | //boost::thread udpThread_; 246 | }; 247 | 248 | } 249 | #endif /* UDPSERVER_H */ 250 | 251 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/Undistort.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | #pragma once 26 | 27 | #include "util/ImageAndExposure.h" 28 | #include "util/MinimalImage.h" 29 | #include "util/NumType.h" 30 | #include "Eigen/Core" 31 | 32 | 33 | 34 | 35 | 36 | namespace dso 37 | { 38 | 39 | 40 | class PhotometricUndistorter 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 44 | PhotometricUndistorter(std::string file, std::string noiseImage, std::string vignetteImage, int w_, int h_); 45 | ~PhotometricUndistorter(); 46 | 47 | // removes readout noise, and converts to irradiance. 48 | // affine normalizes values to 0 <= I < 256. 49 | // raw irradiance = a*I + b. 50 | // output will be written in [output]. 51 | template void processFrame(T* image_in, float exposure_time, float factor=1); 52 | void unMapFloatImage(float* image); 53 | 54 | ImageAndExposure* output; 55 | 56 | float* getG() {if(!valid) return 0; else return G;}; 57 | private: 58 | float G[256*256]; 59 | int GDepth; 60 | float* vignetteMap; 61 | float* vignetteMapInv; 62 | int w,h; 63 | bool valid; 64 | }; 65 | 66 | 67 | class Undistort 68 | { 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 71 | virtual ~Undistort(); 72 | 73 | virtual void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const = 0; 74 | 75 | 76 | inline const Mat33 getK() const {return K;}; 77 | inline const Eigen::Vector2i getSize() const {return Eigen::Vector2i(w,h);}; 78 | inline const VecX getOriginalParameter() const {return parsOrg;}; 79 | inline const Eigen::Vector2i getOriginalSize() {return Eigen::Vector2i(wOrg,hOrg);}; 80 | inline bool isValid() {return valid;}; 81 | 82 | template 83 | ImageAndExposure* undistort(const MinimalImage* image_raw, float exposure=0, double timestamp=0, float factor=1) const; 84 | static Undistort* getUndistorterForFile(std::string configFilename, std::string gammaFilename, std::string vignetteFilename); 85 | 86 | void loadPhotometricCalibration(std::string file, std::string noiseImage, std::string vignetteImage); 87 | 88 | PhotometricUndistorter* photometricUndist; 89 | 90 | protected: 91 | int w, h, wOrg, hOrg, wUp, hUp; 92 | int upsampleUndistFactor; 93 | Mat33 K; 94 | VecX parsOrg; 95 | bool valid; 96 | bool passthrough; 97 | 98 | float* remapX; 99 | float* remapY; 100 | 101 | void applyBlurNoise(float* img) const; 102 | 103 | void makeOptimalK_crop(); 104 | void makeOptimalK_full(); 105 | 106 | void readFromFile(const char* configFileName, int nPars, std::string prefix = ""); 107 | }; 108 | 109 | class UndistortFOV : public Undistort 110 | { 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 113 | 114 | UndistortFOV(const char* configFileName, bool noprefix); 115 | ~UndistortFOV(); 116 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 117 | 118 | }; 119 | 120 | class UndistortRadTan : public Undistort 121 | { 122 | public: 123 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 124 | UndistortRadTan(const char* configFileName, bool noprefix); 125 | ~UndistortRadTan(); 126 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 127 | 128 | }; 129 | 130 | class UndistortEquidistant : public Undistort 131 | { 132 | public: 133 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 134 | UndistortEquidistant(const char* configFileName, bool noprefix); 135 | ~UndistortEquidistant(); 136 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 137 | 138 | }; 139 | 140 | class UndistortPinhole : public Undistort 141 | { 142 | public: 143 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 144 | UndistortPinhole(const char* configFileName, bool noprefix); 145 | ~UndistortPinhole(); 146 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 147 | 148 | private: 149 | float inputCalibration[8]; 150 | }; 151 | 152 | class UndistortKB : public Undistort 153 | { 154 | public: 155 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 156 | UndistortKB(const char* configFileName, bool noprefix); 157 | ~UndistortKB(); 158 | void distortCoordinates(float* in_x, float* in_y, float* out_x, float* out_y, int n) const; 159 | 160 | }; 161 | 162 | } 163 | 164 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/globalCalib.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #include "util/globalCalib.h" 27 | #include "stdio.h" 28 | #include 29 | 30 | namespace dso 31 | { 32 | int wG[PYR_LEVELS], hG[PYR_LEVELS]; 33 | float fxG[PYR_LEVELS], fyG[PYR_LEVELS], 34 | cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 35 | 36 | float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], 37 | cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; 38 | 39 | Eigen::Matrix3f KG[PYR_LEVELS], KiG[PYR_LEVELS]; 40 | 41 | 42 | float wM3G; 43 | float hM3G; 44 | 45 | void setGlobalCalib(int w, int h,const Eigen::Matrix3f &K) 46 | { 47 | int wlvl=w; 48 | int hlvl=h; 49 | pyrLevelsUsed=1; 50 | while(wlvl%2==0 && hlvl%2==0 && wlvl*hlvl > 5000 && pyrLevelsUsed < PYR_LEVELS) 51 | { 52 | wlvl /=2; 53 | hlvl /=2; 54 | pyrLevelsUsed++; 55 | } 56 | printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n", 57 | pyrLevelsUsed-1, wlvl, hlvl); 58 | if(wlvl>100 && hlvl > 100) 59 | { 60 | printf("\n\n===============WARNING!===================\n " 61 | "using not enough pyramid levels.\n" 62 | "Consider scaling to a resolution that is a multiple of a power of 2.\n"); 63 | } 64 | if(pyrLevelsUsed < 3) 65 | { 66 | printf("\n\n===============WARNING!===================\n " 67 | "I need higher resolution.\n" 68 | "I will probably segfault.\n"); 69 | } 70 | 71 | wM3G = w-3; 72 | hM3G = h-3; 73 | 74 | wG[0] = w; 75 | hG[0] = h; 76 | KG[0] = K; 77 | fxG[0] = K(0,0); 78 | fyG[0] = K(1,1); 79 | cxG[0] = K(0,2); 80 | cyG[0] = K(1,2); 81 | KiG[0] = KG[0].inverse(); 82 | fxiG[0] = KiG[0](0,0); 83 | fyiG[0] = KiG[0](1,1); 84 | cxiG[0] = KiG[0](0,2); 85 | cyiG[0] = KiG[0](1,2); 86 | 87 | for (int level = 1; level < pyrLevelsUsed; ++ level) 88 | { 89 | wG[level] = w >> level; 90 | hG[level] = h >> level; 91 | 92 | fxG[level] = fxG[level-1] * 0.5; 93 | fyG[level] = fyG[level-1] * 0.5; 94 | cxG[level] = (cxG[0] + 0.5) / ((int)1<, 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #pragma once 27 | #include "util/settings.h" 28 | #include "util/NumType.h" 29 | 30 | namespace dso 31 | { 32 | extern int wG[PYR_LEVELS], hG[PYR_LEVELS]; 33 | extern float fxG[PYR_LEVELS], fyG[PYR_LEVELS], 34 | cxG[PYR_LEVELS], cyG[PYR_LEVELS]; 35 | 36 | extern float fxiG[PYR_LEVELS], fyiG[PYR_LEVELS], 37 | cxiG[PYR_LEVELS], cyiG[PYR_LEVELS]; 38 | 39 | extern Eigen::Matrix3f KG[PYR_LEVELS],KiG[PYR_LEVELS]; 40 | 41 | extern float wM3G; 42 | extern float hM3G; 43 | 44 | void setGlobalCalib(int w, int h, const Eigen::Matrix3f &K ); 45 | } 46 | -------------------------------------------------------------------------------- /03_Application/dso/src/util/settings.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | #pragma once 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | 33 | namespace dso 34 | { 35 | #define SOLVER_SVD (int)1 36 | #define SOLVER_ORTHOGONALIZE_SYSTEM (int)2 37 | #define SOLVER_ORTHOGONALIZE_POINTMARG (int)4 38 | #define SOLVER_ORTHOGONALIZE_FULL (int)8 39 | #define SOLVER_SVD_CUT7 (int)16 40 | #define SOLVER_REMOVE_POSEPRIOR (int)32 41 | #define SOLVER_USE_GN (int)64 42 | #define SOLVER_FIX_LAMBDA (int)128 43 | #define SOLVER_ORTHOGONALIZE_X (int)256 44 | #define SOLVER_MOMENTUM (int)512 45 | #define SOLVER_STEPMOMENTUM (int)1024 46 | #define SOLVER_ORTHOGONALIZE_X_LATER (int)2048 47 | 48 | 49 | // ============== PARAMETERS TO BE DECIDED ON COMPILE TIME ================= 50 | #define PYR_LEVELS 6 51 | extern int pyrLevelsUsed; 52 | 53 | // Added by ADAM: 54 | extern bool setting_useCameraPoses; 55 | extern bool setting_initKalmanFilter; 56 | extern bool setting_useHTTP; 57 | extern bool setting_useUDP; 58 | 59 | 60 | 61 | extern float setting_keyframesPerSecond; 62 | extern bool setting_realTimeMaxKF; 63 | extern float setting_maxShiftWeightT; 64 | extern float setting_maxShiftWeightR; 65 | extern float setting_maxShiftWeightRT; 66 | extern float setting_maxAffineWeight; 67 | extern float setting_kfGlobalWeight; 68 | 69 | 70 | 71 | extern float setting_idepthFixPrior; 72 | extern float setting_idepthFixPriorMargFac; 73 | extern float setting_initialRotPrior; 74 | extern float setting_initialTransPrior; 75 | extern float setting_initialAffBPrior; 76 | extern float setting_initialAffAPrior; 77 | extern float setting_initialCalibHessian; 78 | 79 | extern int setting_solverMode; 80 | extern double setting_solverModeDelta; 81 | 82 | 83 | extern float setting_minIdepthH_act; 84 | extern float setting_minIdepthH_marg; 85 | 86 | 87 | 88 | extern float setting_maxIdepth; 89 | extern float setting_maxPixSearch; 90 | extern float setting_desiredImmatureDensity; // done 91 | extern float setting_desiredPointDensity; // done 92 | extern float setting_minPointsRemaining; 93 | extern float setting_maxLogAffFacInWindow; 94 | extern int setting_minFrames; 95 | extern int setting_maxFrames; 96 | extern int setting_minFrameAge; 97 | extern int setting_maxOptIterations; 98 | extern int setting_minOptIterations; 99 | extern float setting_thOptIterations; 100 | extern float setting_outlierTH; 101 | extern float setting_outlierTHSumComponent; 102 | 103 | 104 | 105 | extern int setting_pattern; 106 | extern float setting_margWeightFac; 107 | extern int setting_GNItsOnPointActivation; 108 | 109 | 110 | extern float setting_minTraceQuality; 111 | extern int setting_minTraceTestRadius; 112 | extern float setting_reTrackThreshold; 113 | 114 | 115 | extern int setting_minGoodActiveResForMarg; 116 | extern int setting_minGoodResForMarg; 117 | extern int setting_minInlierVotesForMarg; 118 | 119 | 120 | extern int setting_photometricCalibration; 121 | extern bool setting_useExposure; 122 | extern float setting_affineOptModeA; 123 | extern float setting_affineOptModeB; 124 | extern int setting_gammaWeightsPixelSelect; 125 | 126 | 127 | 128 | extern bool setting_forceAceptStep; 129 | 130 | 131 | 132 | extern float setting_huberTH; 133 | 134 | 135 | extern bool setting_logStuff; 136 | extern float benchmarkSetting_fxfyfac; 137 | extern int benchmarkSetting_width; 138 | extern int benchmarkSetting_height; 139 | extern float benchmark_varNoise; 140 | extern float benchmark_varBlurNoise; 141 | extern int benchmark_noiseGridsize; 142 | extern float benchmark_initializerSlackFactor; 143 | 144 | extern float setting_frameEnergyTHConstWeight; 145 | extern float setting_frameEnergyTHN; 146 | 147 | extern float setting_frameEnergyTHFacMedian; 148 | extern float setting_overallEnergyTHWeight; 149 | extern float setting_coarseCutoffTH; 150 | 151 | extern float setting_minGradHistCut; 152 | extern float setting_minGradHistAdd; 153 | extern float setting_gradDownweightPerLevel; 154 | extern bool setting_selectDirectionDistribution; 155 | 156 | 157 | 158 | extern float setting_trace_stepsize; 159 | extern int setting_trace_GNIterations; 160 | extern float setting_trace_GNThreshold; 161 | extern float setting_trace_extraSlackOnTH; 162 | extern float setting_trace_slackInterval; 163 | extern float setting_trace_minImprovementFactor; 164 | 165 | 166 | extern bool setting_render_displayCoarseTrackingFull; 167 | extern bool setting_render_renderWindowFrames; 168 | extern bool setting_render_plotTrackingFull; 169 | extern bool setting_render_display3D; 170 | extern bool setting_render_displayResidual; 171 | extern bool setting_render_displayVideo; 172 | extern bool setting_render_displayDepth; 173 | 174 | extern bool setting_fullResetRequested; 175 | 176 | extern bool setting_debugout_runquiet; 177 | 178 | extern bool disableAllDisplay; 179 | extern bool disableReconfigure; 180 | 181 | 182 | extern bool setting_onlyLogKFPoses; 183 | 184 | 185 | 186 | 187 | extern bool debugSaveImages; 188 | 189 | 190 | extern int sparsityFactor; 191 | extern bool goStepByStep; 192 | extern bool plotStereoImages; 193 | extern bool multiThreading; 194 | 195 | extern float freeDebugParam1; 196 | extern float freeDebugParam2; 197 | extern float freeDebugParam3; 198 | extern float freeDebugParam4; 199 | extern float freeDebugParam5; 200 | 201 | 202 | void handleKey(char k); 203 | 204 | 205 | 206 | 207 | extern int staticPattern[10][40][2]; 208 | extern int staticPatternNum[10]; 209 | extern int staticPatternPadding[10]; 210 | 211 | 212 | //#define patternNum staticPatternNum[setting_pattern] 213 | //#define patternP staticPattern[setting_pattern] 214 | //#define patternPadding staticPatternPadding[setting_pattern] 215 | 216 | // 217 | #define patternNum 8 218 | #define patternP staticPattern[8] 219 | #define patternPadding 2 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | } 234 | -------------------------------------------------------------------------------- /03_Application/dso_ros/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | 32 | -------------------------------------------------------------------------------- /03_Application/dso_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | project(dso_ros) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | roscpp 7 | sensor_msgs 8 | cv_bridge 9 | ) 10 | 11 | set(DSO_PATH $ENV{DSO_PATH}) 12 | message("\n\n---- LOOKING FOR DSO at\n\"${DSO_PATH}\"") 13 | 14 | 15 | find_package(Pangolin 0.2 REQUIRED) 16 | message("\n\n---- FOUND Pangolin Headers at\n\"${Pangolin_INCLUDE_DIRS}\"") 17 | message("---- FOUND Pangolin Libs at\n\"${Pangolin_LIBRARIES}\"") 18 | 19 | find_package(OpenCV REQUIRED) 20 | message("\n\n---- FOUND OpenCV Headers at\n\"${OpenCV_INCLUDE_DIRS}\"") 21 | message("---- FOUND OpenCV Libs at\n\"${OpenCV_LIBS}\"") 22 | 23 | find_package(Eigen3 REQUIRED) 24 | find_package(Boost COMPONENTS system thread) 25 | find_library(DSO_LIBRARY dso ${DSO_PATH}/build/lib) 26 | 27 | catkin_package( 28 | CATKIN_DEPENDS 29 | geometry_msgs 30 | roscpp 31 | sensor_msgs 32 | cv_bridge 33 | ) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | set(CMAKE_CXX_FLAGS 40 | "${SSE_FLAGS} -O2 -g -std=c++0x -march=native -fno-omit-frame-pointer" 41 | ) 42 | 43 | include_directories( 44 | ${PROJECT_SOURCE_DIR}/src 45 | ${DSO_PATH}/src 46 | ${DSO_PATH}/thirdparty/Sophus 47 | ${Pangolin_INCLUDE_DIRS} 48 | ${EIGEN3_INCLUDE_DIR} 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | 52 | ## Declare a C++ executable 53 | add_executable(dso_live src/main.cpp) 54 | 55 | target_link_libraries(dso_live 56 | ${DSO_LIBRARY} 57 | ${Pangolin_LIBRARIES} 58 | ${OpenCV_LIBS} 59 | ${catkin_LIBRARIES}) 60 | -------------------------------------------------------------------------------- /03_Application/dso_ros/README.md: -------------------------------------------------------------------------------- 1 | # ROS Wrapper around DSO: Direct Sparse Odometry 2 | 3 | For more information see 4 | [https://vision.in.tum.de/dso](https://vision.in.tum.de/dso) 5 | 6 | This is meant as simple, minimal example of how to integrate DSO from a different project, and run it on real-time input data. 7 | It does not provide a full ROS interface (no reconfigure / pointcloud output / pose output). 8 | To access computed information in real-time, I recommend to implement your own Output3DWrapper; see the DSO code. 9 | 10 | 11 | ### Related Papers 12 | 13 | * **Direct Sparse Odometry**, *J. Engel, V. Koltun, D. Cremers*, In arXiv:1607.02565, 2016 14 | 15 | * **A Photometrically Calibrated Benchmark For Monocular Visual Odometry**, *J. Engel, V. Usenko, D. Cremers*, In arXiv:1607.02555, 2016 16 | 17 | 18 | 19 | # 1. Installation 20 | 21 | 1. Install DSO. We need DSO to be compiled with OpenCV (to read the vignette image), and with Pangolin (for 3D visualization). 22 | 2. run 23 | 24 | export DSO_PATH=[PATH_TO_DSO]/dso 25 | rosmake 26 | 27 | 28 | 29 | # 3 Usage 30 | everything as described in the DSO project - only this is for real-time camera input. 31 | 32 | 33 | rosrun dso_ros dso_live image:=image_raw \ 34 | calib=XXXXX/camera.txt \ 35 | gamma=XXXXX/pcalib.txt \ 36 | vignette=XXXXX/vignette.png \ 37 | 38 | 39 | ## 3.1 Accessing Data. 40 | see the DSO Readme. As of now, there is no default ROS-based `Output3DWrapper` - you will have to write your own. 41 | 42 | 43 | 44 | 45 | # 4 Dependencies 46 | 47 | ## 4.1 Pangolin 48 | removing 49 | 50 | fullSystem->outputWrapper = new IOWrap::PangolinDSOViewer( 51 | (int)undistorter->getSize()[0], 52 | (int)undistorter->getSize()[1]); 53 | 54 | will allow you to use DSO compiled without Pangolin. However, then there is no 3D visualization. 55 | You can also implement your own Output3DWrapper to fit your needs. 56 | 57 | ## 4.2 OpenCV 58 | you can use DSO compiled without OpenCV. 59 | In that case, the vignette image will not be read, and no photometric calibration can be used. Also, there will not be any image visualizations / image saving. 60 | You can also implement your own version of ImageRW.h / ImageDisplay.h, instead of the dummies. 61 | 62 | 63 | ### 5 License 64 | This ROS wrapper around DSO is licensed under the GNU General Public License 65 | Version 3 (GPLv3). 66 | For commercial purposes, we also offer a professional version, see 67 | [http://vision.in.tum.de/dso](http://vision.in.tum.de/dso) for details. 68 | -------------------------------------------------------------------------------- /03_Application/dso_ros/dsoCameraZED_720P.txt: -------------------------------------------------------------------------------- 1 | 700.982 700.982 692.124 376.428 0.0 2 | 1280 720 3 | crop 4 | 1280 720 5 | -------------------------------------------------------------------------------- /03_Application/dso_ros/dsoCameraZED_FullHD.txt: -------------------------------------------------------------------------------- 1 | 1401.96 1401.96 1067.25 575.856 0.0 2 | 1920 1080 3 | crop 4 | 768 432 5 | -------------------------------------------------------------------------------- /03_Application/dso_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dso_ros 4 | 1.0.0 5 | 6 | 7 | ROS Wrapper DSO. 8 | 9 | 10 | 11 | Jakob Engel 12 | see http://vision.in.tum.de/dso 13 | 14 | Jakob Engel 15 | 16 | 17 | http://vision.in.tum.de/dso 18 | 19 | catkin 20 | geometry_msgs 21 | roscpp 22 | sensor_msgs 23 | cv_bridge 24 | 25 | geometry_msgs 26 | roscpp 27 | sensor_msgs 28 | cv_bridge 29 | 30 | 31 | -------------------------------------------------------------------------------- /03_Application/dso_ros/runDSOOffline.sh: -------------------------------------------------------------------------------- 1 | dso/build/bin/dso_dataset \ 2 | files=Downloads/sequence_22/images.zip \ 3 | calib=Downloads/sequence_22/camera.txt \ 4 | gamma=Downloads/sequence_22/pcalib.txt \ 5 | vignette=Downloads/sequence_22/vignette.png \ 6 | preset=0 \ 7 | mode=0 8 | -------------------------------------------------------------------------------- /03_Application/dso_ros/runDSOOnline.sh: -------------------------------------------------------------------------------- 1 | rosrun dso_ros dso_live topic=zed/left/image_rect_color \ 2 | calib=dso/dsoCameraZED_720P.txt \ 3 | sampleoutput=1 \ 4 | quiet=1 \ 5 | udp=1 \ 6 | http=1 #\ 7 | #gamma=XXXXX/pcalib.txt \ 8 | #vignette=XXXXX/vignette.png \ 9 | 10 | -------------------------------------------------------------------------------- /03_Application/dso_ros/runDemo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #Shebang 3 | 4 | #Start roscore and ZED wrapper 5 | #----------------------------- 6 | #roscore& 7 | roslaunch zed_wrapper zed.launch& 8 | Process1=$! 9 | sleep 10s 10 | 11 | # Start DSO 12 | #----------------------------- 13 | source runDSOOnline.sh & 14 | Process2=$! 15 | 16 | wait $Process1 $Process2 17 | -------------------------------------------------------------------------------- /03_Application/dso_ros/src/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of DSO. 3 | * 4 | * Copyright 2016 Technical University of Munich and Intel. 5 | * Developed by Jakob Engel , 6 | * for more information see . 7 | * If you use this code, please cite the respective publications as 8 | * listed on the above website. 9 | * 10 | * DSO is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * DSO is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with DSO. If not, see . 22 | */ 23 | 24 | 25 | 26 | 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "util/settings.h" 36 | #include "FullSystem/FullSystem.h" 37 | #include "util/Undistort.h" 38 | #include "IOWrapper/Pangolin/PangolinDSOViewer.h" 39 | #include "IOWrapper/OutputWrapper/SampleOutputWrapper.h" 40 | 41 | #include "util/UDPServer.h" 42 | 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include "cv_bridge/cv_bridge.h" 50 | 51 | 52 | std::string calib = ""; 53 | std::string vignetteFile = ""; 54 | std::string gammaFile = ""; 55 | std::string rosTopic = "zed/left/image_rect_color"; 56 | bool useSampleOutput=false; 57 | 58 | using namespace dso; 59 | 60 | void parseArgument(char* arg) 61 | { 62 | int option; 63 | char buf[1000]; 64 | 65 | if(1==sscanf(arg,"sampleoutput=%d",&option)) 66 | { 67 | if(option==1) 68 | { 69 | useSampleOutput = true; 70 | printf("USING SAMPLE OUTPUT WRAPPER!\n"); 71 | } 72 | return; 73 | } 74 | 75 | if(1==sscanf(arg,"quiet=%d",&option)) 76 | { 77 | if(option==1) 78 | { 79 | setting_debugout_runquiet = true; 80 | printf("QUIET MODE, I'll shut up!\n"); 81 | } 82 | return; 83 | } 84 | 85 | 86 | if(1==sscanf(arg,"nolog=%d",&option)) 87 | { 88 | if(option==1) 89 | { 90 | setting_logStuff = false; 91 | printf("DISABLE LOGGING!\n"); 92 | } 93 | return; 94 | } 95 | 96 | if(1==sscanf(arg,"nogui=%d",&option)) 97 | { 98 | if(option==1) 99 | { 100 | disableAllDisplay = true; 101 | printf("NO GUI!\n"); 102 | } 103 | return; 104 | } 105 | if(1==sscanf(arg,"nomt=%d",&option)) 106 | { 107 | if(option==1) 108 | { 109 | multiThreading = false; 110 | printf("NO MultiThreading!\n"); 111 | } 112 | return; 113 | } 114 | if(1==sscanf(arg,"calib=%s",buf)) 115 | { 116 | calib = buf; 117 | printf("loading calibration from %s!\n", calib.c_str()); 118 | return; 119 | } 120 | if(1==sscanf(arg,"vignette=%s",buf)) 121 | { 122 | vignetteFile = buf; 123 | printf("loading vignette from %s!\n", vignetteFile.c_str()); 124 | return; 125 | } 126 | 127 | if(1==sscanf(arg,"gamma=%s",buf)) 128 | { 129 | gammaFile = buf; 130 | printf("loading gammaCalib from %s!\n", gammaFile.c_str()); 131 | return; 132 | } 133 | 134 | if(1==sscanf(arg,"topic=%s",buf)) 135 | { 136 | rosTopic = buf; 137 | printf("ROS Topic set as %s!\n", rosTopic.c_str()); 138 | return; 139 | } 140 | /* 141 | Add feature to init kalmanFilter with GPS or Pose 142 | */ 143 | if(1==sscanf(arg, "initKalmanFilter=%d", &option)) 144 | { 145 | if(option==1) 146 | { 147 | setting_initKalmanFilter = true; 148 | printf("Will initialize KalmanFilter from poses or gps!\n"); 149 | } 150 | return; 151 | } 152 | /* 153 | Add feature to read in GPS measurements via UDP 154 | */ 155 | if(1==sscanf(arg, "udp=%d", &option)) 156 | { 157 | if(option==1) 158 | { 159 | setting_useUDP = true; 160 | printf("Using UDP Server!\n"); 161 | } 162 | return; 163 | } 164 | 165 | /* 166 | Add feature to read in GPS measurements via UDP 167 | */ 168 | if(1==sscanf(arg, "http=%d", &option)) 169 | { 170 | if(option==1) 171 | { 172 | setting_useHTTP = true; 173 | printf("Using HTTP POST Requests!\n"); 174 | } 175 | return; 176 | } 177 | 178 | printf("could not parse argument \"%s\"!!\n", arg); 179 | } 180 | 181 | 182 | 183 | 184 | FullSystem* fullSystem = 0; 185 | Undistort* undistorter = 0; 186 | IOWrap::SampleOutputWrapper* sampleOutput = 0; 187 | UDPServer* ptrUDPServer = 0; 188 | int frameID = 0; 189 | 190 | // ADDED by Adam 191 | 192 | void vidCb(const sensor_msgs::ImageConstPtr img) 193 | { 194 | //printf("New vidCb call"); 195 | //printf("Height: %d, Width: %d, Stamp: %s", img->height, img->width, img->header.stamp); 196 | 197 | cv_bridge::CvImagePtr cv_ptr; 198 | try 199 | { 200 | cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); 201 | } 202 | catch (cv_bridge::Exception& e) 203 | { 204 | ROS_ERROR("cv_bridge exception: %s", e.what()); 205 | return; 206 | } 207 | 208 | assert(cv_ptr->image.type() == CV_8U); 209 | assert(cv_ptr->image.channels() == 1); 210 | 211 | //printf("ImageConstPtr->image.type() = %s", cv_ptr->image.type()); 212 | 213 | if(setting_fullResetRequested) 214 | { 215 | std::vector wraps = fullSystem->outputWrapper; 216 | delete fullSystem; 217 | for(IOWrap::Output3DWrapper* ow : wraps) ow->reset(); 218 | fullSystem = new FullSystem(); 219 | fullSystem->linearizeOperation=false; 220 | fullSystem->outputWrapper = wraps; 221 | 222 | if(setting_useUDP) 223 | { 224 | fullSystem->setUDPServer(*ptrUDPServer); 225 | } 226 | if(setting_useHTTP) 227 | { 228 | fullSystem->setHTTPPOSTRequest(sampleOutput->httpPOSTRequest); 229 | } 230 | 231 | 232 | if(undistorter->photometricUndist != 0) 233 | fullSystem->setGammaFunction(undistorter->photometricUndist->getG()); 234 | setting_fullResetRequested=false; 235 | } 236 | 237 | MinimalImageB minImg((int)cv_ptr->image.cols, (int)cv_ptr->image.rows,(unsigned char*)cv_ptr->image.data); 238 | ImageAndExposure* undistImg = undistorter->undistort(&minImg, 1,0, 1.0f); 239 | fullSystem->addActiveFrame(undistImg, frameID); 240 | frameID++; 241 | delete undistImg; 242 | 243 | 244 | } 245 | 246 | 247 | 248 | 249 | 250 | int main( int argc, char** argv ) 251 | { 252 | ros::init(argc, argv, "dso_live"); 253 | 254 | 255 | 256 | for(int i=1; igetSize()[0], 280 | (int)undistorter->getSize()[1], 281 | undistorter->getK().cast()); 282 | 283 | 284 | fullSystem = new FullSystem(); 285 | fullSystem->linearizeOperation=false; 286 | 287 | // ******** 288 | // UDP Server for GPS measurements 289 | // ******** 290 | 291 | //if(setting_useUDP) 292 | //{ 293 | // Create a server object to accept incoming client requests (i.e. the smartphone) 294 | 295 | boost::asio::io_service io_service; 296 | UDPServer udpServer(io_service); 297 | fullSystem->setUDPServer(udpServer); 298 | ptrUDPServer = &udpServer; 299 | 300 | //io_service.run(); 301 | std::thread udpThread([&io_service]() { io_service.run(); }); 302 | //} 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | if(!disableAllDisplay) 311 | fullSystem->outputWrapper.push_back(new IOWrap::PangolinDSOViewer( 312 | (int)undistorter->getSize()[0], 313 | (int)undistorter->getSize()[1])); 314 | 315 | 316 | 317 | if(useSampleOutput) 318 | { 319 | sampleOutput = new IOWrap::SampleOutputWrapper(); 320 | if(setting_useHTTP) 321 | { 322 | sampleOutput->httpPOSTRequest.setActive(); 323 | sampleOutput->httpPOSTRequest.addProject(); 324 | } 325 | else 326 | { 327 | sampleOutput->httpPOSTRequest.setInactive(); 328 | } 329 | fullSystem->setHTTPPOSTRequest(sampleOutput->httpPOSTRequest); 330 | 331 | fullSystem->outputWrapper.push_back(sampleOutput); 332 | } 333 | 334 | 335 | 336 | if(undistorter->photometricUndist != 0) 337 | fullSystem->setGammaFunction(undistorter->photometricUndist->getG()); 338 | 339 | ros::NodeHandle nh; 340 | ros::Subscriber imgSub = nh.subscribe(rosTopic, 1, &vidCb); 341 | 342 | ros::spin(); 343 | 344 | //if(setting_useUDP) 345 | //{ 346 | //udpThread.join(); 347 | //} 348 | 349 | for(IOWrap::Output3DWrapper* ow : fullSystem->outputWrapper) 350 | { 351 | ow->join(); 352 | delete ow; 353 | } 354 | 355 | delete undistorter; 356 | delete fullSystem; 357 | 358 | return 0; 359 | } 360 | 361 | -------------------------------------------------------------------------------- /03_Application/median_filter/median_filter.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Code written by Dominik Penk, (c) 2018 3 | (Used with permission) 4 | 5 | Usage: 6 | python median_filter.py [kernel Size has to be odd] 7 | ''' 8 | 9 | import sys 10 | import cv2 11 | import os 12 | 13 | if __name__ == "__main__": 14 | folder = sys.argv[1] 15 | out_folder = sys.argv[2] 16 | if not os.path.isdir(out_folder): 17 | os.mkdir(out_folder) 18 | files = os.listdir(folder) 19 | filter_size = 3 if len(sys.argv) == 3 else int(sys.argv[3]) 20 | for f in files: 21 | path = os.path.join(folder, f) 22 | img = cv2.imread(path) 23 | img = cv2.medianBlur(img, filter_size) 24 | cv2.imwrite(os.path.join(out_folder, f), img) 25 | -------------------------------------------------------------------------------- /03_Application/video2bag/img2bag.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Source: https://answers.ros.org/question/11537/creating-a-bag-file-out-of-a-image-sequence/ 3 | (plus some additions from the comment section) 4 | ''' 5 | 6 | import time, sys, os 7 | from ros import rosbag 8 | import roslib 9 | import rospy 10 | roslib.load_manifest('sensor_msgs') 11 | from sensor_msgs.msg import Image 12 | 13 | import ImageFile 14 | 15 | def CompSortFileNamesNr(f): 16 | g = os.path.splitext(os.path.split(f)[1])[0] 17 | numbertext = ''.join(c for c in g if c.isdigit()) 18 | return int(numbertext) 19 | 20 | def GetFilesFromDir(dir): 21 | '''Generates a list of files from the directory''' 22 | print( "Searching directory %s" % dir ) 23 | all = [] 24 | left_files = [] 25 | right_files = [] 26 | if os.path.exists(dir): 27 | for path, names, files in os.walk(dir): 28 | for f in sorted(files, key=CompSortFileNamesNr): 29 | if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg', '.ppm']: 30 | if 'left' in f or 'left' in path: 31 | left_files.append( os.path.join( path, f ) ) 32 | elif 'right' in f or 'right' in path: 33 | right_files.append( os.path.join( path, f ) ) 34 | all.append( os.path.join( path, f ) ) 35 | return all, left_files, right_files 36 | 37 | def CreateStereoBag(left_imgs, right_imgs, bagname): 38 | '''Creates a bag file containing stereo image pairs''' 39 | bag =rosbag.Bag(bagname, 'w') 40 | 41 | try: 42 | for i in range(len(left_imgs)): 43 | print("Adding %s" % left_imgs[i]) 44 | fp_left = open( left_imgs[i], "r" ) 45 | p_left = ImageFile.Parser() 46 | 47 | while 1: 48 | s = fp_left.read(1024) 49 | if not s: 50 | break 51 | p_left.feed(s) 52 | 53 | im_left = p_left.close() 54 | 55 | fp_right = open( right_imgs[i], "r" ) 56 | print("Adding %s" % right_imgs[i]) 57 | p_right = ImageFile.Parser() 58 | 59 | while 1: 60 | s = fp_right.read(1024) 61 | if not s: 62 | break 63 | p_right.feed(s) 64 | 65 | im_right = p_right.close() 66 | 67 | Stamp = roslib.rostime.Time.from_sec(time.time()) 68 | 69 | Img_left = Image() 70 | Img_left.header.stamp = Stamp 71 | Img_left.width = im_left.size[0] 72 | Img_left.height = im_left.size[1] 73 | Img_left.encoding = "rgb8" 74 | Img_left.header.frame_id = "camera/left" 75 | Img_left_data = [pix for pixdata in im_left.getdata() for pix in pixdata] 76 | Img_left.data = Img_left_data 77 | Img_right = Image() 78 | Img_right.header.stamp = Stamp 79 | Img_right.width = im_right.size[0] 80 | Img_right.height = im_right.size[1] 81 | Img_right.encoding = "rgb8" 82 | Img_right.header.frame_id = "camera/right" 83 | Img_right_data = [pix for pixdata in im_right.getdata() for pix in pixdata] 84 | Img_right.data = Img_right_data 85 | 86 | bag.write('camera/left/image_raw', Img_left, Stamp) 87 | bag.write('camera/right/image_raw', Img_right, Stamp) 88 | finally: 89 | bag.close() 90 | 91 | def CreateMonoBag(imgs,bagname): 92 | '''Creates a bag file with camera images''' 93 | bag =rosbag.Bag(bagname, 'w') 94 | 95 | try: 96 | for i in range(len(imgs)): 97 | print("Adding %s" % imgs[i]) 98 | fp = open( imgs[i], "r" ) 99 | p = ImageFile.Parser() 100 | 101 | while 1: 102 | s = fp.read(1024) 103 | if not s: 104 | break 105 | p.feed(s) 106 | 107 | im = p.close() 108 | 109 | #Stamp = rospy.rostime.Time.from_sec(time.time()) 110 | Stamp = rospy.Time.from_sec(time.time()) 111 | Img = Image() 112 | Img.header.stamp = Stamp 113 | Img.width = im.size[0] 114 | Img.height = im.size[1] 115 | #Img.encoding = "rgb8" 116 | Img.encoding = "mono8" 117 | Img.header.frame_id = "camera" 118 | Img_data = [pix for pixdata in im.getdata() for pix in pixdata] 119 | #Img_data = [pix for pixdata in [im.getdata()] for pix in pixdata] 120 | Img.data = Img_data 121 | #Change "rgb8" encoding to "mono8", and "[pix for pixdata in im.getdata() for pix in pixdata]" to "[pix for pixdata in [im.getdata()] for pix in pixdata]" 122 | 123 | bag.write('camera/image_raw', Img, Stamp) 124 | finally: 125 | bag.close() 126 | 127 | 128 | def CreateBag(args): 129 | '''Creates the actual bag file by successively adding images''' 130 | all_imgs, left_imgs, right_imgs = GetFilesFromDir(args[0]) 131 | if len(all_imgs) <= 0: 132 | print("No images found in %s" % args[0]) 133 | exit() 134 | 135 | if len(left_imgs) > 0 and len(right_imgs) > 0: 136 | # create bagfile with stereo camera image pairs 137 | CreateStereoBag(left_imgs, right_imgs, args[1]) 138 | else: 139 | # create bagfile with mono camera image stream 140 | CreateMonoBag(all_imgs, args[1]) 141 | 142 | if __name__ == "__main__": 143 | if len( sys.argv ) == 3: 144 | CreateBag( sys.argv[1:]) 145 | else: 146 | print( "Usage: img2bag imagedir bagfilename") 147 | --------------------------------------------------------------------------------