From cd6f0c5b97f2322571806c689602a6de62eafc6e Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 26 May 2021 10:50:20 +0200 Subject: [PATCH] debug GPU version --- GPU_version/svo/CMakeLists.txt | 18 +- .../svo/include/gpu_svo/abstract_camera.h | 77 ++++ GPU_version/svo/include/gpu_svo/aligned_mem.h | 127 ++++++ GPU_version/svo/include/gpu_svo/atan_camera.h | 91 ++++ GPU_version/svo/include/gpu_svo/config.h | 6 +- .../svo/include/gpu_svo/depth_filter.h | 6 +- GPU_version/svo/include/gpu_svo/for_it.hpp | 87 ++++ GPU_version/svo/include/gpu_svo/frame.h | 22 +- .../svo/include/gpu_svo/frame_handler_base.h | 16 +- .../svo/include/gpu_svo/frame_handler_mono.h | 5 +- GPU_version/svo/include/gpu_svo/global.h | 4 +- GPU_version/svo/include/gpu_svo/homography.h | 85 ++++ .../svo/include/gpu_svo/imu_integration.h | 84 +--- .../svo/include/gpu_svo/initialization.h | 1 + GPU_version/svo/include/gpu_svo/math_utils.h | 171 ++++++++ GPU_version/svo/include/gpu_svo/nlls_solver.h | 169 ++++++++ .../svo/include/gpu_svo/nlls_solver_impl.hpp | 323 ++++++++++++++ .../svo/include/gpu_svo/params_helper.h | 57 +++ GPU_version/svo/include/gpu_svo/patch_score.h | 225 ++++++++++ GPU_version/svo/include/gpu_svo/robust_cost.h | 156 +++++++ .../svo/include/gpu_svo/sparse_img_align.h | 7 +- .../svo/include/gpu_svo/svoVisionFactor.h | 27 +- .../include/gpu_svo/svoVisionFactorLandMark.h | 87 ++++ .../include/gpu_svo/svoVisionFactorSmall.h | 70 +++ GPU_version/svo/include/gpu_svo/timer.h | 81 ++++ GPU_version/svo/include/gpu_svo/vision.h | 77 ++++ GPU_version/svo/src/atan_camera.cpp | 82 ++++ GPU_version/svo/src/bundle_adjustment.cpp | 2 +- GPU_version/svo/src/config.cpp | 42 +- GPU_version/svo/src/depth_filter.cpp | 15 +- GPU_version/svo/src/feature_alignment.cpp | 4 +- GPU_version/svo/src/feature_detection.cpp | 2 +- GPU_version/svo/src/frame.cpp | 5 +- GPU_version/svo/src/frame_handler_base.cpp | 67 +-- GPU_version/svo/src/frame_handler_mono.cpp | 95 ++-- GPU_version/svo/src/homography.cpp | 285 ++++++++++++ GPU_version/svo/src/imu_integration.cpp | 118 +++-- GPU_version/svo/src/initialization.cpp | 72 +-- GPU_version/svo/src/matcher.cpp | 9 +- GPU_version/svo/src/math_utils.cpp | 202 +++++++++ GPU_version/svo/src/point.cpp | 2 +- GPU_version/svo/src/pose_optimizer.cpp | 4 +- GPU_version/svo/src/reprojector.cpp | 26 +- GPU_version/svo/src/robust_cost.cpp | 162 +++++++ GPU_version/svo/src/sparse_img_align.cpp | 12 +- GPU_version/svo/src/vision.cpp | 295 +++++++++++++ .../vio/include/vio_gpu/camera_loader.h | 46 ++ .../vio/include/vio_gpu/output_helper.h | 97 +++++ .../vio/include/vio_gpu/params_helper.h | 54 +++ GPU_version/vio/launch/Px30.launch | 8 +- GPU_version/vio/param/camera_atan.yaml | 8 - GPU_version/vio/param/camera_pinhole.yaml | 11 - GPU_version/vio/param/unity.yaml | 8 - GPU_version/vio/param/vo_accurate.yaml | 4 - GPU_version/vio/param/vo_fast.yaml | 13 +- GPU_version/vio/src/output_helper.cpp | 410 ++++++++++++++++++ GPU_version/vio/src/visualizer.cpp | 8 +- GPU_version/vio/src/vo_node.cpp | 6 +- 58 files changed, 3828 insertions(+), 425 deletions(-) create mode 100644 GPU_version/svo/include/gpu_svo/abstract_camera.h create mode 100644 GPU_version/svo/include/gpu_svo/aligned_mem.h create mode 100644 GPU_version/svo/include/gpu_svo/atan_camera.h create mode 100755 GPU_version/svo/include/gpu_svo/for_it.hpp create mode 100644 GPU_version/svo/include/gpu_svo/homography.h create mode 100644 GPU_version/svo/include/gpu_svo/math_utils.h create mode 100644 GPU_version/svo/include/gpu_svo/nlls_solver.h create mode 100644 GPU_version/svo/include/gpu_svo/nlls_solver_impl.hpp create mode 100644 GPU_version/svo/include/gpu_svo/params_helper.h create mode 100644 GPU_version/svo/include/gpu_svo/patch_score.h create mode 100644 GPU_version/svo/include/gpu_svo/robust_cost.h create mode 100644 GPU_version/svo/include/gpu_svo/svoVisionFactorLandMark.h create mode 100644 GPU_version/svo/include/gpu_svo/svoVisionFactorSmall.h create mode 100644 GPU_version/svo/include/gpu_svo/timer.h create mode 100644 GPU_version/svo/include/gpu_svo/vision.h create mode 100644 GPU_version/svo/src/atan_camera.cpp create mode 100644 GPU_version/svo/src/homography.cpp create mode 100644 GPU_version/svo/src/math_utils.cpp create mode 100644 GPU_version/svo/src/robust_cost.cpp create mode 100644 GPU_version/svo/src/vision.cpp create mode 100644 GPU_version/vio/include/vio_gpu/camera_loader.h create mode 100644 GPU_version/vio/include/vio_gpu/output_helper.h create mode 100644 GPU_version/vio/include/vio_gpu/params_helper.h delete mode 100644 GPU_version/vio/param/camera_atan.yaml delete mode 100644 GPU_version/vio/param/camera_pinhole.yaml delete mode 100644 GPU_version/vio/param/unity.yaml delete mode 100644 GPU_version/vio/param/vo_accurate.yaml create mode 100644 GPU_version/vio/src/output_helper.cpp diff --git a/GPU_version/svo/CMakeLists.txt b/GPU_version/svo/CMakeLists.txt index ed41132..ac021d6 100644 --- a/GPU_version/svo/CMakeLists.txt +++ b/GPU_version/svo/CMakeLists.txt @@ -5,8 +5,8 @@ CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo SET(CMAKE_VERBOSE_MAKEFILE OFF) SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") -set(CMAKE_C_STANDARD 14) -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 11) # Set definitions ADD_DEFINITIONS(-DSVO_TRACE) @@ -21,11 +21,6 @@ IF(DEFINED ENV{ARM_ARCHITECTURE}) ELSE() SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") ENDIF() -IF(CMAKE_COMPILER_IS_GNUCC) - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -ELSE() - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -ENDIF() SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") # Add plain cmake packages @@ -88,12 +83,17 @@ LIST(APPEND SOURCEFILES src/config.cpp src/sparse_img_align.cpp src/imu_integration.cpp - src/cl_class.cpp) + src/cl_class.cpp + src/atan_camera.cpp + src/math_utils.cpp + src/robust_cost.cpp + src/vision.cpp + src/homography.cpp) # Create svo library ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCEFILES}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS} ) -set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for) ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\") diff --git a/GPU_version/svo/include/gpu_svo/abstract_camera.h b/GPU_version/svo/include/gpu_svo/abstract_camera.h new file mode 100644 index 0000000..5d643dc --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/abstract_camera.h @@ -0,0 +1,77 @@ +/* + * abstract_camera.h + * + * Created on: Jul 23, 2012 + * Author: cforster + */ + +#ifndef ABSTRACT_CAMERA_H_ +#define ABSTRACT_CAMERA_H_ + +#include + +namespace vk +{ + +using namespace std; +using namespace Eigen; + +class AbstractCamera +{ +protected: + + int width_; // TODO cannot be const because of omni-camera model + int height_; + +public: + + AbstractCamera() {}; // need this constructor for omni camera + AbstractCamera(int width, int height) : width_(width), height_(height) {}; + + virtual ~AbstractCamera() {}; + + /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. + virtual Vector3d + cam2world(const double& x, const double& y) const = 0; + + /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. + virtual Vector3d + cam2world(const Vector2d& px) const = 0; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const = 0; + + /// projects unit plane coordinates to camera coordinates + virtual Vector2d + world2cam(const Vector2d& uv) const = 0; + + virtual double + errorMultiplier2() const = 0; + + virtual double + errorMultiplier() const = 0; + + inline int width() const { return width_; } + + inline int height() const { return height_; } + + inline bool isInFrame(const Vector2i & obs, int boundary=0) const + { + if(obs[0]>=boundary && obs[0]=boundary && obs[1]= boundary && obs[0] < width()/(1<= boundary && obs[1] // memset +#include // memset +#include +#include + +namespace vk { +namespace aligned_mem { + + /// Check if the pointer is aligned to the specified byte granularity + inline bool + is_aligned8(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0x7) == 0; + } + + inline bool + is_aligned16(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0xF) == 0; + } + + template struct placement_delete + { + enum { Size = (1<= Size) { + placement_delete::free(buf+Size,M-Size); + placement_delete::destruct(buf); + } else { + placement_delete::free(buf, M); + } + } + }; + + template struct placement_delete + { + static inline void free(T*, size_t ) {} + }; + + inline void * aligned_alloc(size_t count, size_t alignment){ + void * mem = NULL; + assert(posix_memalign(&mem, alignment, count) == 0); + return mem; + } + + inline void aligned_free(void * memory) { + free(memory); + } + + template + inline T * aligned_alloc(size_t count, size_t alignment){ + void * data = aligned_alloc(sizeof(T)* count, alignment); + return new (data) T[count]; + } + + template + inline void aligned_free(T * memory, size_t count){ + placement_delete::free(memory, count); + aligned_free(memory); + } + + template inline void memfill(T* data, int n, const T val) + { + T* de = data + n; + for(;data < de; data++) + *data=val; + } + + template<> inline void memfill(unsigned char* data, int n, const unsigned char val) + { + memset(data, val, n); + } + + template<> inline void memfill(signed char* data, int n, const signed char val) + { + memset(data, val, n); + } + + template<> inline void memfill(char* data, int n, const char val) + { + memset(data, val, n); + } + + template + struct AlignedMem { + T* mem; + size_t count; + AlignedMem(size_t c) : count(c) { + mem = aligned_alloc(count, N); + } + ~AlignedMem() { + aligned_free(mem, count); + } + T* data() { return mem; } + const T* data() const { return mem; } + }; + +} // namespace aligned_mem +} // namespace vikit + + +#endif // VIKIT_ALIGNED_MEM_H_ diff --git a/GPU_version/svo/include/gpu_svo/atan_camera.h b/GPU_version/svo/include/gpu_svo/atan_camera.h new file mode 100644 index 0000000..49550b2 --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/atan_camera.h @@ -0,0 +1,91 @@ +/* + * atan_camera.h + * + * Created on: Aug 21, 2012 + * Author: cforster + * + * This class implements the FOV distortion model of Deverneay and Faugeras, + * Straight lines have to be straight, 2001. + * + * The code is an implementation of the ATAN class in PTAM by Georg Klein using Eigen. + */ + +#ifndef ATAN_CAMERA_H_ +#define ATAN_CAMERA_H_ + +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class ATANCamera : public AbstractCamera { + +private: + double fx_, fy_; //!< focal length + double fx_inv_, fy_inv_; //!< inverse focal length + double cx_, cy_; //!< projection center + double s_, s_inv_; //!< distortion model coeff + double tans_; //!< distortion model coeff + double tans_inv_; //!< distortion model coeff + bool distortion_; //!< use distortion model? + + //! Radial distortion transformation factor: returns ration of distorted / undistorted radius. + inline double rtrans_factor(double r) const + { + if(r < 0.001 || s_ == 0.0) + return 1.0; + else + return (s_inv_* atan(r * tans_) / r); + }; + + //! Inverse radial distortion: returns un-distorted radius from distorted. + inline double invrtrans(double r) const + { + if(s_ == 0.0) + return r; + return (tan(r * s_) * tans_inv_); + }; + +public: + + ATANCamera(double width, double height, double fx, double fy, double dx, double dy, double s); + + ~ATANCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + virtual double errorMultiplier2() const + { + return fx_; + } + + virtual double errorMultiplier() const + { + return 4*fx_*fy_; + } +}; + +} // end namespace vk + +#endif /* ATAN_CAMERA_H_ */ diff --git a/GPU_version/svo/include/gpu_svo/config.h b/GPU_version/svo/include/gpu_svo/config.h index 1001fb6..9b3c26f 100644 --- a/GPU_version/svo/include/gpu_svo/config.h +++ b/GPU_version/svo/include/gpu_svo/config.h @@ -134,6 +134,10 @@ public: static double& EBP() { return getInstance().ebp; } /// the noise used for projection static double& NRP() { return getInstance().nrp; } + static double& Gravity() { return getInstance().gravity; } + static double& X_off() { return getInstance().acc_x_off; } + static double& Y_off() { return getInstance().acc_y_off; } + static double& Teta_off() { return getInstance().gyr_z_off; } private: @@ -169,7 +173,7 @@ private: size_t max_fts; size_t quality_min_fts; int quality_max_drop_fts; - double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise; + double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise,gravity,acc_x_off,acc_y_off,gyr_z_off; }; } // namespace svo diff --git a/GPU_version/svo/include/gpu_svo/depth_filter.h b/GPU_version/svo/include/gpu_svo/depth_filter.h index d56d256..07d71e5 100644 --- a/GPU_version/svo/include/gpu_svo/depth_filter.h +++ b/GPU_version/svo/include/gpu_svo/depth_filter.h @@ -20,7 +20,7 @@ #include #include #include -#include +//#include #include #include #include @@ -79,7 +79,7 @@ public: epi_search_1d(false), verbose(false), use_photometric_disparity_error(false), - max_n_kfs(3), + max_n_kfs(10), sigma_i_sq(5e-4), seed_convergence_sigma2_thresh(200.0) {} @@ -150,7 +150,7 @@ protected: bool new_keyframe_set_; //!< Do we have a new keyframe to process?. double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds. double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds. - vk::PerformanceMonitor permon_; //!< Separate performance monitor since the DepthFilter runs in a parallel thread. + //vk::PerformanceMonitor permon_; //!< Separate performance monitor since the DepthFilter runs in a parallel thread. Matcher matcher_; /// Initialize new seeds from a frame. diff --git a/GPU_version/svo/include/gpu_svo/for_it.hpp b/GPU_version/svo/include/gpu_svo/for_it.hpp new file mode 100755 index 0000000..808f3cf --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/for_it.hpp @@ -0,0 +1,87 @@ +// +// Created by root on 5/12/21. +// + +#ifndef GPU_SVO_FOR_IT_H +#define GPU_SVO_FOR_IT_H +#include +#include +#include + +//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop +// Wrapper class +template +class for_impl +{ +public: + // The return value of the operator* of the iterator, this + // is what you will get inside of the for loop + struct item + { + size_t index; + typename T::value_type & item; + }; + typedef item value_type; + + // Custom iterator with minimal interface + struct iterator + { + iterator(typename T::iterator _it, size_t counter=0) : + it(_it), counter(counter) + {} + + iterator operator++() + { + return iterator(++it, ++counter); + } + + bool operator!=(iterator other) + { + return it != other.it; + } + + typename T::iterator::value_type item() + { + return *it; + } + + value_type operator*() + { + return value_type{counter, *it}; + } + + size_t index() + { + return counter; + } + + private: + typename T::iterator it; + size_t counter; + }; + + for_impl(T & t) : container(t) {} + + iterator begin() + { + return iterator(container.begin()); + } + + iterator end() + { + return iterator(container.end()); + } + +private: + T & container; +}; + + +// A templated free function allows you to create the wrapper class +// conveniently +template +for_impl _for(T & t) +{ + return for_impl(t); +} +#endif //GPU_SVO_FOR_IT_H diff --git a/GPU_version/svo/include/gpu_svo/frame.h b/GPU_version/svo/include/gpu_svo/frame.h index 43b344e..3a0fef6 100644 --- a/GPU_version/svo/include/gpu_svo/frame.h +++ b/GPU_version/svo/include/gpu_svo/frame.h @@ -18,8 +18,8 @@ #define SVO_FRAME_H_ #include -#include -#include +#include +#include #include #include @@ -92,6 +92,8 @@ public: /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); } + /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). + inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); } /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); } @@ -111,6 +113,7 @@ public: /// Return the pose of the frame in the (w)orld coordinate frame. inline Vector3d pos() const { return T_f_w_.inverse().translation(); } + /// Frame jacobian for projection of 3D point in (f)rame coordinate to /// unit plane coordinates uv (focal length = 1). inline static void jacobian_xyz2uv( @@ -129,13 +132,26 @@ public: J(0,4) = -(1.0 + x*J(0,2)); // -(1.0 + x^2/z^2) J(0,5) = y*z_inv; // y/z - J(1,0) = 0.0; // 0 + J(1,0) = 0.0; // 0 J(1,1) = -z_inv; // -1/z J(1,2) = y*z_inv_2; // y/z^2 J(1,3) = 1.0 + y*J(1,2); // 1.0 + y^2/z^2 J(1,4) = -J(0,3); // -x*y/z^2 J(1,5) = -x*z_inv; // x/z } + inline static void jacobian_l2uv( + const Matrix& p, + Matrix& J){ + double A = -1.0*sqrt(p.z()); + double z_inv = -1/p.z(); + J(0,0) = z_inv; + J(0,1) = 0.0; + J(0,2) = p.x()*A; + + J(1,0) = 0.0; + J(1,1) = z_inv; + J(1,2) =A*p.y(); + } }; diff --git a/GPU_version/svo/include/gpu_svo/frame_handler_base.h b/GPU_version/svo/include/gpu_svo/frame_handler_base.h index 9b207b3..c8d84e5 100644 --- a/GPU_version/svo/include/gpu_svo/frame_handler_base.h +++ b/GPU_version/svo/include/gpu_svo/frame_handler_base.h @@ -18,8 +18,8 @@ #define SVO_FRAME_HANDLER_BASE_H_ #include -#include -#include +//#include +//#include #include #include #include @@ -29,7 +29,7 @@ namespace vk { class AbstractCamera; -class PerformanceMonitor; +//class PerformanceMonitor; } namespace svo @@ -80,19 +80,19 @@ public: TrackingQuality trackingQuality() const { return tracking_quality_; } /// Get the processing time of the previous iteration. - double lastProcessingTime() const { return timer_.getTime(); } + //double lastProcessingTime() const { return timer_.getTime(); } /// Get the number of feature observations of the last frame. - size_t lastNumObservations() const { return num_obs_last_; } + // size_t lastNumObservations() const { return num_obs_last_; } protected: Stage stage_; //!< Current stage of the algorithm. bool set_reset_; //!< Flag that the user can set. Will reset the system before the next iteration. bool set_start_; //!< Flag the user can set to start the system when the next image is received. Map map_; //!< Map of keyframes created by the slam system. - vk::Timer timer_; //!< Stopwatch to measure time to process frame. - vk::RingBuffer acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance. - vk::RingBuffer acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance. + //vk::Timer timer_; //!< Stopwatch to measure time to process frame. + //vk::RingBuffer acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance. + //vk::RingBuffer acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance. size_t num_obs_last_; //!< Number of observations in the previous frame. TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features. diff --git a/GPU_version/svo/include/gpu_svo/frame_handler_mono.h b/GPU_version/svo/include/gpu_svo/frame_handler_mono.h index 58b8b22..22180d8 100644 --- a/GPU_version/svo/include/gpu_svo/frame_handler_mono.h +++ b/GPU_version/svo/include/gpu_svo/frame_handler_mono.h @@ -18,7 +18,7 @@ #define SVO_FRAME_HANDLER_H_ #include -#include +#include #include #include #include @@ -61,7 +61,7 @@ public: const SE3& T_kf_f, const cv::Mat& img, const double timestamp); - std::unique_ptr imu_integPtr_; //Integration of IMU + std::shared_ptr imu_integPtr_; //Integration of IMU protected: vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). @@ -73,6 +73,7 @@ protected: initialization::KltHomographyInit* klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography. DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points. opencl* gpu_fast_; + uint image_count_=0; /// Initialize the visual odometry algorithm. virtual void initialize(); diff --git a/GPU_version/svo/include/gpu_svo/global.h b/GPU_version/svo/include/gpu_svo/global.h index 03aff4e..f10358d 100644 --- a/GPU_version/svo/include/gpu_svo/global.h +++ b/GPU_version/svo/include/gpu_svo/global.h @@ -27,7 +27,7 @@ #include #include #include -#include +//#include #include #include #ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit @@ -78,6 +78,7 @@ namespace svo const double PI = 3.14159265; #ifdef SVO_TRACE + /* extern vk::PerformanceMonitor* g_permon; #define SVO_LOG(value) g_permon->log(std::string((#value)),(value)) #define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2) @@ -85,6 +86,7 @@ namespace svo #define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4) #define SVO_START_TIMER(name) g_permon->startTimer((name)) #define SVO_STOP_TIMER(name) g_permon->stopTimer((name)) + */ #else #define SVO_LOG(v) #define SVO_LOG2(v1, v2) diff --git a/GPU_version/svo/include/gpu_svo/homography.h b/GPU_version/svo/include/gpu_svo/homography.h new file mode 100644 index 0000000..7f5a7b2 --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/homography.h @@ -0,0 +1,85 @@ +/* + * homography.cpp + * Adaptation of PTAM-GPL HomographyInit class. + * https://github.com/Oxford-PTAM/PTAM-GPL + * Licence: GPLv3 + * Copyright 2008 Isis Innovation Limited + * + * Created on: Sep 2, 2012 + * by: cforster + * + * This class implements the homography decomposition of Faugeras and Lustman's + * 1988 tech report. Code converted to Eigen from PTAM. + * + */ + +#ifndef HOMOGRAPHY_H_ +#define HOMOGRAPHY_H_ + +#include +#include +#include +#include + +namespace vk { + +using namespace Eigen; +using namespace std; + +struct HomographyDecomposition +{ + Vector3d t; + Matrix3d R; + double d; + Vector3d n; + + // Resolved Composition + Sophus::SE3 T; //!< second from first + int score; +}; + +class Homography +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Homography (const vector >& _fts1, + const vector >& _fts2, + double _error_multiplier2, + double _thresh_in_px); + + void + calcFromPlaneParams (const Vector3d & normal, + const Vector3d & point_on_plane); + + void + calcFromMatches (); + + size_t + computeMatchesInliers (); + + bool + computeSE3fromMatches (); + + bool + decompose (); + + void + findBestDecomposition (); + + double thresh; + double error_multiplier2; + const vector >& fts_c1; //!< Features on first image on unit plane + const vector >& fts_c2; //!< Features on second image on unit plane + vector inliers; + SE3 T_c2_from_c1; //!< Relative translation and rotation of two images + Matrix3d H_c2_from_c1; //!< Homography + vector decompositions; +}; + + + + +} /* end namespace vk */ + +#endif /* HOMOGRAPHY_H_ */ diff --git a/GPU_version/svo/include/gpu_svo/imu_integration.h b/GPU_version/svo/include/gpu_svo/imu_integration.h index 2692f71..738608f 100644 --- a/GPU_version/svo/include/gpu_svo/imu_integration.h +++ b/GPU_version/svo/include/gpu_svo/imu_integration.h @@ -18,83 +18,7 @@ using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) using gtsam::symbol_shorthand::P; // Pose3 (x,y,z,r,p,y) -using gtsam::symbol_shorthand::C; // Camera pose - -//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop -// Wrapper class -template -class enumerate_impl -{ -public: - // The return value of the operator* of the iterator, this - // is what you will get inside of the for loop - struct item - { - size_t index; - typename T::value_type & item; - }; - typedef item value_type; - - // Custom iterator with minimal interface - struct iterator - { - iterator(typename T::iterator _it, size_t counter=0) : - it(_it), counter(counter) - {} - - iterator operator++() - { - return iterator(++it, ++counter); - } - - bool operator!=(iterator other) - { - return it != other.it; - } - - typename T::iterator::value_type item() - { - return *it; - } - - value_type operator*() - { - return value_type{counter, *it}; - } - - size_t index() - { - return counter; - } - - private: - typename T::iterator it; - size_t counter; - }; - - enumerate_impl(T & t) : container(t) {} - - iterator begin() - { - return iterator(container.begin()); - } - - iterator end() - { - return iterator(container.end()); - } - -private: - T & container; -}; - -// A templated free function allows you to create the wrapper class -// conveniently -template -enumerate_impl enumerate(T & t) -{ - return enumerate_impl(t); -} +using gtsam::symbol_shorthand::L; // Landmark class Imu_Integration{ public: @@ -103,7 +27,10 @@ public: bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr& new_frame); bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame); bool update(double* imu= nullptr); - bool predict(boost::shared_ptr&,std::size_t&,const double reproj_thresh); + bool predict(boost::shared_ptr&, + boost::shared_ptr& last_frame ,std::size_t&,const double reproj_thresh); + bool init(boost::shared_ptr&); + Sophus::SE3 preintegrate_predict(); private: std::shared_ptr statePtr; std::shared_ptr imu_biasPtr; @@ -116,6 +43,7 @@ private: std::uint32_t imu_factor_id=0; uint imu_n=0; std::shared_ptr optimizerParamPtr; + gtsam::NavState Integ_predict; bool syn=false; }; #endif //SVO_IMU_INTEGRATION_H diff --git a/GPU_version/svo/include/gpu_svo/initialization.h b/GPU_version/svo/include/gpu_svo/initialization.h index 21f9350..6c3b3b5 100644 --- a/GPU_version/svo/include/gpu_svo/initialization.h +++ b/GPU_version/svo/include/gpu_svo/initialization.h @@ -19,6 +19,7 @@ #include #include +#include namespace svo { diff --git a/GPU_version/svo/include/gpu_svo/math_utils.h b/GPU_version/svo/include/gpu_svo/math_utils.h new file mode 100644 index 0000000..6fffc38 --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/math_utils.h @@ -0,0 +1,171 @@ +/* + * math_utils.h + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#ifndef MATH_UTILS_H_ +#define MATH_UTILS_H_ + + +#include +#include +#include + +namespace vk +{ + +using namespace Eigen; +using namespace std; +using namespace Sophus; + +Vector3d triangulateFeatureNonLin( + const Matrix3d& R, + const Vector3d& t, + const Vector3d& feature1, + const Vector3d& feature2); + +/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e. +/// perfect triangulation without noise! +bool depthFromTriangulationExact( + const Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c); + +double reprojError( + const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2); + +double computeInliers( + const vector& features1, + const vector& features2, + const Matrix3d& R, + const Vector3d& t, + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, + vector& inliers, + vector& outliers); + +void computeInliersOneView( + const vector & feature_sphere_vec, + const vector & xyz_vec, + const Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers); + +//! Direct Cosine Matrix to Roll Pitch Yaw +Vector3d dcm2rpy(const Matrix3d &R); + +//! Roll Pitch Yaw to Direct Cosine Matrix +Matrix3d rpy2dcm(const Vector3d &rpy); + +//! Angle Axis parametrization to Quaternion +Quaterniond angax2quat(const Vector3d& n, const double& angle); + +//! Angle Axis parametrization to Matrix representation +Matrix3d angax2dcm(const Vector3d& n, const double& angle); + +double sampsonusError( + const Vector2d &v2Dash, + const Matrix3d& m3Essential, + const Vector2d& v2); + +inline Matrix3d sqew(const Vector3d& v) +{ + Matrix3d v_sqew; + v_sqew << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return v_sqew; +} + +inline double norm_max(const Eigen::VectorXd & v) +{ + double max = -1; + for (int i=0; imax){ + max = abs; + } + } + return max; +} + +inline Vector2d project2d(const Vector3d& v) +{ + return v.head<2>()/v[2]; +} + +inline Vector3d unproject2d(const Vector2d& v) +{ + return Vector3d(v[0], v[1], 1.0); +} + +inline Vector3d project3d(const Vector4d& v) +{ + return v.head<3>()/v[3]; +} + +inline Vector4d unproject3d(const Vector3d& v) +{ + return Vector4d(v[0], v[1], v[2], 1.0); +} + +template +T getMedian(vector& data_vec) +{ + assert(!data_vec.empty()); + typename vector::iterator it = data_vec.begin()+floor(data_vec.size()/2); + nth_element(data_vec.begin(), it, data_vec.end()); + return *it; +} + +inline double pyrFromZero_d(double x_0, int level) +{ + return x_0/(1< & frame_jac) +{ + const double x = xyz[0]; + const double y = xyz[1]; + const double z = xyz[2]; + const double z_2 = z*z; + + frame_jac(0,0) = -1./z *focal_length; + frame_jac(0,1) = 0; + frame_jac(0,2) = x/z_2 *focal_length; + frame_jac(0,3) = x*y/z_2 * focal_length; + frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length; + frame_jac(0,5) = y/z *focal_length; + + frame_jac(1,0) = 0; + frame_jac(1,1) = -1./z *focal_length; + frame_jac(1,2) = y/z_2 *focal_length; + frame_jac(1,3) = (1+y*y/z_2) *focal_length; + frame_jac(1,4) = -x*y/z_2 *focal_length; + frame_jac(1,5) = -x/z *focal_length; +} + +} // end namespace vk + +#endif /* MATH_UTILS_H_ */ diff --git a/GPU_version/svo/include/gpu_svo/nlls_solver.h b/GPU_version/svo/include/gpu_svo/nlls_solver.h new file mode 100644 index 0000000..cb6320b --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/nlls_solver.h @@ -0,0 +1,169 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_H_ +#define LM_SOLVER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +/** + * \brief Abstract Class for solving nonlinear least-squares (NLLS) problems. + * + * The function implements two algorithms: Levenberg Marquardt and Gauss Newton + * + * Example implementations of this function can be found in the rpl_examples + * package: img_align_2d.cpp, img_align_3d.cpp + * + * Template Parameters: + * D : dimension of the residual + * T : type of the model, e.g. SE2, SE3 + */ + +template +class NLLSSolver { + +public: + typedef T ModelType; + enum Method{GaussNewton, LevenbergMarquardt}; + enum ScaleEstimatorType{UnitScale, TDistScale, MADScale, NormalScale}; + enum WeightFunctionType{UnitWeight, TDistWeight, TukeyWeight, HuberWeight}; + +protected: + Matrix H_; //!< Hessian approximation + Matrix Jres_; //!< Jacobian x Residual + Matrix x_; //!< update step + bool have_prior_; + ModelType prior_; + Matrix I_prior_; //!< Prior information matrix (inverse covariance) + double chi2_; + double rho_; + Method method_; + + /// If the flag linearize_system is set, the function must also compute the + /// Jacobian and set the member variables H_, Jres_ + virtual double + computeResiduals (const ModelType& model, + bool linearize_system, + bool compute_weight_scale) = 0; + + /// Solve the linear system H*x = Jres. This function must set the update + /// step in the member variable x_. Must return true if the system could be + /// solved and false if it was singular. + virtual int + solve () = 0; + + virtual void + update (const ModelType& old_model, ModelType& new_model) = 0; + + virtual void + applyPrior (const ModelType& current_model) { } + + virtual void + startIteration () { } + + virtual void + finishIteration () { } + + virtual void + finishTrial () { } + +public: + + /// Damping parameter. If mu > 0, coefficient matrix is positive definite, this + /// ensures that x is a descent direction. If mu is large, x is a short step in + /// the steepest direction. This is good if the current iterate is far from the + /// solution. If mu is small, LM approximates gauss newton iteration and we + /// have (almost) quadratic convergence in the final stages. + double mu_init_, mu_; + double nu_init_, nu_; //!< Increase factor of mu after fail + size_t n_iter_init_, n_iter_; //!< Number of Iterations + size_t n_trials_; //!< Number of trials + size_t n_trials_max_; //!< Max number of trials + size_t n_meas_; //!< Number of measurements + bool stop_; //!< Stop flag + bool verbose_; //!< Output Statistics + double eps_; //!< Stop if update norm is smaller than eps + size_t iter_; //!< Current Iteration + + // robust least squares + bool use_weights_; + float scale_; + robust_cost::ScaleEstimatorPtr scale_estimator_; + robust_cost::WeightFunctionPtr weight_function_; + + NLLSSolver() : + have_prior_(false), + method_(LevenbergMarquardt), + mu_init_(0.01f), + mu_(mu_init_), + nu_init_(2.0), + nu_(nu_init_), + n_iter_init_(15), + n_iter_(n_iter_init_), + n_trials_(0), + n_trials_max_(5), + n_meas_(0), + stop_(false), + verbose_(true), + eps_(0.0000000001), + iter_(0), + use_weights_(false), + scale_(0.0), + scale_estimator_(NULL), + weight_function_(NULL) + { } + + virtual ~NLLSSolver() {} + + /// Calls the GaussNewton or LevenbergMarquardt optimization strategy + void optimize(ModelType& model); + + /// Gauss Newton optimization strategy + void optimizeGaussNewton(ModelType& model); + + /// Levenberg Marquardt optimization strategy + void optimizeLevenbergMarquardt(ModelType& model); + + /// Specify the robust cost that should be used and the appropriate scale estimator + void setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function); + + /// Add prior to optimization. + void setPrior( + const ModelType& prior, + const Matrix& Information); + + /// Reset all parameters to restart the optimization + void reset(); + + /// Get the squared error + const double& getChi2() const; + + /// The Information matrix is equal to the inverse covariance matrix. + const Matrix& getInformationMatrix() const; +}; + +} // end namespace vk + +#include "gpu_svo/nlls_solver_impl.hpp" + +#endif /* LM_SOLVER_H_ */ diff --git a/GPU_version/svo/include/gpu_svo/nlls_solver_impl.hpp b/GPU_version/svo/include/gpu_svo/nlls_solver_impl.hpp new file mode 100644 index 0000000..ec36be8 --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/nlls_solver_impl.hpp @@ -0,0 +1,323 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_IMPL_HPP_ +#define LM_SOLVER_IMPL_HPP_ + +#include + +template +void vk::NLLSSolver::optimize(ModelType& model) +{ + if(method_ == GaussNewton) + optimizeGaussNewton(model); + else if(method_ == LevenbergMarquardt) + optimizeLevenbergMarquardt(model); +} + +template +void vk::NLLSSolver::optimizeGaussNewton(ModelType& model) +{ + // Compute weight scale + if(use_weights_) + computeResiduals(model, false, true); + + // Save the old model to rollback in case of unsuccessful update + ModelType old_model(model); + + // perform iterative estimation + for (iter_ = 0; iter_::Identity(D,D); + Jres_.setZero(); + + // compute initial error + n_meas_ = 0; + computeResiduals(model, true, false); + + // add damping term: + H_ += (H_.diagonal()*mu_).asDiagonal(); + + // add prior + if(have_prior_) + applyPrior(model); + + // solve the linear system + if(solve()) + { + // update the model + update(model, new_model); + + // compute error with new model and compare to old error + n_meas_ = 0; + new_chi2 = computeResiduals(new_model, false, false); + rho_ = chi2_-new_chi2; + } + else + { + // matrix was singular and could not be computed + cout << "Matrix is close to singular!" << endl; + cout << "H = " << H_ << endl; + cout << "Jres = " << Jres_ << endl; + rho_ = -1; + } + + if(rho_>0) + { + // update decrased the error -> success + model = new_model; + chi2_ = new_chi2; + stop_ = vk::norm_max(x_)<=eps_; + mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.)); + nu_ = 2.; + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Success" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << endl; + } + } + else + { + // update increased the error -> fail + mu_ *= nu_; + nu_ *= 2.; + ++n_trials_; + if (n_trials_ >= n_trials_max_) + stop_ = true; + + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Failure" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << endl; + } + } + + finishTrial(); + + } while(!(rho_>0 || stop_)); + if (stop_) + break; + + finishIteration(); + } +} + + +template +void vk::NLLSSolver::setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function) +{ + switch(scale_estimator) + { + case TDistScale: + if(verbose_) + printf("Using TDistribution Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::TDistributionScaleEstimator()); + use_weights_=true; + break; + case MADScale: + if(verbose_) + printf("Using MAD Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::MADScaleEstimator()); + use_weights_=true; + break; + case NormalScale: + if(verbose_) + printf("Using Normal Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::NormalDistributionScaleEstimator()); + use_weights_=true; + break; + default: + if(verbose_) + printf("Using Unit Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::UnitScaleEstimator()); + use_weights_=false; + } + + switch(weight_function) + { + case TDistWeight: + if(verbose_) + printf("Using TDistribution Weight Function\n"); + weight_function_.reset(new robust_cost::TDistributionWeightFunction()); + break; + case TukeyWeight: + if(verbose_) + printf("Using Tukey Weight Function\n"); + weight_function_.reset(new robust_cost::TukeyWeightFunction()); + break; + case HuberWeight: + if(verbose_) + printf("Using Huber Weight Function\n"); + weight_function_.reset(new robust_cost::HuberWeightFunction()); + break; + default: + if(verbose_) + printf("Using Unit Weight Function\n"); + weight_function_.reset(new robust_cost::UnitWeightFunction()); + } +} + +template +void vk::NLLSSolver::setPrior( + const T& prior, + const Matrix& Information) +{ + have_prior_ = true; + prior_ = prior; + I_prior_ = Information; +} + +template +void vk::NLLSSolver::reset() +{ + have_prior_ = false; + chi2_ = 1e10; + mu_ = mu_init_; + nu_ = nu_init_; + n_meas_ = 0; + n_iter_ = n_iter_init_; + iter_ = 0; + stop_ = false; +} + +template +inline const double& vk::NLLSSolver::getChi2() const +{ + return chi2_; +} + +template +inline const vk::Matrix& vk::NLLSSolver::getInformationMatrix() const +{ + return H_; +} + +#endif /* LM_SOLVER_IMPL_HPP_ */ diff --git a/GPU_version/svo/include/gpu_svo/params_helper.h b/GPU_version/svo/include/gpu_svo/params_helper.h new file mode 100644 index 0000000..f8a03dc --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/params_helper.h @@ -0,0 +1,57 @@ +// +// Created by root on 5/24/21. +// + +#ifndef GPU_SVO_PARAMS_HELPER_H +#define GPU_SVO_PARAMS_HELPER_H +/* + * ros_params_helper.h + * + * Created on: Feb 22, 2013 + * Author: cforster + * + * from libpointmatcher_ros + */ + +#include +#include + +namespace vk { + + inline + bool hasParam(const std::string& name) + { + return ros::param::has(name); + } + + template + T getParam(const std::string& name, const T& defaultValue) + { + T v; + if(ros::param::get(name, v)) + { + ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); + return v; + } + else + ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); + return defaultValue; + } + + template + T getParam(const std::string& name) + { + T v; + if(ros::param::get(name, v)) + { + ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); + return v; + } + else + ROS_ERROR_STREAM("Cannot find value for parameter: " << name); + return T(); + } + +} // namespace vk + +#endif //GPU_SVO_PARAMS_HELPER_H diff --git a/GPU_version/svo/include/gpu_svo/patch_score.h b/GPU_version/svo/include/gpu_svo/patch_score.h new file mode 100644 index 0000000..8f0516c --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/patch_score.h @@ -0,0 +1,225 @@ +/* + * patch_score.h + * + * Created on: Dec 5, 2013 + * Author: cforster + */ + +#ifndef VIKIT_PATCH_SCORE_H_ +#define VIKIT_PATCH_SCORE_H_ + +#include + +#if __SSE2__ +#include +#endif + +namespace vk { +namespace patch_score { + + +#if __SSE2__ +// Horizontal sum of uint16s stored in an XMM register +inline int SumXMM_16(__m128i &target) +{ + unsigned short int sums_store[8]; + _mm_storeu_si128((__m128i*)sums_store, target); + return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3] + + sums_store[4] + sums_store[5] + sums_store[6] + sums_store[7]; +} +// Horizontal sum of uint32s stored in an XMM register +inline int SumXMM_32(__m128i &target) +{ + unsigned int sums_store[4]; + _mm_storeu_si128((__m128i*)sums_store, target); + return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3]; +} +#endif + +/// Zero Mean Sum of Squared Differences Cost +template +class ZMSSD { +public: + + static const int patch_size_ = 2*HALF_PATCH_SIZE; + static const int patch_area_ = patch_size_*patch_size_; + static const int threshold_ = 2000*patch_area_; + uint8_t* ref_patch_; + int sumA_, sumAA_; + + ZMSSD(uint8_t* ref_patch) : + ref_patch_(ref_patch) + { + uint32_t sumA_uint=0, sumAA_uint=0; + for(int r = 0; r < patch_area_; r++) + { + uint8_t n = ref_patch_[r]; + sumA_uint += n; + sumAA_uint += n*n; + } + sumA_ = sumA_uint; + sumAA_ = sumAA_uint; + } + + static int threshold() { return threshold_; } + + int computeScore(uint8_t* cur_patch) const + { + uint32_t sumB_uint = 0; + uint32_t sumBB_uint = 0; + uint32_t sumAB_uint = 0; + for(int r = 0; r < patch_area_; r++) + { + const uint8_t cur_pixel = cur_patch[r]; + sumB_uint += cur_pixel; + sumBB_uint += cur_pixel*cur_pixel; + sumAB_uint += cur_pixel * ref_patch_[r]; + } + const int sumB = sumB_uint; + const int sumBB = sumBB_uint; + const int sumAB = sumAB_uint; + return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_; + } + + int computeScore(uint8_t* cur_patch, int stride) const + { + int sumB, sumBB, sumAB; +#if __SSE2__ + if(patch_size_ == 8) + { + // From PTAM-GPL, Copyright 2008 Isis Innovation Limited + __m128i xImageAsEightBytes; + __m128i xImageAsWords; + __m128i xTemplateAsEightBytes; + __m128i xTemplateAsWords; + __m128i xZero; + __m128i xImageSums; // These sums are 8xuint16 + __m128i xImageSqSums; // These sums are 4xint32 + __m128i xCrossSums; // These sums are 4xint32 + __m128i xProduct; + + xImageSums = _mm_setzero_si128(); + xImageSqSums = _mm_setzero_si128(); + xCrossSums = _mm_setzero_si128(); + xZero = _mm_setzero_si128(); + + uint8_t* imagepointer = cur_patch; + uint8_t* templatepointer = ref_patch_; + long unsigned int cur_stride = stride; + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + sumB = SumXMM_16(xImageSums); + sumAB = SumXMM_32(xCrossSums); + sumBB = SumXMM_32(xImageSqSums); + } + else +#endif + { + uint32_t sumB_uint = 0; + uint32_t sumBB_uint = 0; + uint32_t sumAB_uint = 0; + for(int y=0, r=0; y < patch_size_; ++y) + { + uint8_t* cur_patch_ptr = cur_patch + y*stride; + for(int x=0; x < patch_size_; ++x, ++r) + { + const uint8_t cur_px = cur_patch_ptr[x]; + sumB_uint += cur_px; + sumBB_uint += cur_px * cur_px; + sumAB_uint += cur_px * ref_patch_[r]; + } + } + sumB = sumB_uint; + sumBB = sumBB_uint; + sumAB = sumAB_uint; + } + return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_; + } +}; + +} // namespace patch_score +} // namespace vk + +#endif // VIKIT_PATCH_SCORE_H_ diff --git a/GPU_version/svo/include/gpu_svo/robust_cost.h b/GPU_version/svo/include/gpu_svo/robust_cost.h new file mode 100644 index 0000000..d03881a --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/robust_cost.h @@ -0,0 +1,156 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo 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 +* (at your option) any later version. +* +* dvo 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 dvo. If not, see . +*/ + +#ifndef VIKIT_ROBUST_COST_H_ +#define VIKIT_ROBUST_COST_H_ + +#include +#include +#include +namespace vk { +namespace robust_cost { + +// interface for scale estimators +class ScaleEstimator +{ +public: + virtual ~ScaleEstimator() {}; + virtual float compute(std::vector& errors) const = 0; +}; +typedef std::shared_ptr ScaleEstimatorPtr; + +class UnitScaleEstimator : public ScaleEstimator +{ +public: + UnitScaleEstimator() {} + virtual ~UnitScaleEstimator() {} + virtual float compute(std::vector& errors) const { return 1.0f; }; +}; + +// estimates scale by fitting a t-distribution to the data with the given degrees of freedom +class TDistributionScaleEstimator : public ScaleEstimator +{ +public: + TDistributionScaleEstimator(const float dof = DEFAULT_DOF); + virtual ~TDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + + static const float DEFAULT_DOF; + static const float INITIAL_SIGMA; +protected: + float dof_; + float initial_sigma_; +}; + +// estimates scale by computing the median absolute deviation +class MADScaleEstimator : public ScaleEstimator +{ +public: + MADScaleEstimator() {}; + virtual ~MADScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + +private: + static const float NORMALIZER;; +}; + +// estimates scale by computing the standard deviation +class NormalDistributionScaleEstimator : public ScaleEstimator +{ +public: + NormalDistributionScaleEstimator() {}; + virtual ~NormalDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; +private: +}; + +/** + * Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)). + * The errors are assumed to be normalized to unit variance. + * + * See: + * "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54) + */ +class WeightFunction +{ +public: + virtual ~WeightFunction() {}; + virtual float value(const float& x) const = 0; + virtual void configure(const float& param) {}; +}; +typedef std::shared_ptr WeightFunctionPtr; + +class UnitWeightFunction : public WeightFunction +{ +public: + UnitWeightFunction() {}; + virtual ~UnitWeightFunction() {}; + virtual float value(const float& x) const { return 1.0f; }; +}; + +/** + * Tukey's hard re-descending function. + * + * See: + * http://en.wikipedia.org/wiki/Redescending_M-estimator + */ +class TukeyWeightFunction : public WeightFunction +{ +public: + TukeyWeightFunction(const float b = DEFAULT_B); + virtual ~TukeyWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_B; +private: + float b_square; +}; + +class TDistributionWeightFunction : public WeightFunction +{ +public: + TDistributionWeightFunction(const float dof = DEFAULT_DOF); + virtual ~TDistributionWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_DOF; +private: + float dof_; + float normalizer_; +}; + +class HuberWeightFunction : public WeightFunction +{ +public: + HuberWeightFunction(const float k = DEFAULT_K); + virtual ~HuberWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_K; +private: + float k; +}; + +} // namespace robust_cost +} // namespace vk +#endif // VIKIT_ROBUST_COST_H_ diff --git a/GPU_version/svo/include/gpu_svo/sparse_img_align.h b/GPU_version/svo/include/gpu_svo/sparse_img_align.h index 58374be..6200066 100644 --- a/GPU_version/svo/include/gpu_svo/sparse_img_align.h +++ b/GPU_version/svo/include/gpu_svo/sparse_img_align.h @@ -17,8 +17,7 @@ #ifndef SVO_SPARSE_IMG_ALIGN_H_ #define SVO_SPARSE_IMG_ALIGN_H_ -#include -#include +#include #include namespace vk { @@ -32,8 +31,8 @@ class Feature; /// Optimize the pose of the frame by minimizing the photometric error of feature patches. class SparseImgAlign : public vk::NLLSSolver<6, SE3> { - static const int patch_halfsize_ = 2; - static const int patch_size_ = 2*patch_halfsize_; + static const int patch_halfsize_ = 6; + static const int patch_size_ = 6*patch_halfsize_; static const int patch_area_ = patch_size_*patch_size_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/GPU_version/svo/include/gpu_svo/svoVisionFactor.h b/GPU_version/svo/include/gpu_svo/svoVisionFactor.h index 738bd95..417357c 100644 --- a/GPU_version/svo/include/gpu_svo/svoVisionFactor.h +++ b/GPU_version/svo/include/gpu_svo/svoVisionFactor.h @@ -16,13 +16,11 @@ namespace svo { - class VisionFactor: public gtsam::NoiseModelFactor1 { + class VisionFactor: public gtsam::NoiseModelFactor2 { private: // measurement information gtsam::Point3 feature; /// body_P_sensor_; ///< TODO The pose of the sensor in the body frame int level; ///< Image pyramid level where feature was extracted. public: @@ -39,22 +37,27 @@ namespace svo { * @param body_P_sensor is the transform from body to sensor frame (default identity) * @param level Image pyramid level where feature was extracted. */ - VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp, - gtsam::Key poseKey,boost::optional body_P_sensor = boost::none) : - gtsam::NoiseModelFactor1(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){} + VisionFactor(const gtsam::Point3& feature,const int level, + const gtsam::SharedNoiseModel& H, + gtsam::Key poseKey,gtsam::Key landKey ) : + gtsam::NoiseModelFactor2(H, poseKey,landKey), feature(feature),level(level){} virtual ~VisionFactor(){} /// error function /// @param p the Estimated pose in Pose3 /// @param H the optional Jacobian matrix, which use boost optional and has default null pointer - gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional Hp = boost::none) const { + gtsam::Vector evaluateError(const gtsam::Pose3& p,const gtsam::Point3& l, + boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { // note that use boost optional like a pointer // only calculate jacobian matrix when non-null pointer exists - gtsam::Matrix26 J; - gtsam::Point3 point(p*landmark); - svo::Frame::jacobian_xyz2uv(point, J); - Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1< +#include +#include +#include +#include +#include +#include + +namespace svo { + + class VisionFactorLandMark: public gtsam::NoiseModelFactor3 { + + private: + // measurement information + gtsam::Point3 feature_new; /// shared_ptr; + + /** + * Constructor + * @param feature Unit-bearing vector of the feature. + * @param model is the standard deviation + * @param pointKey is the index of the landmark + * @param poseKey is the index of the camera pose + * @param landmark 3D position of the landmark + * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param level Image pyramid level where feature was extracted. + */ + VisionFactorLandMark(const gtsam::Point3& feature_new,const gtsam::Point3& feature_ref, int level, + const gtsam::SharedNoiseModel& H, + gtsam::Key poseKey_ref, gtsam::Key poseKey_new,gtsam::Key landmarkKey ) : + gtsam::NoiseModelFactor3(H, poseKey_ref,poseKey_new,landmarkKey), feature_new(feature_new), + feature_ref(feature_ref),level(level){} + virtual ~VisionFactorLandMark(){} + + /// error function + /// @param p_new the Estimated pose in Pose3 + /// @param P_ref the Estimated point of landmark + /// @param l the Estimated point of landmark + /// @param H the optional Jacobian matrix, which use boost optional and has default null pointer + gtsam::Vector evaluateError(const gtsam::Pose3& p_ref,const gtsam::Pose3& p_new, const gtsam::Point3& l, + boost::optional H1 = boost::none,boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + // note that use boost optional like a pointer + // only calculate jacobian matrix when non-null pointer exists + gtsam::Matrix26 J,J1; + gtsam::Matrix23 J2,J3; + + gtsam::Point3 point(p_ref*l); + svo::Frame::jacobian_xyz2uv(point, J); + svo::Frame::jacobian_l2uv(point,J2); + + gtsam::Point3 point2(p_new*l); + svo::Frame::jacobian_xyz2uv(point2, J1); + svo::Frame::jacobian_l2uv(point2,J3); + Vector2d e1=vk::project2d(feature_ref) - vk::project2d(point)*1.0 / (1<( + gtsam::NonlinearFactor::shared_ptr(new VisionFactorLandMark(*this))); } + + }; + +} // namespace gtsamexamples +#endif //SVO_SVOVISIONFACTOR_H diff --git a/GPU_version/svo/include/gpu_svo/svoVisionFactorSmall.h b/GPU_version/svo/include/gpu_svo/svoVisionFactorSmall.h new file mode 100644 index 0000000..738bd95 --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/svoVisionFactorSmall.h @@ -0,0 +1,70 @@ +// +// Created by root on 4/13/21. +// + +#ifndef SVO_SVOVISIONFACTOR_H +#define SVO_SVOVISIONFACTOR_H +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + + class VisionFactor: public gtsam::NoiseModelFactor1 { + + private: + // measurement information + gtsam::Point3 feature; /// body_P_sensor_; ///< TODO The pose of the sensor in the body frame + int level; ///< Image pyramid level where feature was extracted. + + public: + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param feature Unit-bearing vector of the feature. + * @param model is the standard deviation + * @param pointKey is the index of the landmark + * @param poseKey is the index of the camera pose + * @param landmark 3D position of the landmark + * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param level Image pyramid level where feature was extracted. + */ + VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp, + gtsam::Key poseKey,boost::optional body_P_sensor = boost::none) : + gtsam::NoiseModelFactor1(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){} + virtual ~VisionFactor(){} + + /// error function + /// @param p the Estimated pose in Pose3 + /// @param H the optional Jacobian matrix, which use boost optional and has default null pointer + gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional Hp = boost::none) const { + // note that use boost optional like a pointer + // only calculate jacobian matrix when non-null pointer exists + gtsam::Matrix26 J; + gtsam::Point3 point(p*landmark); + svo::Frame::jacobian_xyz2uv(point, J); + Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<( + gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); } + + }; + +} // namespace gtsamexamples +#endif //SVO_SVOVISIONFACTOR_H diff --git a/GPU_version/svo/include/gpu_svo/timer.h b/GPU_version/svo/include/gpu_svo/timer.h new file mode 100644 index 0000000..45bbc7f --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/timer.h @@ -0,0 +1,81 @@ +#ifndef TIMER_H +#define TIMER_H + +#include +#include +#include + +namespace vk +{ + +class Timer +{ +private: + timeval start_time_; + double time_; + double accumulated_; +public: + + /// The constructor directly starts the timer. + Timer() : + time_(0.0), + accumulated_(0.0) + { + start(); + } + + ~Timer() + {} + + inline void start() + { + accumulated_ = 0.0; + gettimeofday(&start_time_, NULL); + } + + inline void resume() + { + gettimeofday(&start_time_, NULL); + } + + inline double stop() + { + timeval end_time; + gettimeofday(&end_time, NULL); + long seconds = end_time.tv_sec - start_time_.tv_sec; + long useconds = end_time.tv_usec - start_time_.tv_usec; + time_ = ((seconds) + useconds*0.000001) + accumulated_; + accumulated_ = time_; + return time_; + } + + inline double getTime() const + { + return time_; + } + + inline void reset() + { + time_ = 0.0; + accumulated_ = 0.0; + } + + static double getCurrentTime() + { + timeval time_now; + gettimeofday(&time_now, NULL); + return time_now.tv_sec + time_now.tv_usec*0.000001; + } + + static double getCurrentSecond() + { + timeval time_now; + gettimeofday(&time_now, NULL); + return time_now.tv_sec; + } + +}; + +} // end namespace vk + +#endif diff --git a/GPU_version/svo/include/gpu_svo/vision.h b/GPU_version/svo/include/gpu_svo/vision.h new file mode 100644 index 0000000..1533775 --- /dev/null +++ b/GPU_version/svo/include/gpu_svo/vision.h @@ -0,0 +1,77 @@ +/* + * vision.h + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#ifndef VIKIT_VISION_H_ +#define VIKIT_VISION_H_ + +#include +#include +#include + +namespace vk +{ + +//! Return value between 0 and 1 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_32f(const cv::Mat& mat, float u, float v) +{ + assert(mat.type()==CV_32F); + float x = floor(u); + float y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + float wx0 = 1.0-subpix_x; + float wx1 = subpix_x; + float wy0 = 1.0-subpix_y; + float wy1 = subpix_y; + + float val00 = mat.at(y,x); + float val10 = mat.at(y,x+1); + float val01 = mat.at(y+1,x); + float val11 = mat.at(y+1,x+1); + return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11; +} + +//! Return value between 0 and 255 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_8u(const cv::Mat& mat, float u, float v) +{ + assert(mat.type()==CV_8U); + int x = floor(u); + int y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + + float w00 = (1.0f-subpix_x)*(1.0f-subpix_y); + float w01 = (1.0f-subpix_x)*subpix_y; + float w10 = subpix_x*(1.0f-subpix_y); + float w11 = 1.0f - w00 - w01 - w10; + + const int stride = mat.step.p[0]; + unsigned char* ptr = mat.data + y*stride + x; + return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; +} + +void halfSample(const cv::Mat& in, cv::Mat& out); + +float shiTomasiScore(const cv::Mat& img, int u, int v); + +void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst); + +#ifdef __SSE2__ + +/// Used to convert a Kinect depthmap +/// Code by Christian Kerl DVO, GPL Licence +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale); + +#endif + +} // namespace vk + +#endif // VIKIT_VISION_H_ diff --git a/GPU_version/svo/src/atan_camera.cpp b/GPU_version/svo/src/atan_camera.cpp new file mode 100644 index 0000000..c86f027 --- /dev/null +++ b/GPU_version/svo/src/atan_camera.cpp @@ -0,0 +1,82 @@ +/* + * atan_camera.cpp + * + * Created on: Aug 21, 2012 + * Author: cforster + */ + + +#include +#include +#include + +namespace vk { + +ATANCamera:: +ATANCamera(double width, double height, + double fx, double fy, + double cx, double cy, + double s) : + AbstractCamera(width, height), + fx_(width*fx), fy_(height*fy), + fx_inv_(1.0/fx_), fy_inv_(1.0/fy_), + cx_(cx*width - 0.5), cy_(cy*height - 0.5), + s_(s), s_inv_(1.0/s_) +{ + if(s_ != 0.0) + { + tans_ = 2.0 * tan(s_ / 2.0); + tans_inv_ = 1.0 / tans_; + s_inv_ = 1.0 / s_; + distortion_ = true; + } + else + { + s_inv_ = 0.0; + tans_ = 0.0; + distortion_ = false; + } +} + +ATANCamera:: +~ATANCamera() +{} + +Vector3d ATANCamera:: +cam2world(const double& x, const double& y) const +{ + Vector2d dist_cam((x - cx_) * fx_inv_, + (y - cy_) * fy_inv_); + double dist_r = dist_cam.norm(); + double r = invrtrans(dist_r); + double d_factor; + if(dist_r > 0.01) + d_factor = r / dist_r; + else + d_factor = 1.0; + return unproject2d(d_factor * dist_cam).normalized(); +} + +Vector3d ATANCamera:: +cam2world (const Vector2d& px) const +{ + return cam2world(px[0], px[1]); +} + +Vector2d ATANCamera:: +world2cam(const Vector3d& xyz_c) const +{ + return world2cam(project2d(xyz_c)); +} + +Vector2d ATANCamera:: +world2cam(const Vector2d& uv) const +{ + double r = uv.norm(); + double factor = rtrans_factor(r); + Vector2d dist_cam = factor * uv; + return Vector2d(cx_ + fx_ * dist_cam[0], + cy_ + fy_ * dist_cam[1]); +} + +} /* end vk */ diff --git a/GPU_version/svo/src/bundle_adjustment.cpp b/GPU_version/svo/src/bundle_adjustment.cpp index 883116f..9bebb61 100644 --- a/GPU_version/svo/src/bundle_adjustment.cpp +++ b/GPU_version/svo/src/bundle_adjustment.cpp @@ -14,7 +14,7 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include +#include #include #include #include diff --git a/GPU_version/svo/src/config.cpp b/GPU_version/svo/src/config.cpp index 2f09b60..8801d19 100644 --- a/GPU_version/svo/src/config.cpp +++ b/GPU_version/svo/src/config.cpp @@ -14,15 +14,12 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#ifdef SVO_USE_ROS -#include -#endif +#include #include namespace svo { Config::Config() : -#ifdef SVO_USE_ROS trace_name(vk::getParam("svo/trace_name", "svo")), trace_dir(vk::getParam("svo/trace_dir", "/tmp")), n_pyr_levels(vk::getParam("svo/n_pyr_levels", 3)), @@ -58,38 +55,11 @@ Config::Config() : abc(vk::getParam("svo/ABC", 1e-1)), gbc(vk::getParam("svo/GBC", 1e-1)), ebp(vk::getParam("svo/EBP", 1e-2)), - nrp(vk::getParam("svo/NRP", 1e-5)) -#else - trace_name("svo"), - trace_dir("/tmp"), - n_pyr_levels(3), - use_imu(false), - core_n_kfs(3), - map_scale(1.0), - grid_size(25), - init_min_disparity(50.0), - init_min_tracked(50), - init_min_inliers(40), - klt_max_level(4), - klt_min_level(2), - reproj_thresh(2.0), - poseoptim_thresh(2.0), - poseoptim_num_iter(10), - structureoptim_max_pts(20), - structureoptim_num_iter(5), - loba_thresh(2.0), - loba_robust_huber_width(1.0), - loba_num_iter(0), - kfselect_mindist(0.12), - triang_min_corner_score(20.0), - triang_half_patch_size(4), - subpix_n_iter(10), - max_n_kfs(0), - img_imu_delay(0.0), - max_fts(120), - quality_min_fts(50), - quality_max_drop_fts(40) -#endif + nrp(vk::getParam("svo/NRP", 1e-5)), + gravity(vk::getParam("svo/gravity", 9.8)), + acc_x_off(vk::getParam("svo/Acceleration_offset_x", 0.0)), + acc_y_off(vk::getParam("svo/Acceleration_offset_y", 0.0)), + gyr_z_off(vk::getParam("svo/Gyroscope_offset_z", 0.0)) {} Config& Config::getInstance() diff --git a/GPU_version/svo/src/depth_filter.cpp b/GPU_version/svo/src/depth_filter.cpp index f6fa977..da00ff0 100644 --- a/GPU_version/svo/src/depth_filter.cpp +++ b/GPU_version/svo/src/depth_filter.cpp @@ -15,9 +15,9 @@ // along with this program. If not, see . #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -340,13 +340,8 @@ double DepthFilter::computeTau( Vector3d t(T_ref_cur.translation()); Vector3d a = f*z-t; double t_norm = t.norm(); - double a_norm = a.norm(); - double alpha = acos(f.dot(t)/t_norm); // dot product - double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product - double beta_plus = beta + px_error_angle; - double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI - double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines - return (z_plus - z); // tau + double beta_plus = acos(a.dot(-t)/(t_norm*a.norm())) + px_error_angle; + return (t_norm*sin(beta_plus)/sin(PI-acos(f.dot(t)/t_norm)-beta_plus) - z); // ( triangle angles sum to PI and law of sines ) tau } } // namespace svo diff --git a/GPU_version/svo/src/feature_alignment.cpp b/GPU_version/svo/src/feature_alignment.cpp index 3605333..496ce33 100644 --- a/GPU_version/svo/src/feature_alignment.cpp +++ b/GPU_version/svo/src/feature_alignment.cpp @@ -25,7 +25,7 @@ namespace svo { namespace feature_alignment { -#define SUBPIX_VERBOSE 0 +#define SUBPIX_VERBOSE 1 bool align1D( const cv::Mat& cur_img, @@ -258,7 +258,7 @@ bool align2D( #if SUBPIX_VERBOSE cout << "Iter " << iter << ":" << "\t u=" << u << ", v=" << v - << "\t update = " << update[0] << ", " << update[1] + << "\t update = " << update[0] << ", " << update[1]<< endl; // << "\t new chi2 = " << new_chi2 << endl; #endif diff --git a/GPU_version/svo/src/feature_detection.cpp b/GPU_version/svo/src/feature_detection.cpp index b5823dd..333e602 100644 --- a/GPU_version/svo/src/feature_detection.cpp +++ b/GPU_version/svo/src/feature_detection.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include namespace svo { namespace feature_detection { diff --git a/GPU_version/svo/src/frame.cpp b/GPU_version/svo/src/frame.cpp index 632e376..3aa9273 100644 --- a/GPU_version/svo/src/frame.cpp +++ b/GPU_version/svo/src/frame.cpp @@ -19,9 +19,8 @@ #include #include #include -#include -#include -#include +#include +#include namespace svo { diff --git a/GPU_version/svo/src/frame_handler_base.cpp b/GPU_version/svo/src/frame_handler_base.cpp index 6dcef82..2f22f65 100644 --- a/GPU_version/svo/src/frame_handler_base.cpp +++ b/GPU_version/svo/src/frame_handler_base.cpp @@ -14,7 +14,7 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include +#include #include #include #include @@ -30,48 +30,14 @@ namespace svo { // definition of global and static variables which were declared in the header -#ifdef SVO_TRACE -vk::PerformanceMonitor* g_permon = NULL; -#endif FrameHandlerBase::FrameHandlerBase() : stage_(STAGE_PAUSED), set_reset_(false), set_start_(false), - acc_frame_timings_(10), - acc_num_obs_(10), num_obs_last_(0), tracking_quality_(TRACKING_INSUFFICIENT) { -#ifdef SVO_TRACE - // Initialize Performance Monitor - g_permon = new vk::PerformanceMonitor(); - g_permon->addTimer("pyramid_creation"); - g_permon->addTimer("sparse_img_align"); - g_permon->addTimer("reproject"); - g_permon->addTimer("reproject_kfs"); - g_permon->addTimer("reproject_candidates"); - g_permon->addTimer("feature_align"); - g_permon->addTimer("pose_optimizer"); - g_permon->addTimer("point_optimizer"); - g_permon->addTimer("local_ba"); - g_permon->addTimer("tot_time"); - g_permon->addLog("timestamp"); - g_permon->addLog("img_align_n_tracked"); - g_permon->addLog("repr_n_mps"); - g_permon->addLog("repr_n_new_references"); - g_permon->addLog("sfba_thresh"); - g_permon->addLog("sfba_error_init"); - g_permon->addLog("sfba_error_final"); - g_permon->addLog("sfba_n_edges_final"); - g_permon->addLog("loba_n_erredges_init"); - g_permon->addLog("loba_n_erredges_fin"); - g_permon->addLog("loba_err_init"); - g_permon->addLog("loba_err_fin"); - g_permon->addLog("n_candidates"); - g_permon->addLog("dropout"); - g_permon->init(Config::traceName(), Config::traceDir()); -#endif SVO_INFO_STREAM("SVO initialized"); } @@ -79,9 +45,6 @@ FrameHandlerBase::FrameHandlerBase() : FrameHandlerBase::~FrameHandlerBase() { SVO_INFO_STREAM("SVO destructor invoked"); -#ifdef SVO_TRACE - delete g_permon; -#endif } bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp) @@ -95,10 +58,7 @@ bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp) if(stage_ == STAGE_PAUSED) return false; - SVO_LOG(timestamp); SVO_DEBUG_STREAM("New Frame"); - SVO_START_TIMER("tot_time"); - timer_.start(); // some cleanup from last iteration, can't do before because of visualization map_.emptyTrash(); @@ -110,24 +70,15 @@ int FrameHandlerBase::finishFrameProcessingCommon( const UpdateResult dropout, const size_t num_observations) { - SVO_DEBUG_STREAM("Frame: "<writeToFile(); - { - boost::unique_lock lock(map_.point_candidates_.mut_); - size_t n_candidates = map_.point_candidates_.candidates_.size(); - SVO_LOG(n_candidates); - } -#endif + // SVO_STOP_TIMER("tot_time"); if(dropout == RESULT_FAILURE && (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING )) @@ -181,10 +132,8 @@ void FrameHandlerBase::optimizeStructure( int max_iter) { deque pts; - for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) - { - if((*it)->point != NULL) - pts.push_back((*it)->point); + for(auto&& it:frame->fts_){ + if(it->point!=NULL)pts.push_back(it->point); } max_n_pts = min(max_n_pts, pts.size()); nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator); diff --git a/GPU_version/svo/src/frame_handler_mono.cpp b/GPU_version/svo/src/frame_handler_mono.cpp index cb4acf6..278c95b 100644 --- a/GPU_version/svo/src/frame_handler_mono.cpp +++ b/GPU_version/svo/src/frame_handler_mono.cpp @@ -22,8 +22,8 @@ #include #include #include -#include #include +#include #ifdef USE_BUNDLE_ADJUSTMENT #include #endif @@ -47,7 +47,7 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init) gpu_fast_->write_buf(0,3,(1 << 18)); klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); initialize(); - imu_integPtr_=std::make_unique(SE_init); + imu_integPtr_=std::make_shared(SE_init); } void FrameHandlerMono::initialize() @@ -111,21 +111,19 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() { + new_frame_->T_f_w_ = last_frame_->T_f_w_; initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_); - if(res == initialization::FAILURE) - return RESULT_FAILURE; - else if(res == initialization::NO_KEYFRAME) + if(res == initialization::NO_KEYFRAME) return RESULT_NO_KEYFRAME; - // two-frame bundle adjustment -#ifdef USE_BUNDLE_ADJUSTMENT - ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_); -#endif - new_frame_->setKeyframe(); double depth_mean, depth_min; frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); + depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); + if(!imu_integPtr_->init(new_frame_)){ + return RESULT_FAILURE; + } // add frame to map map_.addKeyframe(new_frame_); @@ -135,37 +133,49 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() return RESULT_IS_KEYFRAME; } + FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() { - // Set initial pose TODO use prior - new_frame_->T_f_w_ = last_frame_->T_f_w_; - + SE3 tem=imu_integPtr_->preintegrate_predict(); + Eigen::Quaternion q(tem.rotation_matrix()); + q.vec() *= 1/q.norm(); + q.w() *= 1/q.norm(); + q.x()=0.0; + q.z()=0.0; + tem.translation().y()=0.0; + new_frame_->T_f_w_ = SE3(q,tem.translation()); // sparse image align SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), - 30, SparseImgAlign::GaussNewton, false, false); + 30, SparseImgAlign::LevenbergMarquardt, false, true); img_align.run(last_frame_, new_frame_); // map reprojection & feature alignment reprojector_.reprojectMap(new_frame_, overlap_kfs_); - std::cerr<<"Reprojection Map:\t nPoints = "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps - tracking_quality_ = TRACKING_INSUFFICIENT; - return RESULT_FAILURE; + std::cerr<<"Reprojection Map: "<<"\t nPoint:"<predict(new_frame_,last_frame_,sfba_n_edges_final,Config::poseOptimThresh())){ + std::cerr<<"imu failed\n"; + return RESULT_FAILURE; } + //usleep(5000); + double sfba_thresh, sfba_error_init, sfba_error_final; + pose_optimizer::optimizeGaussNewton( + Config::poseOptimThresh(), Config::poseOptimNumIter(), false, + new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); // pose optimization subject to change for adding IMU - size_t sfba_n_edges_final; - if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh())) + /* + if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,Config::poseOptimThresh())){ + std::cerr<<"imu failed\n"; return RESULT_FAILURE; - + }*/ std::cerr<<"Reprojected points after opmization: "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps return RESULT_FAILURE; } double depth_mean, depth_min; @@ -187,28 +196,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() new_frame_->setKeyframe(); // new keyframe selected - for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it) - if((*it)->point != NULL) - (*it)->point->addFrameRef(*it); - map_.point_candidates_.addCandidatePointToFrame(new_frame_); - - // optional bundle adjustment -#ifdef USE_BUNDLE_ADJUSTMENT - if(Config::lobaNumIter() > 0) - { - SVO_START_TIMER("local_ba"); - setCoreKfs(Config::coreNKfs()); - size_t loba_n_erredges_init, loba_n_erredges_fin; - double loba_err_init, loba_err_fin; - ba::localBA(new_frame_.get(), &core_kfs_, &map_, - loba_n_erredges_init, loba_n_erredges_fin, - loba_err_init, loba_err_fin); - SVO_STOP_TIMER("local_ba"); - SVO_LOG4(loba_n_erredges_init, loba_n_erredges_fin, loba_err_init, loba_err_fin); - SVO_DEBUG_STREAM("Local BA:\t RemovedEdges {"<fts_){ + if(it->point != NULL)it->point->addFrameRef(it); } -#endif + map_.point_candidates_.addCandidatePointToFrame(new_frame_); // init new depth-filters depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); @@ -294,12 +285,12 @@ void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame) bool FrameHandlerMono::needNewKf(double scene_depth_mean) { - for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it) + for(auto&& it:overlap_kfs_) { - Vector3d relpos = new_frame_->w2f(it->first->pos()); + Vector3d relpos = new_frame_->w2f(it.first->pos()); if(fabs(relpos.x())/scene_depth_mean < Config::kfSelectMinDist() && - fabs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist()*0.8 && - fabs(relpos.z())/scene_depth_mean < Config::kfSelectMinDist()*1.3) + fabs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist() && + fabs(relpos.z())/scene_depth_mean < Config::kfSelectMinDist()) return false; } return true; diff --git a/GPU_version/svo/src/homography.cpp b/GPU_version/svo/src/homography.cpp new file mode 100644 index 0000000..c73617d --- /dev/null +++ b/GPU_version/svo/src/homography.cpp @@ -0,0 +1,285 @@ +// +// Created by root on 5/24/21. +// +/* + * homography.cpp + * Adaptation of PTAM-GPL HomographyInit class. + * https://github.com/Oxford-PTAM/PTAM-GPL + * Licence: GPLv3 + * Copyright 2008 Isis Innovation Limited + * + * Created on: Sep 2, 2012 + * by: cforster + */ + +#include +#include +#include + +namespace vk { + + Homography:: + Homography(const vector >& _fts1, + const vector >& _fts2, + double _error_multiplier2, + double _thresh_in_px) : + thresh(_thresh_in_px), + error_multiplier2(_error_multiplier2), + fts_c1(_fts1), + fts_c2(_fts2) + { + } + + void Homography:: + calcFromPlaneParams(const Vector3d& n_c1, const Vector3d& xyz_c1) + { + double d = n_c1.dot(xyz_c1); // normal distance from plane to KF + H_c2_from_c1 = T_c2_from_c1.rotation_matrix() + (T_c2_from_c1.translation()*n_c1.transpose())/d; + } + + void Homography:: + calcFromMatches() + { + vector src_pts(fts_c1.size()), dst_pts(fts_c1.size()); + for(size_t i=0; i(0,0); + H_c2_from_c1(0,1) = cvH.at(0,1); + H_c2_from_c1(0,2) = cvH.at(0,2); + H_c2_from_c1(1,0) = cvH.at(1,0); + H_c2_from_c1(1,1) = cvH.at(1,1); + H_c2_from_c1(1,2) = cvH.at(1,2); + H_c2_from_c1(2,0) = cvH.at(2,0); + H_c2_from_c1(2,1) = cvH.at(2,1); + H_c2_from_c1(2,2) = cvH.at(2,2); + } + + size_t Homography:: + computeMatchesInliers() + { + inliers.clear(); inliers.resize(fts_c1.size()); + size_t n_inliers = 0; + for(size_t i=0; i q(decompositions.front().T.rotation_matrix()); + q.vec() *= 1/q.norm(); + q.w() *= 1/q.norm(); + q.x()=0.0; + q.z()=0.0; + T_c2_from_c1=Sophus::SE3(q.toRotationMatrix(),Eigen::Vector3d(decompositions.front().T.translation().x(),0.0,decompositions.front().T.translation().z())); + return true; + } + + bool Homography:: + decompose() + { + decompositions.clear(); + JacobiSVD svd(H_c2_from_c1, ComputeThinU | ComputeThinV); + + Vector3d singular_values = svd.singularValues(); + + double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT) + double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book. + double d3 = fabs(singular_values[2]); + + Matrix3d U = svd.matrixU(); + Matrix3d V = svd.matrixV(); // VT^T + double s = U.determinant() * V.determinant(); + + if(!(d1 != d2 && d2 != d3)) + { + printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. "); + return false; + } + + double x1_PM; + double x3_PM; + + // All below deals with the case = 1 case. + // Case 1 implies (d1 != d3) + { // Eq. 12 + x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3)); + x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3)); + }; + + double e1[4] = {1.0,-1.0, 1.0,-1.0}; + double e3[4] = {1.0, 1.0,-1.0,-1.0}; + + Vector3d np; + HomographyDecomposition decomp; + + // Case 1, d' > 0: + decomp.d = s * d2; + for(size_t signs=0; signs<4; signs++) + { + // Eq 13 + decomp.R = Matrix3d::Identity(); + double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; + double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2; + /* + * correct the rotation matrix by Majid + */ + decomp.R(0,0) = dCosTheta; //1 + decomp.R(0,2) = -dSinTheta; //0 + decomp.R(2,0) = dSinTheta; + decomp.R(2,2) = dCosTheta; + + // Eq 14 + decomp.t[0] = (d1 - d3) * x1_PM * e1[signs]; + decomp.t[1] = 0.0; + decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs]; + + np[0] = x1_PM * e1[signs]; + np[1] = 0.0; + np[2] = x3_PM * e3[signs]; + decomp.n = V * np; + + decompositions.push_back(decomp); + } + + // Case 1, d' < 0: + decomp.d = s * -d2; + for(size_t signs=0; signs<4; signs++) + { + // Eq 15 + decomp.R = -1 * Matrix3d::Identity(); + double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; + double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2; + /* + * correct the rotation matrix by Majid + */ + decomp.R(0,0) = dCosPhi; + decomp.R(0,2) = dSinPhi; + decomp.R(2,0) = dSinPhi; + decomp.R(2,2) = -dCosPhi; + + // Eq 16 + decomp.t[0] = (d1 + d3) * x1_PM * e1[signs]; + decomp.t[1] = 0.0; + decomp.t[2] = (d1 + d3) * x3_PM * e3[signs]; + + np[0] = x1_PM * e1[signs]; + np[1] = 0.0; + np[2] = x3_PM * e3[signs]; + decomp.n = V * np; + + decompositions.push_back(decomp); + } + + // Save rotation and translation of the decomposition + for(unsigned int i=0; i 0.0) + nPositive++; + } + decom.score = -nPositive; + } + + sort(decompositions.begin(), decompositions.end()); + decompositions.resize(4); + + for(size_t i=0; i 0.0) + nPositive++; + }; + decom.score = -nPositive; + } + + sort(decompositions.begin(), decompositions.end()); + decompositions.resize(2); + + // According to Faugeras and Lustman, ambiguity exists if the two scores are equal + // but in practive, better to look at the ratio! + double dRatio = (double) decompositions[1].score / (double) decompositions[0].score; + + if(dRatio < 0.9) // no ambiguity! + decompositions.erase(decompositions.begin() + 1); + else // two-way ambiguity. Resolve by sampsonus score of all points. + { + double dErrorSquaredLimit = thresh * thresh * 4; + double adSampsonusScores[2]; + for(size_t i=0; i<2; i++) + { + Sophus::SE3 T = decompositions[i].T; + Matrix3d Essential = T.rotation_matrix() * sqew(T.translation()); + double dSumError = 0; + for(size_t m=0; m < fts_c1.size(); m++ ) + { + double d = sampsonusError(fts_c1[m], Essential, fts_c2[m]); + if(d > dErrorSquaredLimit) + d = dErrorSquaredLimit; + dSumError += d; + } + adSampsonusScores[i] = dSumError; + } + + if(adSampsonusScores[0] <= adSampsonusScores[1]) + decompositions.erase(decompositions.begin() + 1); + else + decompositions.erase(decompositions.begin()); + } + } + + +} /* end namespace vk */ + diff --git a/GPU_version/svo/src/imu_integration.cpp b/GPU_version/svo/src/imu_integration.cpp index 922ef66..3b371d7 100644 --- a/GPU_version/svo/src/imu_integration.cpp +++ b/GPU_version/svo/src/imu_integration.cpp @@ -7,11 +7,14 @@ #include #include "gpu_svo/feature.h" #include "gpu_svo/point.h" -#include "gpu_svo/svoVisionFactor.h" +#include "gpu_svo/svoVisionFactorSmall.h" +//#include "gpu_svo/svoVisionFactor.h" +//#include "gpu_svo/svoVisionFactorLandMark.h" #include #include #include #include +#include Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ graphPtr=std::make_shared(); @@ -19,7 +22,7 @@ Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0); + gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, svo::Config::Gravity(),1.0); parameterPtr = boost::shared_ptr(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z()))); parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous @@ -34,7 +37,8 @@ Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ optimizerParamPtr=std::make_shared(); optimizerParamPtr->relinearizeThreshold = 0.1; optimizerParamPtr->relinearizeSkip = 1; - //optimizerParamPtr->evaluateNonlinearError=true; + optimizerParamPtr->evaluateNonlinearError=true; + optimizerParamPtr->enableDetailedResults=true; assert(preintegratedPtr); } Imu_Integration::~Imu_Integration(){ @@ -46,9 +50,9 @@ bool Imu_Integration::update(double* imu){ auto in=std::chrono::steady_clock::now(); if(std::chrono::duration(in-t_1).count()>20.0){ t_1=in; - preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005); + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),imu[1]-svo::Config::Y_off(),imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]-svo::Config::Teta_off()),0.005); }else{ - preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration(in-t_1).count()); + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),imu[1]-svo::Config::Y_off(),imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]-svo::Config::Teta_off()),std::chrono::duration(in-t_1).count()); t_1=in; ++imu_n; } @@ -73,10 +77,18 @@ bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptraddPrior(P(imu_factor_id-1),statePtr->pose(),optimizer.marginalCovariance(P(imu_factor_id-1))); graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),optimizer.marginalCovariance(V(imu_factor_id-1))); graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.marginalCovariance(B(imu_factor_id-1))); + for(auto&& f_n:_for(new_frame->fts_)){ + if(f_n.item->point == NULL)continue; + if(!result.exists(L(f_n.index)))continue; + f_n.item->point->pos_=result.at(L(f_n.index)); + // valuesPtr->insert(L(f_n.index),f_n.item->point->pos_); +// graphPtr->emplace_shared(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index)); + } return true; } bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame){ gtsam::Values result = optimizer.optimize(); + optimizer.print(); // Overwrite the beginning of the preintegration for the next step. statePtr=std::make_shared(result.at(P(imu_factor_id)), result.at(V(imu_factor_id))); imu_biasPtr=std::make_shared(result.at(B(imu_factor_id))); @@ -84,6 +96,7 @@ bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost: ++imu_factor_id; syn=false; gtsam::Marginals marginals(*graphPtr,result); + marginals.print(); new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t()); new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1)); graphPtr->resize(0); @@ -94,50 +107,66 @@ bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost: graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),marginals.marginalCovariance(P(imu_factor_id-1))); graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1))); graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1))); + /* + for(auto&& f_n:_for(new_frame->fts_)){ + if(f_n.item->point == NULL)continue; + if(!result.exists(L(f_n.index)))continue; + f_n.item->point->pos_=result.at(L(f_n.index)); + //valuesPtr->insert(L(f_n.index),f_n.item->point->pos_); + //graphPtr->emplace_shared(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index)); + } + //graphPtr->addPrior(L(0),new_frame->fts_.front()->point->pos_,gtsam::noiseModel::Isotropic::Sigma(3, 1e-4)); + */ return true; } -bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size_t& num_obs,const double reproj_thresh){ - usleep(5000); - syn=true; - if(imu_factor_id<1){ - statePtr=std::make_shared(gtsam::Pose3(new_frame->T_f_w_.matrix()), - gtsam::Vector3(1e-12,1e-12,1e-12)); - valuesPtr->insert(P(0),statePtr->pose()); - valuesPtr->insert(V(0),statePtr->v()); - valuesPtr->insert(B(0),*imu_biasPtr); - graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished())); - graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4)); - graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2)); +Sophus::SE3 Imu_Integration::preintegrate_predict() { + if(imu_n<1){ + syn= false; + usleep(10000); + syn=true; }else{ - if(imu_n<1)return false; - auto preint_imu_combined =dynamic_cast(*preintegratedPtr); - //preint_imu_combined.predict(*statePtr,*imu_biasPtr).print(); - //std::cerr<T_f_w_.matrix(); - valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix())); - valuesPtr->insert(V(imu_factor_id), statePtr->v()); - valuesPtr->insert(B(imu_factor_id), *imu_biasPtr); - gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id), - preint_imu_combined); - graphPtr->emplace_shared(imu_factor); - imu_n=0; + syn=true; } + auto preint_imu_combined =dynamic_cast(*preintegratedPtr); + Integ_predict=preint_imu_combined.predict(*statePtr,*imu_biasPtr); + return Sophus::SE3(Integ_predict.R(),Integ_predict.t()); +} +bool Imu_Integration::predict(boost::shared_ptr& new_frame, + boost::shared_ptr& last_frame, + std::size_t& num_obs,const double reproj_thresh){ + + auto preint_imu_combined =dynamic_cast(*preintegratedPtr); + valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix())); + valuesPtr->insert(V(imu_factor_id), Integ_predict.v()); + valuesPtr->insert(B(imu_factor_id), *imu_biasPtr); + gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id), + preint_imu_combined); + graphPtr->emplace_shared(imu_factor); + imu_n=0; - num_obs=0; for(auto&& f:new_frame->fts_){ - if(f->point==NULL)continue; + if(f->point==NULL)continue; graphPtr->emplace_shared(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id)); ++num_obs; } - // Now optimize. - gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr); + std::cout<<"------------------------------------------------------------------------------\n"; + std::cout<<"------------------------------------------------------------------------------\n"; + std::cout<<"------------------------------------------------------------------------------\n"; + graphPtr->print(); + gtsam::LevenbergMarquardtParams params0; + //params0.verbosity=gtsam::LevenbergMarquardtParams::LINEAR; + params0.verbosityLM = gtsam::LevenbergMarquardtParams::SUMMARY; + params0.setlambdaFactor(10.0); + gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr,params0); reset(optimizer,new_frame); + std::cout<<"------------------------------------------------------------------------------\n"; // Remove Measurements with too large reprojection error + /* double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2(); for(auto&& f:new_frame->fts_) { - if(f->point == NULL) - continue; + if(f->point == NULL)continue; Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<level); if(e.norm() > reproj_thresh_scaled) { @@ -146,5 +175,26 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size --num_obs; } } + */ + return true; +} +bool Imu_Integration::init(boost::shared_ptr& new_frame){ + statePtr=std::make_shared(gtsam::Pose3(new_frame->T_f_w_.matrix()), + gtsam::Vector3(1e-9,1e-9,1e-9)); + valuesPtr->insert(P(0),statePtr->pose()); + valuesPtr->insert(V(0),statePtr->v()); + valuesPtr->insert(B(0),*imu_biasPtr); + graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-5, 1e-15, 1e-5, 1e-15, 1e-8, 1e-15).finished())); + graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4)); + graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2)); + /* + for(auto&& f_n:_for(new_frame->fts_)){ + if(f_n.item->point == NULL)continue; + valuesPtr->insert(L(f_n.index),f_n.item->point->pos_); + graphPtr->emplace_shared(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index)); + } + graphPtr->addPrior(L(0),new_frame->fts_.front()->point->pos_,gtsam::noiseModel::Isotropic::Sigma(3, 1e-4)); + */ + ++imu_factor_id; return true; } \ No newline at end of file diff --git a/GPU_version/svo/src/initialization.cpp b/GPU_version/svo/src/initialization.cpp index ce04d24..88f1cb8 100644 --- a/GPU_version/svo/src/initialization.cpp +++ b/GPU_version/svo/src/initialization.cpp @@ -20,8 +20,8 @@ #include #include #include -#include -#include +#include +#include namespace svo { namespace initialization { @@ -43,26 +43,24 @@ InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref) InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) { trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_); - SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features"); - if(disparities_.size() < Config::initMinTracked()) - return FAILURE; - + return NO_KEYFRAME; double disparity = vk::getMedian(disparities_); - SVO_INFO_STREAM("Init: KLT "<cam_->errorMultiplier2(), Config::poseOptimThresh(), inliers_, xyz_in_cur_, T_cur_from_ref_); - SVO_INFO_STREAM("Init: Homography RANSAC "<T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_; - frame_cur->T_f_w_.translation() = - -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); + frame_cur->T_f_w_.translation() = -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); // For each inlier create 3D point and add feature in both frames SE3 T_world_cur = frame_cur->T_f_w_.inverse(); - for(vector::iterator it=inliers_.begin(); it!=inliers_.end(); ++it) - { - Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y); - Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y); - if(frame_ref_->cam_->isInFrame(px_cur.cast(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast(), 10) && xyz_in_cur_[*it].z() > 0) + for(vector::iterator it=inliers_.begin(); it!=inliers_.end(); ++it) { - Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale); - Point* new_point = new Point(pos); + Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y); + Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y); + if(frame_ref_->cam_->isInFrame(px_cur.cast(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast(), 10) && xyz_in_cur_[*it].z() > 0) + { + Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale); + Point* new_point = new Point(pos); - Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0)); - frame_cur->addFeature(ftr_cur); - new_point->addFrameRef(ftr_cur); + Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0)); + frame_cur->addFeature(ftr_cur); + new_point->addFrameRef(ftr_cur); - Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0)); - frame_ref_->addFeature(ftr_ref); - new_point->addFrameRef(ftr_ref); + Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0)); + frame_ref_->addFeature(ftr_ref); + new_point->addFrameRef(ftr_ref); + } } - } + SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "<fts_.size()); return SUCCESS; } @@ -118,11 +118,11 @@ void detectFeatures( // now for all maximum corners, initialize a new seed px_vec.clear(); px_vec.reserve(new_features.size()); f_vec.clear(); f_vec.reserve(new_features.size()); - std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ - px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1])); - f_vec.push_back(ftr->f); - delete ftr; - }); + for(auto&& ftr:new_features){ + px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1])); + f_vec.push_back(ftr->f); + delete ftr; + } } void trackKlt( @@ -134,8 +134,8 @@ void trackKlt( vector& f_cur, vector& disparities) { - const double klt_win_size = 30.0; - const int klt_max_iter = 30; + const double klt_win_size = 45.0;//30.0 + const int klt_max_iter = 30;//30 const double klt_eps = 0.001; vector status; vector error; diff --git a/GPU_version/svo/src/matcher.cpp b/GPU_version/svo/src/matcher.cpp index 9f44701..26e9dfc 100644 --- a/GPU_version/svo/src/matcher.cpp +++ b/GPU_version/svo/src/matcher.cpp @@ -15,10 +15,10 @@ // along with this program. If not, see . #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -139,7 +139,6 @@ bool Matcher::findMatchDirect( { if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_)) return false; - if(!ref_ftr_->frame->cam_->isInFrame( ref_ftr_->px.cast()/(1<level), halfpatch_size_+2, ref_ftr_->level)) return false; diff --git a/GPU_version/svo/src/math_utils.cpp b/GPU_version/svo/src/math_utils.cpp new file mode 100644 index 0000000..9c265ec --- /dev/null +++ b/GPU_version/svo/src/math_utils.cpp @@ -0,0 +1,202 @@ +/* + * math_utils.cpp + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#include + +namespace vk { + +using namespace Eigen; + +Vector3d +triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, + const Vector3d& feature1, const Vector3d& feature2 ) +{ + Vector3d f2 = R * feature2; + Vector2d b; + b[0] = t.dot(feature1); + b[1] = t.dot(f2); + Matrix2d A; + A(0,0) = feature1.dot(feature1); + A(1,0) = feature1.dot(f2); + A(0,1) = -A(1,0); + A(1,1) = -f2.dot(f2); + Vector2d lambda = A.inverse() * b; + Vector3d xm = lambda[0] * feature1; + Vector3d xn = t + lambda[1] * f2; + return ( xm + xn )/2; +} + +bool +depthFromTriangulationExact( + const Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c) +{ + // bearing vectors (f_r, f_c) do not need to be unit length + const Vector3d f_c_in_r(R_r_c*f_c); + const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r); + const double b = f_c_in_r.dot(t_r_c); + const double denom = (a*b - f_c_in_r.dot(f_c_in_r)); + + if(abs(denom) < 0.000001) + return false; + + depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom; + depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm(); + return true; +} + +double +reprojError(const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2) +{ + Vector2d e = project2d(f1) - project2d(f2); + return error_multiplier2 * e.norm(); +} + +double +computeInliers(const vector& features1, // c1 + const vector& features2, // c2 + const Matrix3d& R, // R_c1_c2 + const Vector3d& t, // c1_t + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, // in frame c1 + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(features1.size()); + outliers.clear(); outliers.reserve(features1.size()); + xyz_vec.clear(); xyz_vec.reserve(features1.size()); + double tot_error = 0; + //triangulate all features and compute reprojection errors and inliers + for(size_t j=0; j reproj_thresh || e2 > reproj_thresh) + outliers.push_back(j); + else + { + inliers.push_back(j); + tot_error += e1+e2; + } + } + return tot_error; +} + +void +computeInliersOneView(const vector & feature_sphere_vec, + const vector & xyz_vec, + const Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(xyz_vec.size()); + outliers.clear(); outliers.reserve(xyz_vec.size()); + for(size_t j = 0; j < xyz_vec.size(); j++ ) + { + double e = reprojError(feature_sphere_vec[j], + R.transpose() * ( xyz_vec[j] - t ), + error_multiplier2); + if(e < reproj_thresh) + inliers.push_back(j); + else + outliers.push_back(j); + } +} + +Vector3d +dcm2rpy(const Matrix3d &R) +{ + Vector3d rpy; + rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) ); + if( fabs( rpy[1] - M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + if( fabs( rpy[1] + M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) ); + rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) ); + } + } + return rpy; +} + +Matrix3d +rpy2dcm(const Vector3d &rpy) +{ + Matrix3d R1; + R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0; + R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]); + R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1); + + Matrix3d R2; + R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]); + R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0; + R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0); + + Matrix3d R3; + R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0; + R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0; + R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0; + + return R3 * R2 * R1; +} + +Quaterniond +angax2quat(const Vector3d& n, const double& angle) +{ + // n must be normalized! + double s(sin(angle/2)); + return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s ); +} + + +Matrix3d +angax2dcm(const Vector3d& n, const double& angle) +{ + // n must be normalized + Matrix3d sqewn(sqew(n)); + return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle))); +} + +double +sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2) +{ + Vector3d v3Dash = unproject2d(v2Dash); + Vector3d v3 = unproject2d(v2); + + double dError = v3Dash.transpose() * Essential * v3; + + Vector3d fv3 = Essential * v3; + Vector3d fTv3Dash = Essential.transpose() * v3Dash; + + Vector2d fv3Slice = fv3.head<2>(); + Vector2d fTv3DashSlice = fTv3Dash.head<2>(); + + return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice))); +} + +} // end namespace vk diff --git a/GPU_version/svo/src/point.cpp b/GPU_version/svo/src/point.cpp index 533bed4..24423eb 100644 --- a/GPU_version/svo/src/point.cpp +++ b/GPU_version/svo/src/point.cpp @@ -15,7 +15,7 @@ // along with this program. If not, see . #include -#include +#include #include #include #include diff --git a/GPU_version/svo/src/pose_optimizer.cpp b/GPU_version/svo/src/pose_optimizer.cpp index 8af359f..3d89d1e 100644 --- a/GPU_version/svo/src/pose_optimizer.cpp +++ b/GPU_version/svo/src/pose_optimizer.cpp @@ -19,8 +19,8 @@ #include #include #include -#include -#include +#include +#include namespace svo { namespace pose_optimizer { diff --git a/GPU_version/svo/src/reprojector.cpp b/GPU_version/svo/src/reprojector.cpp index 888d3d2..0305797 100644 --- a/GPU_version/svo/src/reprojector.cpp +++ b/GPU_version/svo/src/reprojector.cpp @@ -24,9 +24,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include namespace svo { @@ -68,10 +68,11 @@ void Reprojector::reprojectMap( resetGrid(); // Identify those Keyframes which share a common field of view. - SVO_START_TIMER("reproject_kfs"); +// SVO_START_TIMER("reproject_kfs"); list< pair > close_kfs; map_.getCloseKeyframes(frame, close_kfs); + std::cout<<"The number of closed frames: "<::second, _1) < boost::bind(&std::pair::second, _2)); @@ -98,14 +99,16 @@ void Reprojector::reprojectMap( if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) continue; (*it_ftr)->point->last_projected_kf_id_ = frame->id_; - if(reprojectPoint(frame, (*it_ftr)->point)) - overlap_kfs.back().second++; + if(reprojectPoint(frame, (*it_ftr)->point)){ + overlap_kfs.back().second++; + } + } } - SVO_STOP_TIMER("reproject_kfs"); + //SVO_STOP_TIMER("reproject_kfs"); // Now project all point candidates - SVO_START_TIMER("reproject_candidates"); + // SVO_START_TIMER("reproject_candidates"); { boost::unique_lock lock(map_.point_candidates_.mut_); auto it=map_.point_candidates_.candidates_.begin(); @@ -124,11 +127,11 @@ void Reprojector::reprojectMap( ++it; } } // unlock the mutex when out of scope - SVO_STOP_TIMER("reproject_candidates"); + //SVO_STOP_TIMER("reproject_candidates"); // Now we go through each grid cell and select one point to match. // At the end, we should have at maximum one reprojected point per cell. - SVO_START_TIMER("feature_align"); + //SVO_START_TIMER("feature_align"); for(size_t i=0; i (size_t) Config::maxFts()) break; } - SVO_STOP_TIMER("feature_align"); +// SVO_STOP_TIMER("feature_align"); } bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs) @@ -161,7 +164,6 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) it = cell.erase(it); continue; } - bool found_match = true; if(options_.find_match_direct) found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); diff --git a/GPU_version/svo/src/robust_cost.cpp b/GPU_version/svo/src/robust_cost.cpp new file mode 100644 index 0000000..ec771ae --- /dev/null +++ b/GPU_version/svo/src/robust_cost.cpp @@ -0,0 +1,162 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo 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 +* (at your option) any later version. +* +* dvo 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 dvo. If not, see . +*/ + +#include +#include +#include +#include + +namespace vk { +namespace robust_cost { + +const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f; +const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f; + +TDistributionScaleEstimator:: +TDistributionScaleEstimator(const float dof) : + dof_(dof), + initial_sigma_(INITIAL_SIGMA) +{} + +float TDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_); + int num = 0; + float lambda = initial_lamda; + int iterations = 0; + do + { + ++iterations; + initial_lamda = lambda; + num = 0; + lambda = 0.0f; + + for(std::vector::iterator it=errors.begin(); it!=errors.end(); ++it) + { + if(std::isfinite(*it)) + { + ++num; + const float error2 = (*it)*(*it); + lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) ); + } + } + lambda = float(num) / lambda; + } while(std::abs(lambda - initial_lamda) > 1e-3); + + return std::sqrt(1.0f / lambda); +} + +const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745 + +float MADScaleEstimator:: +compute(std::vector& errors) const +{ + // error must be in absolute values! + return NORMALIZER * vk::getMedian(errors); +} + +float NormalDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size(); + float var = 0.0; + std::for_each(errors.begin(), errors.end(), [&](const float d) { + var += (d - mean) * (d - mean); + }); + return std::sqrt(var); // return standard deviation +} + +const float TukeyWeightFunction::DEFAULT_B = 4.6851f; + +TukeyWeightFunction::TukeyWeightFunction(const float b) +{ + configure(b); +} + +float TukeyWeightFunction::value(const float& x) const +{ + const float x_square = x * x; + if(x_square <= b_square) + { + const float tmp = 1.0f - x_square / b_square; + return tmp * tmp; + } + else + { + return 0.0f; + } +} + +void TukeyWeightFunction:: +configure(const float& param) +{ + b_square = param * param; +} + +const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f; + +TDistributionWeightFunction:: +TDistributionWeightFunction(const float dof) +{ + configure(dof); +} + +float TDistributionWeightFunction:: +value(const float & x) const +{ + return ((dof_ + 1.0f) / (dof_ + (x * x))); +} + +void TDistributionWeightFunction:: +configure(const float& param) +{ + dof_ = param; + normalizer_ = dof_ / (dof_ + 1.0f); +} + +const float HuberWeightFunction::DEFAULT_K = 1.345f; + +HuberWeightFunction:: +HuberWeightFunction(const float k) +{ + configure(k); +} + +void HuberWeightFunction:: +configure(const float& param) +{ + k = param; +} + +float HuberWeightFunction:: +value(const float& t) const +{ + const float t_abs = std::abs(t); + if(t_abs < k) + return 1.0f; + else + return k / t_abs; +} + +} // namespace robust_cost +} // namespace vk + + diff --git a/GPU_version/svo/src/sparse_img_align.cpp b/GPU_version/svo/src/sparse_img_align.cpp index 21432c6..13ae449 100644 --- a/GPU_version/svo/src/sparse_img_align.cpp +++ b/GPU_version/svo/src/sparse_img_align.cpp @@ -20,9 +20,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include namespace svo { @@ -54,12 +54,12 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) cur_frame_ = cur_frame; ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F); jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_); - visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? - + //visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse()); for(level_=max_level_; level_>=min_level_; --level_) { + visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? mu_ = 0.1; jacobian_cache_.setZero(); have_ref_patch_cache_ = false; @@ -69,8 +69,6 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) } cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_; - - return n_meas_/patch_area_; } diff --git a/GPU_version/svo/src/vision.cpp b/GPU_version/svo/src/vision.cpp new file mode 100644 index 0000000..b6d5f58 --- /dev/null +++ b/GPU_version/svo/src/vision.cpp @@ -0,0 +1,295 @@ +/* + * vision.cpp + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#include + +#if __SSE2__ +# include +#elif __ARM_NEON__ +# include +#endif + +namespace vk { + +#ifdef __SSE2__ +void halfSampleSSE2(const unsigned char* in, unsigned char* out, int w, int h) +{ + const unsigned long long mask[2] = {0x00FF00FF00FF00FFull, 0x00FF00FF00FF00FFull}; + const unsigned char* nextRow = in + w; + __m128i m = _mm_loadu_si128((const __m128i*)mask); + int sw = w >> 4; + int sh = h >> 1; + for (int i=0; i> 1)*out.cols; + for( int x = in.cols; x > 0 ; x-=16, in_top += 16, in_bottom += 16, out_data += 8) + { + uint8x8x2_t top = vld2_u8( (const uint8_t *)in_top ); + uint8x8x2_t bottom = vld2_u8( (const uint8_t *)in_bottom ); + uint16x8_t sum = vaddl_u8( top.val[0], top.val[1] ); + sum = vaddw_u8( sum, bottom.val[0] ); + sum = vaddw_u8( sum, bottom.val[1] ); + uint8x8_t final_sum = vshrn_n_u16(sum, 2); + vst1_u8(out_data, final_sum); + } + } +} +#endif + + +void +halfSample(const cv::Mat& in, cv::Mat& out) +{ + assert( in.rows/2==out.rows && in.cols/2==out.cols); + assert( in.type()==CV_8U && out.type()==CV_8U); + +#ifdef __SSE2__ + if(aligned_mem::is_aligned16(in.data) && aligned_mem::is_aligned16(out.data) && ((in.cols % 16) == 0)) + { + halfSampleSSE2(in.data, out.data, in.cols, in.rows); + return; + } +#endif +#ifdef __ARM_NEON__ + if( (in.cols % 16) == 0 ) + { + halfSampleNEON(in, out); + return; + } +#endif + + const int stride = in.step.p[0]; + uint8_t* top = (uint8_t*) in.data; + uint8_t* bottom = top + stride; + uint8_t* end = top + stride*in.rows; + const int out_width = out.cols; + uint8_t* p = (uint8_t*) out.data; + while (bottom < end) + { + for (int j=0; j( (uint16_t (top[0]) + top[1] + bottom[0] + bottom[1])/4 ); + p++; + top += 2; + bottom += 2; + } + top += stride; + bottom += stride; + } +} + + +float +shiTomasiScore(const cv::Mat& img, int u, int v) +{ + assert(img.type() == CV_8UC1); + + float dXX = 0.0; + float dYY = 0.0; + float dXY = 0.0; + const int halfbox_size = 4; + const int box_size = 2*halfbox_size; + const int box_area = box_size*box_size; + const int x_min = u-halfbox_size; + const int x_max = u+halfbox_size; + const int y_min = v-halfbox_size; + const int y_max = v+halfbox_size; + + if(x_min < 1 || x_max >= img.cols-1 || y_min < 1 || y_max >= img.rows-1) + return 0.0; // patch is too close to the boundary + + const int stride = img.step.p[0]; + for( int y=y_min; y::depth, cn*2)); + + int x, y, delta = (int)alignSize((cols + 2)*cn, 16); + AutoBuffer _tempBuf(delta*2 + 64); + deriv_type *trow0 = alignPtr(_tempBuf + cn, 16), *trow1 = alignPtr(trow0 + delta, 16); + +#ifdef __SSE2__ + __m128i z = _mm_setzero_si128(), c3 = _mm_set1_epi16(3), c10 = _mm_set1_epi16(10); +#endif + + for( y = 0; y < rows; y++ ) + { + const uchar* srow0 = src.ptr(y > 0 ? y-1 : rows > 1 ? 1 : 0); + const uchar* srow1 = src.ptr(y); + const uchar* srow2 = src.ptr(y < rows-1 ? y+1 : rows > 1 ? rows-2 : 0); + deriv_type* drow = dst.ptr(y); + + // do vertical convolution + x = 0; +#ifdef __SSE2__ + for( ; x <= colsn - 8; x += 8 ) + { + __m128i s0 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow0 + x)), z); + __m128i s1 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow1 + x)), z); + __m128i s2 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow2 + x)), z); + __m128i t0 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s0, s2), c3), _mm_mullo_epi16(s1, c10)); + __m128i t1 = _mm_sub_epi16(s2, s0); + _mm_store_si128((__m128i*)(trow0 + x), t0); + _mm_store_si128((__m128i*)(trow1 + x), t1); + } +#endif + for( ; x < colsn; x++ ) + { + int t0 = (srow0[x] + srow2[x])*3 + srow1[x]*10; + int t1 = srow2[x] - srow0[x]; + trow0[x] = (deriv_type)t0; + trow1[x] = (deriv_type)t1; + } + + // make border + int x0 = (cols > 1 ? 1 : 0)*cn, x1 = (cols > 1 ? cols-2 : 0)*cn; + for( int k = 0; k < cn; k++ ) + { + trow0[-cn + k] = trow0[x0 + k]; trow0[colsn + k] = trow0[x1 + k]; + trow1[-cn + k] = trow1[x0 + k]; trow1[colsn + k] = trow1[x1 + k]; + } + + // do horizontal convolution, interleave the results and store them to dst + x = 0; +#ifdef __SSE2__ + for( ; x <= colsn - 8; x += 8 ) + { + __m128i s0 = _mm_loadu_si128((const __m128i*)(trow0 + x - cn)); + __m128i s1 = _mm_loadu_si128((const __m128i*)(trow0 + x + cn)); + __m128i s2 = _mm_loadu_si128((const __m128i*)(trow1 + x - cn)); + __m128i s3 = _mm_load_si128((const __m128i*)(trow1 + x)); + __m128i s4 = _mm_loadu_si128((const __m128i*)(trow1 + x + cn)); + + __m128i t0 = _mm_sub_epi16(s1, s0); + __m128i t1 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s2, s4), c3), _mm_mullo_epi16(s3, c10)); + __m128i t2 = _mm_unpacklo_epi16(t0, t1); + t0 = _mm_unpackhi_epi16(t0, t1); + // this can probably be replaced with aligned stores if we aligned dst properly. + _mm_storeu_si128((__m128i*)(drow + x*2), t2); + _mm_storeu_si128((__m128i*)(drow + x*2 + 8), t0); + } +#endif + for( ; x < colsn; x++ ) + { + deriv_type t0 = (deriv_type)(trow0[x+cn] - trow0[x-cn]); + deriv_type t1 = (deriv_type)((trow1[x+cn] + trow1[x-cn])*3 + trow1[x]*10); + drow[x*2] = t0; drow[x*2+1] = t1; + } + } + +// vector vec_mat; +// cv::split(dst, vec_mat); +// cv::namedWindow("deriv"); +// cv::imshow("deriv", vec_mat[0]); +// cv::namedWindow("derivy"); +// cv::imshow("derivy", vec_mat[1]); +// cv::waitKey(0); +} + +#ifdef __SSE2__ +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale) +{ + depth_32f.create(depth_16u.rows, depth_16u.cols, CV_32FC1); + + const unsigned short* input_ptr = depth_16u.ptr(); + float* output_ptr = depth_32f.ptr(); + + __m128 _scale = _mm_set1_ps(scale); + __m128 _zero = _mm_setzero_ps(); + __m128 _nan = _mm_set1_ps(std::numeric_limits::quiet_NaN()); + + for(int idx = 0; idx < depth_16u.size().area(); idx += 8, input_ptr += 8, output_ptr += 8) + { + __m128 _input, mask; + __m128i _inputi = _mm_load_si128((__m128i*) input_ptr); + + // load low shorts and convert to float + _input = _mm_cvtepi32_ps(_mm_unpacklo_epi16(_inputi, _mm_setzero_si128())); + + mask = _mm_cmpeq_ps(_input, _zero); + + // zero to nan + _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan)); + // scale + _input = _mm_mul_ps(_input, _scale); + // save + _mm_store_ps(output_ptr + 0, _input); + + // load high shorts and convert to float + _input = _mm_cvtepi32_ps(_mm_unpackhi_epi16(_inputi, _mm_setzero_si128())); + + mask = _mm_cmpeq_ps(_input, _zero); + + // zero to nan + _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan)); + // scale + _input = _mm_mul_ps(_input, _scale); + // save + _mm_store_ps(output_ptr + 4, _input); + } +} +#endif + +} + + diff --git a/GPU_version/vio/include/vio_gpu/camera_loader.h b/GPU_version/vio/include/vio_gpu/camera_loader.h new file mode 100644 index 0000000..e9d5a94 --- /dev/null +++ b/GPU_version/vio/include/vio_gpu/camera_loader.h @@ -0,0 +1,46 @@ +/* + * camera_loader.h + * + * Created on: Feb 11, 2014 + * Author: cforster + */ + +#ifndef VIKIT_CAMERA_LOADER_H_ +#define VIKIT_CAMERA_LOADER_H_ + +#include +#include +#include +#include + +namespace vk { +namespace camera_loader { + +/// Load from ROS Namespace +bool loadFromRosNs(const std::string& ns, vk::AbstractCamera*& cam) +{ + bool res = true; + std::string cam_model(getParam(ns+"/cam_model")); + if(cam_model == "ATAN") + { + cam = new vk::ATANCamera( + getParam(ns+"/cam_width"), + getParam(ns+"/cam_height"), + getParam(ns+"/cam_fx"), + getParam(ns+"/cam_fy"), + getParam(ns+"/cam_cx"), + getParam(ns+"/cam_cy"), + getParam(ns+"/cam_d0")); + } + else + { + cam = NULL; + res = false; + } + return res; +} + +} // namespace camera_loader +} // namespace vk + +#endif // VIKIT_CAMERA_LOADER_H_ diff --git a/GPU_version/vio/include/vio_gpu/output_helper.h b/GPU_version/vio/include/vio_gpu/output_helper.h new file mode 100644 index 0000000..a835def --- /dev/null +++ b/GPU_version/vio/include/vio_gpu/output_helper.h @@ -0,0 +1,97 @@ +/* + * output_helper.h + * + * Created on: Jan 20, 2013 + * Author: cforster + */ + +#ifndef VIKIT_OUTPUT_HELPER_H_ +#define VIKIT_OUTPUT_HELPER_H_ + +#include +#include +#include +#include +#include + +namespace vk { +namespace output_helper { + +using namespace std; +using namespace Eigen; + +void +publishTfTransform (const Sophus::SE3& T, const ros::Time& stamp, + const string& frame_id, const string& child_frame_id, + tf::TransformBroadcaster& br); + +void +publishPointMarker (ros::Publisher pub, + const Vector3d& pos, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + ros::Duration lifetime = ros::Duration(0.0)); + +void +publishLineMarker (ros::Publisher pub, + const Vector3d& start, + const Vector3d& end, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + ros::Duration lifetime = ros::Duration(0.0)); + +void +publishArrowMarker (ros::Publisher pub, + const Vector3d& pos, + const Vector3d& dir, + double scale, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color); + +void +publishHexacopterMarker (ros::Publisher pub, + const string& frame_id, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color); + +void +publishCameraMarker(ros::Publisher pub, + const string& frame_id, + const string& ns, + const ros::Time& timestamp, + int id, + double marker_scale, + const Vector3d& color); +void +publishFrameMarker (ros::Publisher pub, + const Matrix3d& rot, + const Vector3d& pos, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + ros::Duration lifetime = ros::Duration(0.0)); + + +} // namespace output_helper +} // namespace vk + + +#endif /* VIKIT_OUTPUT_HELPER_H_ */ diff --git a/GPU_version/vio/include/vio_gpu/params_helper.h b/GPU_version/vio/include/vio_gpu/params_helper.h new file mode 100644 index 0000000..1c8967f --- /dev/null +++ b/GPU_version/vio/include/vio_gpu/params_helper.h @@ -0,0 +1,54 @@ +/* + * ros_params_helper.h + * + * Created on: Feb 22, 2013 + * Author: cforster + * + * from libpointmatcher_ros + */ + +#ifndef ROS_PARAMS_HELPER_H_ +#define ROS_PARAMS_HELPER_H_ + +#include +#include + +namespace vk { + +inline +bool hasParam(const std::string& name) +{ + return ros::param::has(name); +} + +template +T getParam(const std::string& name, const T& defaultValue) +{ + T v; + if(ros::param::get(name, v)) + { + ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); + return v; + } + else + ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue); + return defaultValue; +} + +template +T getParam(const std::string& name) +{ + T v; + if(ros::param::get(name, v)) + { + ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v); + return v; + } + else + ROS_ERROR_STREAM("Cannot find value for parameter: " << name); + return T(); +} + +} // namespace vk + +#endif // ROS_PARAMS_HELPER_H_ diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index c02d6a2..67019b8 100644 --- a/GPU_version/vio/launch/Px30.launch +++ b/GPU_version/vio/launch/Px30.launch @@ -9,10 +9,10 @@ - - - - + + + + diff --git a/GPU_version/vio/param/camera_atan.yaml b/GPU_version/vio/param/camera_atan.yaml deleted file mode 100644 index e3c625c..0000000 --- a/GPU_version/vio/param/camera_atan.yaml +++ /dev/null @@ -1,8 +0,0 @@ -cam_model: ATAN -cam_width: 752 -cam_height: 480 -cam_fx: 0.509326 -cam_fy: 0.796651 -cam_cx: 0.45905 -cam_cy: 0.510056 -cam_d0: 0.9320 \ No newline at end of file diff --git a/GPU_version/vio/param/camera_pinhole.yaml b/GPU_version/vio/param/camera_pinhole.yaml deleted file mode 100644 index 506a856..0000000 --- a/GPU_version/vio/param/camera_pinhole.yaml +++ /dev/null @@ -1,11 +0,0 @@ -cam_model: Pinhole -cam_width: 640 -cam_height: 480 -cam_fx: 789.860721 -cam_fy: 790.097440 -cam_cx: 305.323524 -cam_cy: 260.464072 -cam_d0: 0.027470 -cam_d1: -0.051518 -cam_d2: 0.004375 -cam_d3: -0.004028 \ No newline at end of file diff --git a/GPU_version/vio/param/unity.yaml b/GPU_version/vio/param/unity.yaml deleted file mode 100644 index 023f378..0000000 --- a/GPU_version/vio/param/unity.yaml +++ /dev/null @@ -1,8 +0,0 @@ -cam_model: ATAN -cam_width: 1024 -cam_height: 720 -cam_fx: 1.98248 -cam_fy: 2.68212 -cam_cx: 0.502524 -cam_cy: 0.497744 -cam_d0: -0.000499999 \ No newline at end of file diff --git a/GPU_version/vio/param/vo_accurate.yaml b/GPU_version/vio/param/vo_accurate.yaml deleted file mode 100644 index 8446875..0000000 --- a/GPU_version/vio/param/vo_accurate.yaml +++ /dev/null @@ -1,4 +0,0 @@ -grid_size: 20 -max_n_kfs: 50 -loba_num_iter: 10 -max_fts: 200 \ No newline at end of file diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index e33b18a..a5f9cec 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -4,10 +4,19 @@ svo: loba_num_iter: 0 #Number of iterations in the local bundle adjustment. quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad. - ACC_noise: 0.00001 #acc white noise in continuous - GYO_noise: 0.00001 #gyro white noise in continuous + init_min_tracked: 100 + init_min_disparity: 25 + structureoptim_max_pts: 50 + kfselect_mindist: 0.12 + poseoptim_thresh: 4.0 + ACC_noise: 0.0001 #acc white noise in continuous + GYO_noise: 0.0001 #gyro white noise in continuous IUC: 0.1 #integration uncertainty continuous ABC: 0.1 #acc bias in continuous GBC: 0.1 #gyro bias in continuous EBP: 0.1 #error in the bias used for preintegration NRP: 0.0000001 #noise used for projection in preintegration + gravity: 9.65 + Acceleration_offset_x: -0.05 + Acceleration_offset_y: -0.04 + Gyroscope_offset_z: 0.001 diff --git a/GPU_version/vio/src/output_helper.cpp b/GPU_version/vio/src/output_helper.cpp new file mode 100644 index 0000000..63b1a78 --- /dev/null +++ b/GPU_version/vio/src/output_helper.cpp @@ -0,0 +1,410 @@ +/* + * output_helper.cpp + * + * Created on: Jan 20, 2013 + * Author: chrigi + */ + +#include +#include + +namespace vk { +namespace output_helper { + +void +publishTfTransform(const Sophus::SE3& T, const ros::Time& stamp, + const string& frame_id, const string& child_frame_id, + tf::TransformBroadcaster& br) +{ + tf::Transform transform_msg; + Eigen::Quaterniond q(T.rotation_matrix()); + transform_msg.setOrigin(tf::Vector3(T.translation().x(), T.translation().y(), T.translation().z())); + tf::Quaternion tf_q; tf_q.setX(q.x()); tf_q.setY(q.y()); tf_q.setZ(q.z()); tf_q.setW(q.w()); + transform_msg.setRotation(tf_q); + br.sendTransform(tf::StampedTransform(transform_msg, stamp, frame_id, child_frame_id)); +} + +void +publishPointMarker(ros::Publisher pub, + const Vector3d& pos, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + ros::Duration lifetime) +{ + visualization_msgs::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::Marker::CUBE; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.scale.y = marker_scale; + msg.scale.z = marker_scale; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.lifetime = lifetime; + msg.pose.position.x = pos[0]; + msg.pose.position.y = pos[1]; + msg.pose.position.z = pos[2]; + pub.publish(msg); +} + +void +publishLineMarker(ros::Publisher pub, + const Vector3d& start, + const Vector3d& end, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + ros::Duration lifetime) +{ + visualization_msgs::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::Marker::LINE_STRIP; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.points.resize(2); + msg.lifetime = lifetime; + msg.points[0].x = start[0]; + msg.points[0].y = start[1]; + msg.points[0].z = start[2]; + msg.points[1].x = end[0]; + msg.points[1].y = end[1]; + msg.points[1].z = end[2]; + pub.publish(msg); +} + + +void +publishArrowMarker(ros::Publisher pub, + const Vector3d& pos, + const Vector3d& dir, + double scale, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color) +{ + visualization_msgs::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::Marker::ARROW; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.scale.y = marker_scale*0.35; + msg.scale.z = 0.0; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.points.resize(2); + msg.points[0].x = pos[0]; + msg.points[0].y = pos[1]; + msg.points[0].z = pos[2]; + msg.points[1].x = pos[0] + scale*dir[0]; + msg.points[1].y = pos[1] + scale*dir[1]; + msg.points[1].z = pos[2] + scale*dir[2]; + pub.publish(msg); +} + +void +publishHexacopterMarker(ros::Publisher pub, + const string& frame_id, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color) +{ + /* + * Function by Markus Achtelik from libsfly_viz. + * Thank you. + */ + const double sqrt2_2 = sqrt(2) / 2; + + visualization_msgs::Marker marker; + + // the marker will be displayed in frame_id + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.action = 0; + marker.id = id; + + // make rotors + marker.type = visualization_msgs::Marker::CYLINDER; + marker.scale.x = 0.2*marker_scale; + marker.scale.y = 0.2*marker_scale; + marker.scale.z = 0.01*marker_scale; + marker.color.r = 0.4; + marker.color.g = 0.4; + marker.color.b = 0.4; + marker.color.a = 0.8; + marker.pose.position.z = 0; + + // front left/right + marker.pose.position.x = 0.19*marker_scale; + marker.pose.position.y = 0.11*marker_scale; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = 0.19*marker_scale; + marker.pose.position.y = -0.11*marker_scale; + marker.id--; + pub.publish(marker); + + // left/right + marker.pose.position.x = 0; + marker.pose.position.y = 0.22*marker_scale; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = 0; + marker.pose.position.y = -0.22*marker_scale; + marker.id--; + pub.publish(marker); + + // back left/right + marker.pose.position.x = -0.19*marker_scale; + marker.pose.position.y = 0.11*marker_scale; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = -0.19*marker_scale; + marker.pose.position.y = -0.11*marker_scale; + marker.id--; + pub.publish(marker); + + // make arms + marker.type = visualization_msgs::Marker::CUBE; + marker.scale.x = 0.44*marker_scale; + marker.scale.y = 0.02*marker_scale; + marker.scale.z = 0.01*marker_scale; + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = 1; + + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = -0.015*marker_scale; + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + + marker.pose.orientation.w = sqrt2_2; + marker.pose.orientation.z = sqrt2_2; + marker.id--; + pub.publish(marker); + + // 30 deg rotation 0.9659 0 0 0.2588 + marker.pose.orientation.w = 0.9659; + marker.pose.orientation.z = 0.2588; + marker.id--; + pub.publish(marker); + + marker.pose.orientation.w = 0.9659; + marker.pose.orientation.z = -0.2588; + marker.id--; + pub.publish(marker); +} + +void +publishCameraMarker(ros::Publisher pub, + const string& frame_id, + const string& ns, + const ros::Time& timestamp, + int id, + double marker_scale, + const Vector3d& color) +{ + /* + * draw a pyramid as the camera marker + */ + const double sqrt2_2 = sqrt(2) / 2; + + visualization_msgs::Marker marker; + + // the marker will be displayed in frame_id + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.action = 0; + marker.id = id; + + // make rectangles as frame + double r_w = 1.0; + double z_plane = (r_w / 2.0)*marker_scale; + marker.pose.position.x = 0; + marker.pose.position.y = (r_w / 4.0) *marker_scale; + marker.pose.position.z = z_plane; + + marker.type = visualization_msgs::Marker::CUBE; + marker.scale.x = r_w*marker_scale; + marker.scale.y = 0.04*marker_scale; + marker.scale.z = 0.04*marker_scale; + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = 1; + + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + marker.pose.orientation.z = 0; + marker.pose.orientation.w = 1; + marker.id--; + pub.publish(marker); + marker.pose.position.y = -(r_w/ 4.0)*marker_scale; + marker.id--; + pub.publish(marker); + + marker.scale.x = (r_w/2.0)*marker_scale; + marker.pose.position.x = (r_w / 2.0) *marker_scale; + marker.pose.position.y = 0; + marker.pose.orientation.w = sqrt2_2; + marker.pose.orientation.z = sqrt2_2; + marker.id--; + pub.publish(marker); + marker.pose.position.x = -(r_w / 2.0) *marker_scale; + marker.id--; + pub.publish(marker); + + // make pyramid edges + marker.scale.x = (3.0*r_w/4.0)*marker_scale; + marker.pose.position.z = 0.5*z_plane; + + marker.pose.position.x = (r_w / 4.0) *marker_scale; + marker.pose.position.y = (r_w / 8.0) *marker_scale; + // 0.08198092, -0.34727674, 0.21462883, 0.9091823 + marker.pose.orientation.x = 0.08198092; + marker.pose.orientation.y = -0.34727674; + marker.pose.orientation.z = 0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = -(r_w / 4.0) *marker_scale; + marker.pose.position.y = (r_w / 8.0) *marker_scale; + // -0.27395078, -0.22863284, 0.9091823 , 0.21462883 + marker.pose.orientation.x = 0.08198092; + marker.pose.orientation.y = 0.34727674; + marker.pose.orientation.z = -0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = -(r_w / 4.0) *marker_scale; + marker.pose.position.y = -(r_w / 8.0) *marker_scale; + // -0.08198092, 0.34727674, 0.21462883, 0.9091823 + marker.pose.orientation.x = -0.08198092; + marker.pose.orientation.y = 0.34727674; + marker.pose.orientation.z = 0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub.publish(marker); + + marker.pose.position.x = (r_w / 4.0) *marker_scale; + marker.pose.position.y = -(r_w / 8.0) *marker_scale; + // -0.08198092, -0.34727674, -0.21462883, 0.9091823 + marker.pose.orientation.x = -0.08198092; + marker.pose.orientation.y = -0.34727674; + marker.pose.orientation.z = -0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub.publish(marker); +} + +void publishFrameMarker(ros::Publisher pub, + const Matrix3d& rot, + const Vector3d& pos, + const string& ns, + const ros::Time& timestamp, + int id, + int action, + double marker_scale, + ros::Duration lifetime) +{ + visualization_msgs::Marker marker; + marker.header.frame_id = "/world"; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.id = id++; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = action; // 0 = add/modify + marker.points.reserve(2); + geometry_msgs::Point point; + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 2)); // Draw arrow in z-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 2)); // Draw arrow in z-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 2)); // Draw arrow in z-direction + marker.points.push_back(point); + marker.scale.x = 0.5*marker_scale; + marker.scale.y = 0.5*marker_scale; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.lifetime = lifetime; + pub.publish(marker); + + marker.id = id++; + marker.points.clear(); + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 0)); // Draw arrow in x-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 0)); // Draw arrow in x-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 0)); // Draw arrow in x-direction + marker.points.push_back(point); + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.lifetime = lifetime; + pub.publish(marker); + + marker.id = id++; + marker.points.clear(); + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 1)); // Draw arrow in y-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 1)); // Draw arrow in y-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 1)); // Draw arrow in y-direction + marker.points.push_back(point); + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.lifetime = lifetime; + pub.publish(marker); +} + +} // namespace output_helper +} // namespace vk + + diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index d2327f4..90ad7ab 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -21,8 +21,8 @@ #include #include #include -#include -#include +#include +#include #include namespace svo { @@ -57,16 +57,18 @@ void Visualizer::publishMinimal( { vio_gpu::Info msg_info; msg_info.header = header_msg; - msg_info.processing_time = slam.lastProcessingTime(); + //msg_info.processing_time = slam.lastProcessingTime(); msg_info.keyframes.reserve(slam.map().keyframes_.size()); for(list::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it) msg_info.keyframes.push_back((*it)->id_); msg_info.stage = static_cast(slam.stage()); msg_info.tracking_quality = static_cast(slam.trackingQuality()); + /* if(frame != NULL) msg_info.num_matches = slam.lastNumObservations(); else msg_info.num_matches = 0; + */ pub_info_.publish(msg_info); } if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME) diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index c11c6a0..b167282 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -17,15 +17,15 @@ #include #include #include -#include +#include #include #include #include #include #include #include -#include -#include +#include +#include #include