public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

change the reprojection

Browse Source
This commit is contained in:
gfuse
2021-10-20 15:39:51 +02:00
parent ef14e33bdf
commit 11c230af3f
41 changed files with 213 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+3 -3
View File
@@ -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)
GPU_version/vio/Doxyfile
+2 -2
View File
@@ -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
GPU_version/vio/include/vio/bundle_adjustment.h
+2 -2
View File
@@ -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
GPU_version/vio/include/vio/config.h
+7 -7
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_CONFIG_H_
#define SVO_CONFIG_H_
#ifndef VIO_CONFIG_H_
#define VIO_CONFIG_H_
#include <string>
#include <stdint.h>
@@ -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_
GPU_version/vio/include/vio/feature.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FEATURE_H_
#define SVO_FEATURE_H_
#ifndef VIO_FEATURE_H_
#define VIO_FEATURE_H_
#include <vio/frame.h>
@@ -107,4 +107,4 @@ struct Feature
} // namespace vio
#endif // SVO_FEATURE_H_
#endif // VIO_FEATURE_H_
GPU_version/vio/include/vio/feature_alignment.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FEATURE_ALIGNMENT_H_
#define SVO_FEATURE_ALIGNMENT_H_
#ifndef VIO_FEATURE_ALIGNMENT_H_
#define VIO_FEATURE_ALIGNMENT_H_
#include <vio/global.h>
@@ -60,4 +60,4 @@ bool align2D_NEON(
} // namespace feature_alignment
} // namespace vio
#endif // SVO_FEATURE_ALIGNMENT_H_
#endif // VIO_FEATURE_ALIGNMENT_H_
GPU_version/vio/include/vio/feature_detection.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FEATURE_DETECTION_H_
#define SVO_FEATURE_DETECTION_H_
#ifndef VIO_FEATURE_DETECTION_H_
#define VIO_FEATURE_DETECTION_H_
#include <vio/global.h>
#include <vio/frame.h>
@@ -110,4 +110,4 @@ public:
} // namespace feature_detection
} // namespace vio
#endif // SVO_FEATURE_DETECTION_H_
#endif // VIO_FEATURE_DETECTION_H_
GPU_version/vio/include/vio/frame.h
+7 -7
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FRAME_H_
#define SVO_FRAME_H_
#ifndef VIO_FRAME_H_
#define VIO_FRAME_H_
#include <sophus/se3.h>
#include <vio/math_utils.h>
@@ -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_
GPU_version/vio/include/vio/frame_handler_base.h
+7 -7
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#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 <queue>
#include <boost/noncopyable.hpp>
@@ -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_
GPU_version/vio/include/vio/frame_handler_mono.h
+7 -7
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_FRAME_HANDLER_H_
#define SVO_FRAME_HANDLER_H_
#ifndef VIO_FRAME_HANDLER_H_
#define VIO_FRAME_HANDLER_H_
#include <set>
#include <vio/abstract_camera.h>
@@ -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_
GPU_version/vio/include/vio/global.h
+8 -8
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_GLOBAL_H_
#define SVO_GLOBAL_H_
#ifndef VIO_GLOBAL_H_
#define VIO_GLOBAL_H_
#include <list>
#include <vector>
@@ -30,8 +30,8 @@
#include <sophus/se3.h>
#include <boost/shared_ptr.hpp>
#include<Eigen/StdVector>
#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<Frame> FramePtr;
} // namespace vio
#endif // SVO_GLOBAL_H_
#endif // VIO_GLOBAL_H_
GPU_version/vio/include/vio/global_optimizer.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_DEPTH_FILTER_H_
#define SVO_DEPTH_FILTER_H_
#ifndef VIO_DEPTH_FILTER_H_
#define VIO_DEPTH_FILTER_H_
#include <queue>
#include <boost/thread.hpp>
@@ -106,4 +106,4 @@ protected:
} // namespace vio
#endif // SVO_DEPTH_FILTER_H_
#endif // VIO_DEPTH_FILTER_H_
GPU_version/vio/include/vio/initialization.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_INITIALIZATION_H
#define SVO_INITIALIZATION_H
#ifndef VIO_INITIALIZATION_H
#define VIO_INITIALIZATION_H
#include <vio/global.h>
#include <vio/cl_class.h>
@@ -154,4 +154,4 @@ void trackKlt(
} // namespace initialization
} // namespace vio
#endif // SVO_INITIALIZATION_H
#endif // VIO_INITIALIZATION_H
GPU_version/vio/include/vio/map.h
+7 -7
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_MAP_H_
#define SVO_MAP_H_
#ifndef VIO_MAP_H_
#define VIO_MAP_H_
#include <queue>
#include <boost/noncopyable.hpp>
@@ -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> point, int id);
} // namespace map_debug
} // namespace vio
#endif // SVO_MAP_H_
#endif // VIO_MAP_H_
GPU_version/vio/include/vio/matcher.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_MATCHER_H_
#define SVO_MATCHER_H_
#ifndef VIO_MATCHER_H_
#define VIO_MATCHER_H_
#include <vio/global.h>
@@ -146,4 +146,4 @@ public:
} // namespace vio
#endif // SVO_MATCHER_H_
#endif // VIO_MATCHER_H_
GPU_version/vio/include/vio/point.h
+7 -7
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_POINT_H_
#define SVO_POINT_H_
#ifndef VIO_POINT_H_
#define VIO_POINT_H_
#include <boost/noncopyable.hpp>
#include <vio/global.h>
@@ -32,7 +32,7 @@ class Feature;
typedef Matrix<double, 2, 3> 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_
GPU_version/vio/include/vio/pose_optimizer.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_POSE_OPTIMIZER_H_
#define SVO_POSE_OPTIMIZER_H_
#ifndef VIO_POSE_OPTIMIZER_H_
#define VIO_POSE_OPTIMIZER_H_
#include <vio/global.h>
#include <vio/map.h>
@@ -48,4 +48,4 @@ void optimizeGaussNewton(
} // namespace pose_optimizer
} // namespace vio
#endif // SVO_POSE_OPTIMIZER_H_
#endif // VIO_POSE_OPTIMIZER_H_
GPU_version/vio/include/vio/reprojector.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_REPROJECTION_H_
#define SVO_REPROJECTION_H_
#ifndef VIO_REPROJECTION_H_
#define VIO_REPROJECTION_H_
#include <vio/global.h>
#include <vio/matcher.h>
@@ -101,4 +101,4 @@ private:
} // namespace vio
#endif // SVO_REPROJECTION_H_
#endif // VIO_REPROJECTION_H_
GPU_version/vio/include/vio/sparse_img_align.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#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 <vio/nlls_solver.h>
#include <vio/global.h>
@@ -82,4 +82,4 @@ protected:
} // namespace vio
#endif // SVO_SPARSE_IMG_ALIGN_H_
#endif // VIO_SPARSE_IMG_ALIGN_H_
GPU_version/vio/include/vio/sparse_img_align_gpu.h
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/include/vio/visualizer.h
+6 -6
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef SVO_VISUALIZER_H_
#define SVO_VISUALIZER_H_
#ifndef VIO_VISUALIZER_H_
#define VIO_VISUALIZER_H_
#include <queue>
#include <ros/ros.h>
@@ -59,4 +59,4 @@ public:
} // end namespace vio
#endif /* SVO_VISUALIZER_H_ */
#endif /* VIO_VISUALIZER_H_ */
GPU_version/vio/launch/test.launch
+3 -3
View File
@@ -1,16 +1,16 @@
<launch>
<node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
<node pkg="vio_VIO" type="vo" name="vio" clear_params="true" output="screen">
<!-- Camera topic to subscribe to -->
<param name="cam_topic" value="/camera/image_raw" type="str" />
<param name="imu_topic" value="/SensorNode/imu" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio_svo)/param/camera_atan.yaml" />
<rosparam file="$(find vio_VIO)/param/camera_atan.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
<rosparam file="$(find vio_VIO)/param/vo_fast.yaml" />
<!-- Initial camera orientation, make it point downwards -->
<param name="init_rx" value="3.14" />
GPU_version/vio/launch/unity.launch
+3 -3
View File
@@ -1,16 +1,16 @@
<launch>
<node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
<node pkg="vio_VIO" type="vo" name="vio" clear_params="true" output="screen">
<!-- Camera topic to subscribe to -->
<param name="cam_topic" value="/image_converter/output_image " type="str" />
<param name="imu_topic" value="/imu_unity/imu" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio_svo)/param/unity.yaml" />
<rosparam file="$(find vio_VIO)/param/unity.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
<rosparam file="$(find vio_VIO)/param/vo_fast.yaml" />
<!-- Initial camera orientation, make it point downwards -->
<param name="init_rx" value="0.00" />
GPU_version/vio/src/bundle_adjustment.cpp
+4 -4
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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
*/
GPU_version/vio/src/config.cpp
+4 -4
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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<string>("vio/trace_name", "svo")),
trace_name(vk::getParam<string>("vio/trace_name", "VIO")),
trace_dir(vk::getParam<string>("vio/trace_dir", "/tmp")),
n_pyr_levels(vk::getParam<int>("vio/n_pyr_levels", 3)),
use_imu(vk::getParam<bool>("vio/use_imu", false)),
GPU_version/vio/src/feature_alignment.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/feature_detection.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/frame.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/frame_handler_base.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/frame_handler_mono.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/global_optimizer.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/initialization.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/map.cpp
+16 -7
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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<Point> pt)
{
boost::unique_lock<boost::mutex> 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);
}
GPU_version/vio/src/matcher.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/point.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/pose_optimizer.cpp
+3 -5
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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;
}
GPU_version/vio/src/reprojector.cpp
+17 -16
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
std::unique_ptr<SparseImgAlignGpu> img_align=std::make_unique<SparseImgAlignGpu>(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<SparseImgAlignGpu> img_align=std::make_unique<SparseImgAlignGpu>(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
std::vector<int> 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<FramePtr, size_t>(it_frame.item.first, 0));
list<std::shared_ptr<Feature>>::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<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
if(!matcher_.findMatchDirect(*new_point, *frame, px))continue;
(*it_ref)->point=std::make_shared<Point>(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<Feature>(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<Feature>(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);*/
GPU_version/vio/src/sparse_img_align.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/sparse_img_align_gpu.cpp
+9 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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;
GPU_version/vio/src/visualizer.cpp
+3 -3
View File
@@ -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 <forster at ifi dot uzh dot ch>
// (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.
//
GPU_version/vio/src/vo_node.cpp
+1 -1
View File
@@ -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;
}