From 11c230af3fa7f32f7481ecbde25223e3fbac5540 Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 20 Oct 2021 15:39:51 +0200 Subject: [PATCH] change the reprojection --- GPU_version/vio/CMakeLists.txt | 6 ++-- GPU_version/vio/Doxyfile | 4 +-- .../vio/include/vio/bundle_adjustment.h | 4 +-- GPU_version/vio/include/vio/config.h | 14 ++++---- GPU_version/vio/include/vio/feature.h | 12 +++---- .../vio/include/vio/feature_alignment.h | 12 +++---- .../vio/include/vio/feature_detection.h | 12 +++---- GPU_version/vio/include/vio/frame.h | 14 ++++---- .../vio/include/vio/frame_handler_base.h | 14 ++++---- .../vio/include/vio/frame_handler_mono.h | 14 ++++---- GPU_version/vio/include/vio/global.h | 16 ++++----- .../vio/include/vio/global_optimizer.h | 12 +++---- GPU_version/vio/include/vio/initialization.h | 12 +++---- GPU_version/vio/include/vio/map.h | 14 ++++---- GPU_version/vio/include/vio/matcher.h | 12 +++---- GPU_version/vio/include/vio/point.h | 14 ++++---- GPU_version/vio/include/vio/pose_optimizer.h | 12 +++---- GPU_version/vio/include/vio/reprojector.h | 12 +++---- .../vio/include/vio/sparse_img_align.h | 12 +++---- .../vio/include/vio/sparse_img_align_gpu.h | 6 ++-- GPU_version/vio/include/vio/visualizer.h | 12 +++---- GPU_version/vio/launch/test.launch | 6 ++-- GPU_version/vio/launch/unity.launch | 6 ++-- GPU_version/vio/src/bundle_adjustment.cpp | 8 ++--- GPU_version/vio/src/config.cpp | 8 ++--- GPU_version/vio/src/feature_alignment.cpp | 6 ++-- GPU_version/vio/src/feature_detection.cpp | 6 ++-- GPU_version/vio/src/frame.cpp | 6 ++-- GPU_version/vio/src/frame_handler_base.cpp | 6 ++-- GPU_version/vio/src/frame_handler_mono.cpp | 6 ++-- GPU_version/vio/src/global_optimizer.cpp | 6 ++-- GPU_version/vio/src/initialization.cpp | 6 ++-- GPU_version/vio/src/map.cpp | 23 +++++++++---- GPU_version/vio/src/matcher.cpp | 6 ++-- GPU_version/vio/src/point.cpp | 6 ++-- GPU_version/vio/src/pose_optimizer.cpp | 8 ++--- GPU_version/vio/src/reprojector.cpp | 33 ++++++++++--------- GPU_version/vio/src/sparse_img_align.cpp | 6 ++-- GPU_version/vio/src/sparse_img_align_gpu.cpp | 12 +++++-- GPU_version/vio/src/visualizer.cpp | 6 ++-- GPU_version/vio/src/vo_node.cpp | 2 +- 41 files changed, 213 insertions(+), 199 deletions(-) diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index 5398a7e..fc2c523 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/GPU_version/vio/CMakeLists.txt @@ -12,8 +12,8 @@ set(CMAKE_C_STANDARD 17) set(CMAKE_CXX_STANDARD 17) # Set definitions -ADD_DEFINITIONS(-DSVO_TRACE) -ADD_DEFINITIONS(-DSVO_USE_ROS) +ADD_DEFINITIONS(-DVIO_TRACE) +ADD_DEFINITIONS(-DVIO_USE_ROS) # Set build flags, set ARM_ARCHITECTURE environment variable on Odroid @@ -79,7 +79,7 @@ file(GLOB SRC CONFIGURE_DEPENDS "src/*.cpp" "include/sophus/*.cpp") -# Create svo library +# Create VIO library ADD_EXECUTABLE(${PROJECT_NAME} ${SRC}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS}) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) diff --git a/GPU_version/vio/Doxyfile b/GPU_version/vio/Doxyfile index b1b4256..263bda0 100755 --- a/GPU_version/vio/Doxyfile +++ b/GPU_version/vio/Doxyfile @@ -26,7 +26,7 @@ DOXYFILE_ENCODING = UTF-8 # identify the project. Note that if you do not use Doxywizard you need # to put quotes around the project name if it contains spaces. -PROJECT_NAME = "svo" +PROJECT_NAME = "VIO" # The PROJECT_NUMBER tag can be used to enter a project or revision number. # This could be handy for archiving the generated documentation or @@ -648,7 +648,7 @@ WARN_LOGFILE = # directories like "/usr/src/myproject". Separate the files or directories # with spaces. -INPUT = "src", "include/svo" +INPUT = "src", "include/VIO" # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is diff --git a/GPU_version/vio/include/vio/bundle_adjustment.h b/GPU_version/vio/include/vio/bundle_adjustment.h index e6a75d7..ba92f08 100644 --- a/GPU_version/vio/include/vio/bundle_adjustment.h +++ b/GPU_version/vio/include/vio/bundle_adjustment.h @@ -54,7 +54,7 @@ namespace vio { double& final_error); /// Global bundle adjustment. -/// Optimizes the whole map. Is currently not used in SVO. +/// Optimizes the whole map. Is currently not used in VIO. void globalBA(Map* map); /// Initialize g2o with solver type, optimization strategy and camera model. @@ -88,7 +88,7 @@ namespace vio { double huber_width, double weight = 1); -} // namespace svo +} // namespace VIO #endif //VIO_BUNDLE_ADJUSTMENT_H diff --git a/GPU_version/vio/include/vio/config.h b/GPU_version/vio/include/vio/config.h index 99ee152..939d88b 100644 --- a/GPU_version/vio/include/vio/config.h +++ b/GPU_version/vio/include/vio/config.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_CONFIG_H_ -#define SVO_CONFIG_H_ +#ifndef VIO_CONFIG_H_ +#define VIO_CONFIG_H_ #include #include @@ -25,7 +25,7 @@ namespace vio { using std::string; -/// Global configuration file of SVO. +/// Global configuration file of VIO. /// Implements the Singleton design pattern to allow global access and to ensure /// that only one instance exists. class Config @@ -132,4 +132,4 @@ private: } // namespace vio -#endif // SVO_CONFIG_H_ +#endif // VIO_CONFIG_H_ diff --git a/GPU_version/vio/include/vio/feature.h b/GPU_version/vio/include/vio/feature.h index 6af658a..75aaa13 100644 --- a/GPU_version/vio/include/vio/feature.h +++ b/GPU_version/vio/include/vio/feature.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_FEATURE_H_ -#define SVO_FEATURE_H_ +#ifndef VIO_FEATURE_H_ +#define VIO_FEATURE_H_ #include @@ -107,4 +107,4 @@ struct Feature } // namespace vio -#endif // SVO_FEATURE_H_ +#endif // VIO_FEATURE_H_ diff --git a/GPU_version/vio/include/vio/feature_alignment.h b/GPU_version/vio/include/vio/feature_alignment.h index 5befb9e..455d7fd 100644 --- a/GPU_version/vio/include/vio/feature_alignment.h +++ b/GPU_version/vio/include/vio/feature_alignment.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_FEATURE_ALIGNMENT_H_ -#define SVO_FEATURE_ALIGNMENT_H_ +#ifndef VIO_FEATURE_ALIGNMENT_H_ +#define VIO_FEATURE_ALIGNMENT_H_ #include @@ -60,4 +60,4 @@ bool align2D_NEON( } // namespace feature_alignment } // namespace vio -#endif // SVO_FEATURE_ALIGNMENT_H_ +#endif // VIO_FEATURE_ALIGNMENT_H_ diff --git a/GPU_version/vio/include/vio/feature_detection.h b/GPU_version/vio/include/vio/feature_detection.h index b98e8a1..311c48e 100644 --- a/GPU_version/vio/include/vio/feature_detection.h +++ b/GPU_version/vio/include/vio/feature_detection.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_FEATURE_DETECTION_H_ -#define SVO_FEATURE_DETECTION_H_ +#ifndef VIO_FEATURE_DETECTION_H_ +#define VIO_FEATURE_DETECTION_H_ #include #include @@ -110,4 +110,4 @@ public: } // namespace feature_detection } // namespace vio -#endif // SVO_FEATURE_DETECTION_H_ +#endif // VIO_FEATURE_DETECTION_H_ diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index 50cfccd..f7081eb 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_FRAME_H_ -#define SVO_FRAME_H_ +#ifndef VIO_FRAME_H_ +#define VIO_FRAME_H_ #include #include @@ -37,7 +37,7 @@ namespace vio { /// A frame saves the image, the associated features and the estimated pose. - class Frame : boost::noncopyable + class Frame { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -195,4 +195,4 @@ namespace vio { } // namespace vio -#endif // SVO_FRAME_H_ +#endif // VIO_FRAME_H_ diff --git a/GPU_version/vio/include/vio/frame_handler_base.h b/GPU_version/vio/include/vio/frame_handler_base.h index 8a2fe56..7ae6960 100644 --- a/GPU_version/vio/include/vio/frame_handler_base.h +++ b/GPU_version/vio/include/vio/frame_handler_base.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_FRAME_HANDLER_BASE_H_ -#define SVO_FRAME_HANDLER_BASE_H_ +#ifndef VIO_FRAME_HANDLER_BASE_H_ +#define VIO_FRAME_HANDLER_BASE_H_ #include #include @@ -34,7 +34,7 @@ namespace vio { /// Base class for various VO pipelines. Manages the map and the state machine. -class FrameHandlerBase : boost::noncopyable +class FrameHandlerBase { public: enum Stage { @@ -104,4 +104,4 @@ protected: } // namespace nslam -#endif // SVO_FRAME_HANDLER_BASE_H_ +#endif // VIO_FRAME_HANDLER_BASE_H_ diff --git a/GPU_version/vio/include/vio/frame_handler_mono.h b/GPU_version/vio/include/vio/frame_handler_mono.h index 0b19532..dfab8e4 100644 --- a/GPU_version/vio/include/vio/frame_handler_mono.h +++ b/GPU_version/vio/include/vio/frame_handler_mono.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_FRAME_HANDLER_H_ -#define SVO_FRAME_HANDLER_H_ +#ifndef VIO_FRAME_HANDLER_H_ +#define VIO_FRAME_HANDLER_H_ #include #include @@ -28,7 +28,7 @@ namespace vio { -/// Monocular Visual Odometry Pipeline as described in the SVO paper. +/// Monocular Visual Odometry Pipeline as described in the VIO paper. class FrameHandlerMono : public FrameHandlerBase { public: @@ -86,4 +86,4 @@ protected: } // namespace vio -#endif // SVO_FRAME_HANDLER_H_ +#endif // VIO_FRAME_HANDLER_H_ diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index 559fb0a..3e6c059 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_GLOBAL_H_ -#define SVO_GLOBAL_H_ +#ifndef VIO_GLOBAL_H_ +#define VIO_GLOBAL_H_ #include #include @@ -30,8 +30,8 @@ #include #include #include -#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit -#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED +#ifndef RPG_VIO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit +#define RPG_VIO_VIKIT_IS_VECTOR_SPECIALIZED EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) #endif @@ -143,4 +143,4 @@ namespace vio typedef std::shared_ptr FramePtr; } // namespace vio -#endif // SVO_GLOBAL_H_ +#endif // VIO_GLOBAL_H_ diff --git a/GPU_version/vio/include/vio/global_optimizer.h b/GPU_version/vio/include/vio/global_optimizer.h index ad48c6b..33f9648 100644 --- a/GPU_version/vio/include/vio/global_optimizer.h +++ b/GPU_version/vio/include/vio/global_optimizer.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_DEPTH_FILTER_H_ -#define SVO_DEPTH_FILTER_H_ +#ifndef VIO_DEPTH_FILTER_H_ +#define VIO_DEPTH_FILTER_H_ #include #include @@ -106,4 +106,4 @@ protected: } // namespace vio -#endif // SVO_DEPTH_FILTER_H_ +#endif // VIO_DEPTH_FILTER_H_ diff --git a/GPU_version/vio/include/vio/initialization.h b/GPU_version/vio/include/vio/initialization.h index 2b05789..b97ab92 100644 --- a/GPU_version/vio/include/vio/initialization.h +++ b/GPU_version/vio/include/vio/initialization.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_INITIALIZATION_H -#define SVO_INITIALIZATION_H +#ifndef VIO_INITIALIZATION_H +#define VIO_INITIALIZATION_H #include #include @@ -154,4 +154,4 @@ void trackKlt( } // namespace initialization } // namespace vio -#endif // SVO_INITIALIZATION_H +#endif // VIO_INITIALIZATION_H diff --git a/GPU_version/vio/include/vio/map.h b/GPU_version/vio/include/vio/map.h index afa738b..8ea4722 100644 --- a/GPU_version/vio/include/vio/map.h +++ b/GPU_version/vio/include/vio/map.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_MAP_H_ -#define SVO_MAP_H_ +#ifndef VIO_MAP_H_ +#define VIO_MAP_H_ #include #include @@ -28,7 +28,7 @@ namespace vio { /// Map object which saves all keyframes which are in a map. -class Map : boost::noncopyable +class Map { public: list< FramePtr > keyframes_; //!< List of keyframes in the map. @@ -100,4 +100,4 @@ void pointValidation(std::shared_ptr point, int id); } // namespace map_debug } // namespace vio -#endif // SVO_MAP_H_ +#endif // VIO_MAP_H_ diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h index ef4df32..cb67f05 100644 --- a/GPU_version/vio/include/vio/matcher.h +++ b/GPU_version/vio/include/vio/matcher.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_MATCHER_H_ -#define SVO_MATCHER_H_ +#ifndef VIO_MATCHER_H_ +#define VIO_MATCHER_H_ #include @@ -146,4 +146,4 @@ public: } // namespace vio -#endif // SVO_MATCHER_H_ +#endif // VIO_MATCHER_H_ diff --git a/GPU_version/vio/include/vio/point.h b/GPU_version/vio/include/vio/point.h index 08021cb..dec8c75 100644 --- a/GPU_version/vio/include/vio/point.h +++ b/GPU_version/vio/include/vio/point.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_POINT_H_ -#define SVO_POINT_H_ +#ifndef VIO_POINT_H_ +#define VIO_POINT_H_ #include #include @@ -32,7 +32,7 @@ class Feature; typedef Matrix Matrix23d; /// A 3D point on the surface of the scene. -class Point : boost::noncopyable +class Point { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -136,4 +136,4 @@ public: } // namespace vio -#endif // SVO_POINT_H_ +#endif // VIO_POINT_H_ diff --git a/GPU_version/vio/include/vio/pose_optimizer.h b/GPU_version/vio/include/vio/pose_optimizer.h index b28e336..9ea1d68 100644 --- a/GPU_version/vio/include/vio/pose_optimizer.h +++ b/GPU_version/vio/include/vio/pose_optimizer.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_POSE_OPTIMIZER_H_ -#define SVO_POSE_OPTIMIZER_H_ +#ifndef VIO_POSE_OPTIMIZER_H_ +#define VIO_POSE_OPTIMIZER_H_ #include #include @@ -48,4 +48,4 @@ void optimizeGaussNewton( } // namespace pose_optimizer } // namespace vio -#endif // SVO_POSE_OPTIMIZER_H_ +#endif // VIO_POSE_OPTIMIZER_H_ diff --git a/GPU_version/vio/include/vio/reprojector.h b/GPU_version/vio/include/vio/reprojector.h index f58d42e..7295db9 100644 --- a/GPU_version/vio/include/vio/reprojector.h +++ b/GPU_version/vio/include/vio/reprojector.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_REPROJECTION_H_ -#define SVO_REPROJECTION_H_ +#ifndef VIO_REPROJECTION_H_ +#define VIO_REPROJECTION_H_ #include #include @@ -101,4 +101,4 @@ private: } // namespace vio -#endif // SVO_REPROJECTION_H_ +#endif // VIO_REPROJECTION_H_ diff --git a/GPU_version/vio/include/vio/sparse_img_align.h b/GPU_version/vio/include/vio/sparse_img_align.h index 2dbce7a..c4fd8fa 100644 --- a/GPU_version/vio/include/vio/sparse_img_align.h +++ b/GPU_version/vio/include/vio/sparse_img_align.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_SPARSE_IMG_ALIGN_H_ -#define SVO_SPARSE_IMG_ALIGN_H_ +#ifndef VIO_SPARSE_IMG_ALIGN_H_ +#define VIO_SPARSE_IMG_ALIGN_H_ #include #include @@ -82,4 +82,4 @@ protected: } // namespace vio -#endif // SVO_SPARSE_IMG_ALIGN_H_ +#endif // VIO_SPARSE_IMG_ALIGN_H_ diff --git a/GPU_version/vio/include/vio/sparse_img_align_gpu.h b/GPU_version/vio/include/vio/sparse_img_align_gpu.h index 196687b..51d4eac 100644 --- a/GPU_version/vio/include/vio/sparse_img_align_gpu.h +++ b/GPU_version/vio/include/vio/sparse_img_align_gpu.h @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/include/vio/visualizer.h b/GPU_version/vio/include/vio/visualizer.h index 688ebcb..0b68e01 100644 --- a/GPU_version/vio/include/vio/visualizer.h +++ b/GPU_version/vio/include/vio/visualizer.h @@ -1,21 +1,21 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifndef SVO_VISUALIZER_H_ -#define SVO_VISUALIZER_H_ +#ifndef VIO_VISUALIZER_H_ +#define VIO_VISUALIZER_H_ #include #include @@ -59,4 +59,4 @@ public: } // end namespace vio -#endif /* SVO_VISUALIZER_H_ */ +#endif /* VIO_VISUALIZER_H_ */ diff --git a/GPU_version/vio/launch/test.launch b/GPU_version/vio/launch/test.launch index 4bcd965..b71be7f 100644 --- a/GPU_version/vio/launch/test.launch +++ b/GPU_version/vio/launch/test.launch @@ -1,16 +1,16 @@ - + - + - + diff --git a/GPU_version/vio/launch/unity.launch b/GPU_version/vio/launch/unity.launch index ea34997..e19053c 100644 --- a/GPU_version/vio/launch/unity.launch +++ b/GPU_version/vio/launch/unity.launch @@ -1,16 +1,16 @@ - + - + - + diff --git a/GPU_version/vio/src/bundle_adjustment.cpp b/GPU_version/vio/src/bundle_adjustment.cpp index 519587c..8d47f28 100644 --- a/GPU_version/vio/src/bundle_adjustment.cpp +++ b/GPU_version/vio/src/bundle_adjustment.cpp @@ -5,16 +5,16 @@ #include "vio/bundle_adjustment.h" -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // @@ -436,5 +436,5 @@ namespace vio { return e; } -} // namespace svo +} // namespace VIO */ diff --git a/GPU_version/vio/src/config.cpp b/GPU_version/vio/src/config.cpp index f72851f..e271cff 100644 --- a/GPU_version/vio/src/config.cpp +++ b/GPU_version/vio/src/config.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // @@ -20,7 +20,7 @@ namespace vio { Config::Config() : - trace_name(vk::getParam("vio/trace_name", "svo")), + trace_name(vk::getParam("vio/trace_name", "VIO")), trace_dir(vk::getParam("vio/trace_dir", "/tmp")), n_pyr_levels(vk::getParam("vio/n_pyr_levels", 3)), use_imu(vk::getParam("vio/use_imu", false)), diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp index 9032b76..b69e8eb 100644 --- a/GPU_version/vio/src/feature_alignment.cpp +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/feature_detection.cpp b/GPU_version/vio/src/feature_detection.cpp index 54d7284..cdcfee3 100644 --- a/GPU_version/vio/src/feature_detection.cpp +++ b/GPU_version/vio/src/feature_detection.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/frame.cpp b/GPU_version/vio/src/frame.cpp index d23cc45..97b373c 100644 --- a/GPU_version/vio/src/frame.cpp +++ b/GPU_version/vio/src/frame.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/frame_handler_base.cpp b/GPU_version/vio/src/frame_handler_base.cpp index 16d6df9..a0d8cc0 100644 --- a/GPU_version/vio/src/frame_handler_base.cpp +++ b/GPU_version/vio/src/frame_handler_base.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 6d9dab8..27a2c51 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp index d1d60c4..fe1bbcd 100644 --- a/GPU_version/vio/src/global_optimizer.cpp +++ b/GPU_version/vio/src/global_optimizer.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp index f25b8fe..9a39318 100644 --- a/GPU_version/vio/src/initialization.cpp +++ b/GPU_version/vio/src/initialization.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/map.cpp b/GPU_version/vio/src/map.cpp index 5df1ab7..8d3534e 100644 --- a/GPU_version/vio/src/map.cpp +++ b/GPU_version/vio/src/map.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // @@ -57,7 +57,7 @@ bool Map::safeDeleteFrame(FramePtr frame) if(found) return true; - //SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there."); + //VIO_ERROR_STREAM("Tried to delete Keyframe in map which was not there."); return false; } @@ -79,15 +79,24 @@ void Map::safeDeletePoint(std::shared_ptr pt) { boost::unique_lock lock(point_mut_); // Delete references to mappoints in all keyframes + if(pt == NULL)return; if(pt->obs_.size()>1){ for(auto&& ftr:pt->obs_){ - if(ftr->frame)ftr->frame->removeKeyPoint(ftr); + if(ftr->frame!=nullptr){ + if(ftr->frame->is_keyframe_){ + ftr->frame->removeKeyPoint(ftr); + } + } if(ftr->point!=nullptr)ftr->point.reset(); } }else{ - if(!pt->obs_.empty())if(pt->obs_.back()->point!=nullptr)pt->obs_.back()->point.reset(); + if(!pt->obs_.empty()){ + if(pt->obs_.front()->point!=nullptr){ + pt->obs_.back()->point.reset(); + } + } } - pt->obs_.clear(); + if(!pt->obs_.empty())pt->obs_.clear(); // Delete mappoint deletePoint(pt); } diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index f352c12..e30a5d8 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index d5b2597..de94489 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/pose_optimizer.cpp b/GPU_version/vio/src/pose_optimizer.cpp index 716972a..338f385 100644 --- a/GPU_version/vio/src/pose_optimizer.cpp +++ b/GPU_version/vio/src/pose_optimizer.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // @@ -50,7 +50,6 @@ void optimizeGaussNewton( for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/) { if((*it)->point == NULL) { - map.safeDeletePoint((*it)->point); it = frame->fts_.erase(it); continue; } @@ -133,7 +132,6 @@ void optimizeGaussNewton( for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/) { if((*it)->point == NULL) { - map.safeDeletePoint((*it)->point); it = frame->fts_.erase(it); continue; } diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 3fd31ae..1cf1892 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // @@ -103,14 +103,11 @@ namespace vio { last_frame->T_f_w_.se2().translation()).norm())); close_kfs.sort(boost::bind(&std::pair::second, _1) < boost::bind(&std::pair::second, _2)); - std::unique_ptr img_align=std::make_unique(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); - for (auto &&it_frame:_for(close_kfs)) { - img_align->run(it_frame.item.first, frame, log_); - } overlap_kfs.reserve(options_.max_n_kfs); + std::unique_ptr img_align=std::make_unique(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); std::vector added_keypoints; - int points_count=0; for (auto &&it_frame:_for(close_kfs)) { + int points_count=0; if (it_frame.index > options_.max_n_kfs)continue; overlap_kfs.push_back(pair(it_frame.item.first, 0)); list>::iterator it_ref=it_frame.item.first->fts_.begin(); @@ -142,17 +139,19 @@ namespace vio { (*it_ref)->f,frame->c2f(px)); if(pos.norm()==0. || pos.hasNaN() || pos.z() < 0.01)continue; // point in world frame - std::shared_ptr new_point=std::make_shared(it_frame.item.first->se3()*pos,(*it_ref)); - - if(!matcher_.findMatchDirect(*new_point, *frame, px))continue; + (*it_ref)->point=std::make_shared(it_frame.item.first->se3()*pos,(*it_ref)); + if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px)){ + (*it_ref)->point.reset(); + continue; + } frame->addFeature(std::make_shared(frame, - new_point, + (*it_ref)->point, px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor)); - new_point->addFrameRef(frame->fts_.back()); + (*it_ref)->point->addFrameRef(frame->fts_.back()); added_keypoints.push_back(match.trainIdx); - frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_; - frame->fts_.back()->point->type_=vio::Point::TYPE_UNKNOWN; + (*it_ref)->point->last_frame_overlap_id_=it_frame.item.first->id_; + (*it_ref)->point->type_=vio::Point::TYPE_UNKNOWN; grid_.cells.at(k)->push_back(Candidate( px)); overlap_kfs.back().second++; ++points_count; @@ -162,6 +161,8 @@ namespace vio { frame->addFeature(std::make_shared(frame, (*it_ref)->point, px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor)); + + (*it_ref)->point->addFrameRef(frame->fts_.back()); (*it_ref)->point->type_=vio::Point::TYPE_CANDIDATE; added_keypoints.push_back(match.trainIdx); grid_.cells.at(k)->push_back(Candidate( px)); @@ -171,7 +172,7 @@ namespace vio { } ++it_ref; } - //if(points_count>300)return; + if(points_count>10)img_align->run(it_frame.item.first, frame, log_); } /* std::cerr<<"here\n"; exit(0);*/ diff --git a/GPU_version/vio/src/sparse_img_align.cpp b/GPU_version/vio/src/sparse_img_align.cpp index 1a96465..93ae80f 100644 --- a/GPU_version/vio/src/sparse_img_align.cpp +++ b/GPU_version/vio/src/sparse_img_align.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index c13803f..33c9c98 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // @@ -55,6 +55,9 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log) } if(!feature_counter_) // more than 10 { +#if VIO_DEBUG + fprintf(log,"[%s] residual zero points \n",vio::time_in_HH_MM_SS_MMM().c_str()); +#endif free(featue_px); free(features); return 0; @@ -100,6 +103,9 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log) free(chi2_); free(featue_px); free(features); +#if VIO_DEBUG + fprintf(log,"[%s] residual out:%f %f %f \n",vio::time_in_HH_MM_SS_MMM().c_str(),pos[0].x,pos[0].y,pos[0].z); +#endif if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z) || fabs(pos[0].z-cur_pos[0].z)>M_PI_2)return 1; cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z); return 1; diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index 311df31..8418f0f 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -1,13 +1,13 @@ -// This file is part of SVO - Semi-direct Visual Odometry. +// This file is part of VIO - Semi-direct Visual Odometry. // // Copyright (C) 2014 Christian Forster // (Robotics and Perception Group, University of Zurich, Switzerland). // -// SVO is free software: you can redistribute it and/or modify it under the +// VIO is free software: you can redistribute it and/or modify it under the // terms of the GNU General Public License as published by the Free Software // Foundation, either version 3 of the License, or any later version. // -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// VIO is distributed in the hope that it will be useful, but WITHOUT ANY // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. // diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 9b27e4d..5e54276 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -216,6 +216,6 @@ int main(int argc, char **argv) { Q.callOne(ros::WallDuration(30,0.0)); } - printf("SVO terminated.\n"); + printf("VIO terminated.\n"); return 0; }