diff --git a/GPU_version/svo/CMakeLists.txt b/GPU_version/svo/CMakeLists.txt index 4f0c7c5..ed41132 100644 --- a/GPU_version/svo/CMakeLists.txt +++ b/GPU_version/svo/CMakeLists.txt @@ -96,3 +96,4 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS} ) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14) 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/svo/bundle_adjustment.h b/GPU_version/svo/include/gpu_svo/bundle_adjustment.h similarity index 99% rename from GPU_version/svo/include/svo/bundle_adjustment.h rename to GPU_version/svo/include/gpu_svo/bundle_adjustment.h index f3c3501..9eb8a94 100644 --- a/GPU_version/svo/include/svo/bundle_adjustment.h +++ b/GPU_version/svo/include/gpu_svo/bundle_adjustment.h @@ -17,7 +17,7 @@ #ifndef SVO_BUNDLE_ADJUSTMENT_H_ #define SVO_BUNDLE_ADJUSTMENT_H_ -#include +#include namespace g2o { class EdgeProjectXYZ2UV; diff --git a/GPU_version/svo/include/svo/cl_class.h b/GPU_version/svo/include/gpu_svo/cl_class.h similarity index 98% rename from GPU_version/svo/include/svo/cl_class.h rename to GPU_version/svo/include/gpu_svo/cl_class.h index ff4cd39..bffee33 100644 --- a/GPU_version/svo/include/svo/cl_class.h +++ b/GPU_version/svo/include/gpu_svo/cl_class.h @@ -67,13 +67,15 @@ public: }; int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){ try{ - for(auto i:_images)if(i.second==id) + for(auto i:_images)if(i.second==id){ i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), buf.size().width, buf.size().height, 0, reinterpret_cast(buf.data)); + } + return CL_SUCCESS; }catch (std::exception& err){ std::cerr<<"Reload buffer on GPU failed\n"< #include #include -#include -#include -#include +#include +#include +#include namespace svo { diff --git a/GPU_version/svo/include/svo/feature.h b/GPU_version/svo/include/gpu_svo/feature.h similarity index 98% rename from GPU_version/svo/include/svo/feature.h rename to GPU_version/svo/include/gpu_svo/feature.h index 6f90a9a..2184c4b 100644 --- a/GPU_version/svo/include/svo/feature.h +++ b/GPU_version/svo/include/gpu_svo/feature.h @@ -17,7 +17,7 @@ #ifndef SVO_FEATURE_H_ #define SVO_FEATURE_H_ -#include +#include namespace svo { diff --git a/GPU_version/svo/include/svo/feature_alignment.h b/GPU_version/svo/include/gpu_svo/feature_alignment.h similarity index 98% rename from GPU_version/svo/include/svo/feature_alignment.h rename to GPU_version/svo/include/gpu_svo/feature_alignment.h index 12523e3..e3b40a8 100644 --- a/GPU_version/svo/include/svo/feature_alignment.h +++ b/GPU_version/svo/include/gpu_svo/feature_alignment.h @@ -17,7 +17,7 @@ #ifndef SVO_FEATURE_ALIGNMENT_H_ #define SVO_FEATURE_ALIGNMENT_H_ -#include +#include namespace svo { diff --git a/GPU_version/svo/include/svo/feature_detection.h b/GPU_version/svo/include/gpu_svo/feature_detection.h similarity index 94% rename from GPU_version/svo/include/svo/feature_detection.h rename to GPU_version/svo/include/gpu_svo/feature_detection.h index 6267d20..4dc5ec8 100644 --- a/GPU_version/svo/include/svo/feature_detection.h +++ b/GPU_version/svo/include/gpu_svo/feature_detection.h @@ -17,9 +17,9 @@ #ifndef SVO_FEATURE_DETECTION_H_ #define SVO_FEATURE_DETECTION_H_ -#include -#include -#include +#include +#include +#include namespace svo { @@ -51,8 +51,6 @@ public: const int n_pyr_levels); virtual ~AbstractDetector() { - gpu_fast_->clear_buf(); - gpu_fast_->make_kernel("fast_gray"); }; virtual void detect( @@ -66,7 +64,6 @@ public: /// Set grid cells of existing features as occupied void setExistingFeatures(const Features& fts); - opencl* gpu_fast_= nullptr; protected: @@ -95,6 +92,7 @@ public: const int img_width, const int img_height, const int cell_size, + opencl* gpu_fast_, const int n_pyr_levels); virtual ~FastDetector() {} @@ -104,6 +102,7 @@ public: const ImgPyr& img_pyr, const double detection_threshold, Features& fts); + opencl* gpu_fast_; }; } // namespace feature_detection diff --git a/GPU_version/svo/include/svo/frame.h b/GPU_version/svo/include/gpu_svo/frame.h similarity index 99% rename from GPU_version/svo/include/svo/frame.h rename to GPU_version/svo/include/gpu_svo/frame.h index c926277..43b344e 100644 --- a/GPU_version/svo/include/svo/frame.h +++ b/GPU_version/svo/include/gpu_svo/frame.h @@ -21,7 +21,7 @@ #include #include #include -#include +#include namespace g2o { class VertexSE3Expmap; diff --git a/GPU_version/svo/include/svo/frame_handler_base.h b/GPU_version/svo/include/gpu_svo/frame_handler_base.h similarity index 98% rename from GPU_version/svo/include/svo/frame_handler_base.h rename to GPU_version/svo/include/gpu_svo/frame_handler_base.h index 9a78639..9b207b3 100644 --- a/GPU_version/svo/include/svo/frame_handler_base.h +++ b/GPU_version/svo/include/gpu_svo/frame_handler_base.h @@ -23,8 +23,8 @@ #include #include #include -#include -#include +#include +#include namespace vk { diff --git a/GPU_version/svo/include/svo/frame_handler_mono.h b/GPU_version/svo/include/gpu_svo/frame_handler_mono.h similarity index 90% rename from GPU_version/svo/include/svo/frame_handler_mono.h rename to GPU_version/svo/include/gpu_svo/frame_handler_mono.h index 58789c8..58b8b22 100644 --- a/GPU_version/svo/include/svo/frame_handler_mono.h +++ b/GPU_version/svo/include/gpu_svo/frame_handler_mono.h @@ -19,10 +19,11 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace svo { @@ -48,8 +49,8 @@ public: const set& coreKeyframes() { return core_kfs_; } /// Return the feature track to visualize the KLT tracking during initialization. - const vector& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; } - const vector& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; } + const vector& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; } + const vector& initFeatureTrackCurPx() const { return klt_homography_init_->px_cur_; } /// Access the depth filter. DepthFilter* depthFilter() const { return depth_filter_; } @@ -69,9 +70,9 @@ protected: FramePtr last_frame_; //!< Last frame, not necessarily a keyframe. set core_kfs_; //!< Keyframes in the closer neighbourhood. vector< pair > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!? - initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography. + 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_; /// Initialize the visual odometry algorithm. virtual void initialize(); diff --git a/GPU_version/svo/include/svo/global.h b/GPU_version/svo/include/gpu_svo/global.h similarity index 100% rename from GPU_version/svo/include/svo/global.h rename to GPU_version/svo/include/gpu_svo/global.h diff --git a/GPU_version/svo/include/svo/imu_integration.h b/GPU_version/svo/include/gpu_svo/imu_integration.h similarity index 99% rename from GPU_version/svo/include/svo/imu_integration.h rename to GPU_version/svo/include/gpu_svo/imu_integration.h index 11b9055..2692f71 100644 --- a/GPU_version/svo/include/svo/imu_integration.h +++ b/GPU_version/svo/include/gpu_svo/imu_integration.h @@ -13,7 +13,7 @@ #include #include #include -#include +#include using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) diff --git a/GPU_version/svo/include/svo/initialization.h b/GPU_version/svo/include/gpu_svo/initialization.h similarity index 93% rename from GPU_version/svo/include/svo/initialization.h rename to GPU_version/svo/include/gpu_svo/initialization.h index cb562e5..21f9350 100644 --- a/GPU_version/svo/include/svo/initialization.h +++ b/GPU_version/svo/include/gpu_svo/initialization.h @@ -17,7 +17,8 @@ #ifndef SVO_INITIALIZATION_H #define SVO_INITIALIZATION_H -#include +#include +#include namespace svo { @@ -35,7 +36,7 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW FramePtr frame_ref_; - KltHomographyInit() {}; + KltHomographyInit(opencl* gpu_fast_):gpu_fast_(gpu_fast_) {}; ~KltHomographyInit() {}; InitResult addFirstFrame(FramePtr frame_ref); InitResult addSecondFrame(FramePtr frame_ref); @@ -50,13 +51,15 @@ protected: vector inliers_; //!< inliers after the geometric check (e.g., Homography). vector xyz_in_cur_; //!< 3D points computed during the geometric check. SE3 T_cur_from_ref_; //!< computed transformation between the first two frames. + opencl* gpu_fast_; }; /// Detect Fast corners in the image. void detectFeatures( FramePtr frame, vector& px_vec, - vector& f_vec); + vector& f_vec, + opencl* gpu_fast); /// Compute optical flow (Lucas Kanade) for selected keypoints. void trackKlt( diff --git a/GPU_version/svo/include/svo/map.h b/GPU_version/svo/include/gpu_svo/map.h similarity index 99% rename from GPU_version/svo/include/svo/map.h rename to GPU_version/svo/include/gpu_svo/map.h index 458622b..3bee80b 100644 --- a/GPU_version/svo/include/svo/map.h +++ b/GPU_version/svo/include/gpu_svo/map.h @@ -20,7 +20,7 @@ #include #include #include -#include +#include namespace svo { diff --git a/GPU_version/svo/include/svo/matcher.h b/GPU_version/svo/include/gpu_svo/matcher.h similarity index 99% rename from GPU_version/svo/include/svo/matcher.h rename to GPU_version/svo/include/gpu_svo/matcher.h index b53340e..1eeb9fd 100644 --- a/GPU_version/svo/include/svo/matcher.h +++ b/GPU_version/svo/include/gpu_svo/matcher.h @@ -17,7 +17,7 @@ #ifndef SVO_MATCHER_H_ #define SVO_MATCHER_H_ -#include +#include namespace vk { class AbstractCamera; diff --git a/GPU_version/svo/include/svo/point.h b/GPU_version/svo/include/gpu_svo/point.h similarity index 99% rename from GPU_version/svo/include/svo/point.h rename to GPU_version/svo/include/gpu_svo/point.h index e1bb81b..81d2769 100644 --- a/GPU_version/svo/include/svo/point.h +++ b/GPU_version/svo/include/gpu_svo/point.h @@ -18,7 +18,7 @@ #define SVO_POINT_H_ #include -#include +#include namespace g2o { class VertexSBAPointXYZ; diff --git a/GPU_version/svo/include/svo/pose_optimizer.h b/GPU_version/svo/include/gpu_svo/pose_optimizer.h similarity index 98% rename from GPU_version/svo/include/svo/pose_optimizer.h rename to GPU_version/svo/include/gpu_svo/pose_optimizer.h index ca048ae..d0ad926 100644 --- a/GPU_version/svo/include/svo/pose_optimizer.h +++ b/GPU_version/svo/include/gpu_svo/pose_optimizer.h @@ -17,7 +17,7 @@ #ifndef SVO_POSE_OPTIMIZER_H_ #define SVO_POSE_OPTIMIZER_H_ -#include +#include namespace svo { diff --git a/GPU_version/svo/include/svo/reprojector.h b/GPU_version/svo/include/gpu_svo/reprojector.h similarity index 98% rename from GPU_version/svo/include/svo/reprojector.h rename to GPU_version/svo/include/gpu_svo/reprojector.h index e345f0d..8d50c35 100644 --- a/GPU_version/svo/include/svo/reprojector.h +++ b/GPU_version/svo/include/gpu_svo/reprojector.h @@ -17,8 +17,8 @@ #ifndef SVO_REPROJECTION_H_ #define SVO_REPROJECTION_H_ -#include -#include +#include +#include namespace vk { class AbstractCamera; diff --git a/GPU_version/svo/include/svo/sparse_img_align.h b/GPU_version/svo/include/gpu_svo/sparse_img_align.h similarity index 98% rename from GPU_version/svo/include/svo/sparse_img_align.h rename to GPU_version/svo/include/gpu_svo/sparse_img_align.h index 1bedfed..58374be 100644 --- a/GPU_version/svo/include/svo/sparse_img_align.h +++ b/GPU_version/svo/include/gpu_svo/sparse_img_align.h @@ -19,7 +19,7 @@ #include #include -#include +#include namespace vk { class AbstractCamera; diff --git a/GPU_version/svo/include/svo/svoVisionFactor.h b/GPU_version/svo/include/gpu_svo/svoVisionFactor.h similarity index 99% rename from GPU_version/svo/include/svo/svoVisionFactor.h rename to GPU_version/svo/include/gpu_svo/svoVisionFactor.h index cd00816..738bd95 100644 --- a/GPU_version/svo/include/svo/svoVisionFactor.h +++ b/GPU_version/svo/include/gpu_svo/svoVisionFactor.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include namespace svo { diff --git a/GPU_version/svo/src/bundle_adjustment.cpp b/GPU_version/svo/src/bundle_adjustment.cpp index eaba2b0..883116f 100644 --- a/GPU_version/svo/src/bundle_adjustment.cpp +++ b/GPU_version/svo/src/bundle_adjustment.cpp @@ -26,12 +26,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #define SCHUR_TRICK 1 diff --git a/GPU_version/svo/src/cl_class.cpp b/GPU_version/svo/src/cl_class.cpp index d32f140..a26d455 100644 --- a/GPU_version/svo/src/cl_class.cpp +++ b/GPU_version/svo/src/cl_class.cpp @@ -1,7 +1,7 @@ // // Created by root on 4/27/21. // -#include "svo/cl_class.h" +#include "gpu_svo/cl_class.h" opencl::opencl() { std::vector all_platforms; cl::Platform::get(&all_platforms); @@ -12,7 +12,7 @@ opencl::opencl() { //get default device of the default platform std::vector all_devices; for(auto i:all_platforms){ - std::cout << "Find platform: " << i.getInfo() << "\n"; + std::cout << "Find platform number:"<< i.getInfo() << "\n"; i.getDevices(CL_DEVICE_TYPE_GPU, &all_devices); } @@ -30,7 +30,7 @@ opencl::opencl() { context=new cl::Context({ device }); cl::Program::Sources sources; - read_cl fast("../kernel/fast-gray.cl"); + read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl"); sources.push_back({ fast.src_str, fast.size }); program=new cl::Program(*context, sources); if(program->build({ device },"-DFAST_THRESH=40") !=0) diff --git a/GPU_version/svo/src/config.cpp b/GPU_version/svo/src/config.cpp index e8475fe..2f09b60 100644 --- a/GPU_version/svo/src/config.cpp +++ b/GPU_version/svo/src/config.cpp @@ -17,7 +17,7 @@ #ifdef SVO_USE_ROS #include #endif -#include +#include namespace svo { diff --git a/GPU_version/svo/src/depth_filter.cpp b/GPU_version/svo/src/depth_filter.cpp index db24408..f6fa977 100644 --- a/GPU_version/svo/src/depth_filter.cpp +++ b/GPU_version/svo/src/depth_filter.cpp @@ -20,14 +20,14 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include namespace svo { diff --git a/GPU_version/svo/src/feature_alignment.cpp b/GPU_version/svo/src/feature_alignment.cpp index faa4027..3605333 100644 --- a/GPU_version/svo/src/feature_alignment.cpp +++ b/GPU_version/svo/src/feature_alignment.cpp @@ -20,7 +20,7 @@ #ifdef __ARM_NEON__ #include #endif -#include +#include namespace svo { namespace feature_alignment { diff --git a/GPU_version/svo/src/feature_detection.cpp b/GPU_version/svo/src/feature_detection.cpp index 8aaac0c..3a98d20 100644 --- a/GPU_version/svo/src/feature_detection.cpp +++ b/GPU_version/svo/src/feature_detection.cpp @@ -14,8 +14,8 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include +#include +#include #include namespace svo { @@ -32,14 +32,6 @@ AbstractDetector::AbstractDetector( grid_n_rows_(ceil(static_cast(img_height)/cell_size_)), grid_occupancy_(grid_n_cols_*grid_n_rows_, false) { - gpu_fast_= new opencl(); - cv::Mat init=cv::Mat::zeros(cv::Size(img_width, img_height),CV_8UC1); - gpu_fast_->write_buf(0,0,init); - cl_int2 corners[img_width*img_height]; - gpu_fast_->write_buf(0,1,img_width*img_height,corners); - int icorner[1]={0}; - gpu_fast_->write_buf(0,2,1,icorner); - gpu_fast_->write_buf(0,3,(1 << 18)); } void AbstractDetector::resetGrid() @@ -67,9 +59,11 @@ FastDetector::FastDetector( const int img_width, const int img_height, const int cell_size, + opencl* gpu_fast_, const int n_pyr_levels) : - AbstractDetector(img_width, img_height, cell_size, n_pyr_levels) -{} + AbstractDetector(img_width, img_height, cell_size, n_pyr_levels),gpu_fast_(gpu_fast_) +{ +} void FastDetector::detect( Frame* frame, @@ -81,26 +75,28 @@ void FastDetector::detect( for(int L=0; Lreload_buf(0,0,img_pyr[L].data); + cv::Mat img=img_pyr.at(L); + gpu_fast_->reload_buf(0,0,img); int icorner[1]={0}; gpu_fast_->reload_buf(0,2,icorner); gpu_fast_->run(0,img_pyr[L].cols,img_pyr[L].rows); - cl_int2* fast_corners; + cl_int2 fast_corners[1000000]; gpu_fast_->read(0,1,fast_corners); int count[1]={0}; gpu_fast_->read(0,2,count); - //vector scores, nm_corners; - //fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores); - //fast::fast_nonmax_3x3(fast_corners, scores, nm_corners); - for(uint i=0;iimg_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0)continue; + if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0) + continue; + const int k = static_cast((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_ + static_cast((fast_corners[i].x*scale)/cell_size_); + if(grid_occupancy_[k]) continue; + const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y); + if(score > corners.at(k).score) corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f); } diff --git a/GPU_version/svo/src/frame.cpp b/GPU_version/svo/src/frame.cpp index 440c32d..632e376 100644 --- a/GPU_version/svo/src/frame.cpp +++ b/GPU_version/svo/src/frame.cpp @@ -15,10 +15,10 @@ // along with this program. If not, see . #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include diff --git a/GPU_version/svo/src/frame_handler_base.cpp b/GPU_version/svo/src/frame_handler_base.cpp index 175ee01..6dcef82 100644 --- a/GPU_version/svo/src/frame_handler_base.cpp +++ b/GPU_version/svo/src/frame_handler_base.cpp @@ -19,12 +19,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace svo { diff --git a/GPU_version/svo/src/frame_handler_mono.cpp b/GPU_version/svo/src/frame_handler_mono.cpp index 799cb8c..1e6a98e 100644 --- a/GPU_version/svo/src/frame_handler_mono.cpp +++ b/GPU_version/svo/src/frame_handler_mono.cpp @@ -14,18 +14,18 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include +#include #ifdef USE_BUNDLE_ADJUSTMENT -#include +#include #endif namespace svo { @@ -36,6 +36,16 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init) reprojector_(cam_, map_), depth_filter_(NULL) { + gpu_fast_= new opencl(); + gpu_fast_->make_kernel("fast_gray"); + cv::Mat img=cv::Mat(cv::Size(1000, 1000),CV_8UC1); + gpu_fast_->write_buf(0,0,img); + cl_int2 corners_[1000000]; + gpu_fast_->write_buf(0,1,1000000,corners_); + int icorner[1]={0}; + gpu_fast_->write_buf(0,2,1,icorner); + gpu_fast_->write_buf(0,3,(1 << 18)); + klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); initialize(); imu_integPtr_=std::make_unique(SE_init); } @@ -44,7 +54,7 @@ void FrameHandlerMono::initialize() { feature_detection::DetectorPtr feature_detector( new feature_detection::FastDetector( - cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels())); + cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels())); DepthFilter::callback_t depth_filter_cb = boost::bind( &MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2); depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb); @@ -90,7 +100,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp) FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() { new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero()); - if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE) + if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE) return RESULT_NO_KEYFRAME; new_frame_->setKeyframe(); map_.addKeyframe(new_frame_); @@ -101,7 +111,7 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() { - initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_); + initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_); if(res == initialization::FAILURE) return RESULT_FAILURE; else if(res == initialization::NO_KEYFRAME) @@ -120,7 +130,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() // add frame to map map_.addKeyframe(new_frame_); stage_ = STAGE_DEFAULT_FRAME; - klt_homography_init_.reset(); + klt_homography_init_->reset(); SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map."); return RESULT_IS_KEYFRAME; } diff --git a/GPU_version/svo/src/imu_integration.cpp b/GPU_version/svo/src/imu_integration.cpp index c2a720f..2238f7c 100644 --- a/GPU_version/svo/src/imu_integration.cpp +++ b/GPU_version/svo/src/imu_integration.cpp @@ -2,16 +2,16 @@ // Created by gfuse on 4/9/21. // //#define NDEBUG -#include "svo/imu_integration.h" +#include "gpu_svo/imu_integration.h" #include #include -#include "svo/feature.h" -#include "svo/point.h" -#include "svo/svoVisionFactor.h" +#include "gpu_svo/feature.h" +#include "gpu_svo/point.h" +#include "gpu_svo/svoVisionFactor.h" #include #include #include -#include +#include Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ graphPtr=std::make_shared(); diff --git a/GPU_version/svo/src/initialization.cpp b/GPU_version/svo/src/initialization.cpp index 571a0bc..ce04d24 100644 --- a/GPU_version/svo/src/initialization.cpp +++ b/GPU_version/svo/src/initialization.cpp @@ -14,12 +14,12 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -29,7 +29,7 @@ namespace initialization { InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref) { reset(); - detectFeatures(frame_ref, px_ref_, f_ref_); + detectFeatures(frame_ref, px_ref_, f_ref_,gpu_fast_); if(px_ref_.size() < 100) { SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment."); @@ -107,11 +107,12 @@ void KltHomographyInit::reset() void detectFeatures( FramePtr frame, vector& px_vec, - vector& f_vec) + vector& f_vec, + opencl* gpu_fast) { Features new_features; feature_detection::FastDetector detector( - frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels()); + frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels()); detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features); // now for all maximum corners, initialize a new seed diff --git a/GPU_version/svo/src/map.cpp b/GPU_version/svo/src/map.cpp index 2ecad1c..90c7416 100644 --- a/GPU_version/svo/src/map.cpp +++ b/GPU_version/svo/src/map.cpp @@ -15,10 +15,10 @@ // along with this program. If not, see . #include -#include -#include -#include -#include +#include +#include +#include +#include #include namespace svo { diff --git a/GPU_version/svo/src/matcher.cpp b/GPU_version/svo/src/matcher.cpp index 38a89fb..9f44701 100644 --- a/GPU_version/svo/src/matcher.cpp +++ b/GPU_version/svo/src/matcher.cpp @@ -19,12 +19,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace svo { diff --git a/GPU_version/svo/src/point.cpp b/GPU_version/svo/src/point.cpp index 2e45e06..533bed4 100644 --- a/GPU_version/svo/src/point.cpp +++ b/GPU_version/svo/src/point.cpp @@ -16,9 +16,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace svo { diff --git a/GPU_version/svo/src/pose_optimizer.cpp b/GPU_version/svo/src/pose_optimizer.cpp index 1c491b0..8af359f 100644 --- a/GPU_version/svo/src/pose_optimizer.cpp +++ b/GPU_version/svo/src/pose_optimizer.cpp @@ -15,10 +15,10 @@ // along with this program. If not, see . #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/GPU_version/svo/src/reprojector.cpp b/GPU_version/svo/src/reprojector.cpp index 81a7835..888d3d2 100644 --- a/GPU_version/svo/src/reprojector.cpp +++ b/GPU_version/svo/src/reprojector.cpp @@ -16,12 +16,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include diff --git a/GPU_version/svo/src/sparse_img_align.cpp b/GPU_version/svo/src/sparse_img_align.cpp index ce1570b..21432c6 100644 --- a/GPU_version/svo/src/sparse_img_align.cpp +++ b/GPU_version/svo/src/sparse_img_align.cpp @@ -15,11 +15,11 @@ // along with this program. If not, see . #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/GPU_version/vio/.idea/.gitignore b/GPU_version/vio/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/GPU_version/vio/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/GPU_version/vio/.idea/misc.xml b/GPU_version/vio/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/GPU_version/vio/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/GPU_version/vio/.idea/modules.xml b/GPU_version/vio/.idea/modules.xml new file mode 100644 index 0000000..eb3cacf --- /dev/null +++ b/GPU_version/vio/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/GPU_version/vio/.idea/vcs.xml b/GPU_version/vio/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/GPU_version/vio/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt new file mode 100644 index 0000000..a9872a4 --- /dev/null +++ b/GPU_version/vio/CMakeLists.txt @@ -0,0 +1,92 @@ +SET(PROJECT_NAME vio_gpu) +PROJECT(${PROJECT_NAME}) +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 build flags. Set IS_ARM on odroid board as environment variable +SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") +IF(DEFINED ENV{ARM_ARCHITECTURE}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") +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 catkin and required ROS packages +FIND_PACKAGE(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp + cmake_modules + nav_msgs + std_msgs + visualization_msgs + sensor_msgs + image_transport + cv_bridge + tf + gpu_svo + vikit_common + vikit_ros +) + +# Add plain cmake packages +FIND_PACKAGE(OpenCV REQUIRED) +FIND_PACKAGE(Eigen REQUIRED) +FIND_PACKAGE(Sophus REQUIRED) +FIND_PACKAGE(Boost REQUIRED COMPONENTS thread) +FIND_PACKAGE(GTSAM REQUIRED) +FIND_PACKAGE(OpenCL REQUIRED) + +# notify catkin to look at the previously found msg files +add_message_files( + FILES + Info.msg +) + +# build the header files from the msg files, and notify catkin about the dependencies +generate_messages( + DEPENDENCIES + geometry_msgs + sensor_msgs +) +# Describe catkin Project +catkin_package( + DEPENDS Eigen OpenCV Sophus Boost + CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf gpu_svo vikit_common vikit_ros geometry_msgs + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME}_visualizer +) + +# Include dirs +INCLUDE_DIRECTORIES( + include + ${Eigen_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries +LIST(APPEND LINK_LIBS + ${OpenCV_LIBS} + ${Sophus_LIBRARIES} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${OpenCL_LIBRARIES} +) + +# Create visualizer library +ADD_LIBRARY(${PROJECT_NAME}_visualizer src/visualizer.cpp) +TARGET_LINK_LIBRARIES(${PROJECT_NAME}_visualizer ${LINK_LIBS} gtsam) + +# Create Executables +ADD_EXECUTABLE(${PROJECT_NAME} src/vo_node.cpp) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_visualizer ${LINK_LIBS}) diff --git a/GPU_version/vio/include/vio_gpu/visualizer.h b/GPU_version/vio/include/vio_gpu/visualizer.h new file mode 100644 index 0000000..0ff8834 --- /dev/null +++ b/GPU_version/vio/include/vio_gpu/visualizer.h @@ -0,0 +1,73 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef SVO_VISUALIZER_H_ +#define SVO_VISUALIZER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +class Frame; +class Point; +class Map; +class FrameHandlerMono; + +typedef boost::shared_ptr FramePtr; + +/// This class bundles all functions to publish visualisation messages. +class Visualizer +{ +public: + ros::NodeHandle pnh_; + size_t trace_id_; + ros::Publisher pub_frames_; + ros::Publisher pub_points_; + ros::Publisher pub_pose_; + ros::Publisher pub_info_; + tf::TransformBroadcaster br_; + SE3 T_world_from_vision_; + + Visualizer(); + + ~Visualizer() {}; + + void publishMinimal( + const cv::Mat& img, + const FramePtr& frame, + const FrameHandlerMono& slam, + const double timestamp); + + void visualizeMarkers( + const FramePtr& frame, + const set& core_kfs, + const Map& map); + + void removeDeletedPts(const Map& map); + +}; + +} // end namespace svo + +#endif /* SVO_VISUALIZER_H_ */ diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch new file mode 100644 index 0000000..f57f8c0 --- /dev/null +++ b/GPU_version/vio/launch/Px30.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/GPU_version/vio/launch/live.launch b/GPU_version/vio/launch/live.launch new file mode 100644 index 0000000..7cd0ad8 --- /dev/null +++ b/GPU_version/vio/launch/live.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/GPU_version/vio/launch/test.launch b/GPU_version/vio/launch/test.launch new file mode 100644 index 0000000..4bcd965 --- /dev/null +++ b/GPU_version/vio/launch/test.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/GPU_version/vio/launch/unity.launch b/GPU_version/vio/launch/unity.launch new file mode 100644 index 0000000..ea34997 --- /dev/null +++ b/GPU_version/vio/launch/unity.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/GPU_version/vio/msg/Info.msg b/GPU_version/vio/msg/Info.msg new file mode 100644 index 0000000..b962e1b --- /dev/null +++ b/GPU_version/vio/msg/Info.msg @@ -0,0 +1,6 @@ +Header header +float32 processing_time +int32[] keyframes +int32 num_matches +int32 tracking_quality +int32 stage \ No newline at end of file diff --git a/GPU_version/vio/package.xml b/GPU_version/vio/package.xml new file mode 100644 index 0000000..7f8fbc5 --- /dev/null +++ b/GPU_version/vio/package.xml @@ -0,0 +1,43 @@ + + + vio_gpu + 0.1.0 + + Monocular Visual Odometry - ROS Nodes + + Majid Geravand + Majid Geravand + GPLv3 + + + catkin + + + roscpp + cmake_modules + nav_msgs + std_msgs + visualization_msgs + sensor_msgs + image_transport + cv_bridge + tf + gpu_svo + vikit_common + vikit_ros + geometry_msgs + + + roscpp + nav_msgs + std_msgs + visualization_msgs + sensor_msgs + image_transport + cv_bridge + tf + vikit_common + vikit_ros + geometry_msgs + gpu_svo + \ No newline at end of file diff --git a/GPU_version/vio/param/camera_atan.yaml b/GPU_version/vio/param/camera_atan.yaml new file mode 100644 index 0000000..e3c625c --- /dev/null +++ b/GPU_version/vio/param/camera_atan.yaml @@ -0,0 +1,8 @@ +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 new file mode 100644 index 0000000..506a856 --- /dev/null +++ b/GPU_version/vio/param/camera_pinhole.yaml @@ -0,0 +1,11 @@ +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/px30.yaml b/GPU_version/vio/param/px30.yaml new file mode 100644 index 0000000..10fa932 --- /dev/null +++ b/GPU_version/vio/param/px30.yaml @@ -0,0 +1,8 @@ +cam_model: ATAN +cam_width: 640 +cam_height: 480 +cam_fx: 0.463632 +cam_fy: 0.823661 +cam_cx: 0.497465 +cam_cy: 0.498443 +cam_d0: 1.01687 \ No newline at end of file diff --git a/GPU_version/vio/param/unity.yaml b/GPU_version/vio/param/unity.yaml new file mode 100644 index 0000000..023f378 --- /dev/null +++ b/GPU_version/vio/param/unity.yaml @@ -0,0 +1,8 @@ +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 new file mode 100644 index 0000000..8446875 --- /dev/null +++ b/GPU_version/vio/param/vo_accurate.yaml @@ -0,0 +1,4 @@ +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 new file mode 100644 index 0000000..e33b18a --- /dev/null +++ b/GPU_version/vio/param/vo_fast.yaml @@ -0,0 +1,13 @@ +svo: + grid_size: 30 #Feature grid size of a cell in [px]. + max_n_kfs: 10 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3. + 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 + 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 diff --git a/GPU_version/vio/rqt.perspective b/GPU_version/vio/rqt.perspective new file mode 100644 index 0000000..66dd7ef --- /dev/null +++ b/GPU_version/vio/rqt.perspective @@ -0,0 +1,293 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'rqt_rviz/RViz': [1], u'rqt_reconfigure/Param': [1], u'rqt_image_view/ImageView': [1], u'svo_rqt/Svo': [1]}" + } + }, + "groups": { + "plugin__rqt_rviz__RViz__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_robot_steering__RobotSteering__1": { + "keys": {}, + "groups": { + "dock_widget__RobotSteeringUi": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "topic": { + "type": "repr", + "repr": "u'/cmd_vel'" + }, + "vx_min": { + "type": "repr", + "repr": "u'-1'" + }, + "vw_min": { + "type": "repr", + "repr": "u'-3'" + }, + "vw_max": { + "type": "repr", + "repr": "u'3'" + }, + "vx_max": { + "type": "repr", + "repr": "u'1'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_reconfigure__Param__1": { + "keys": {}, + "groups": { + "dock_widget___plugincontainer_top_widget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000c00000006401000000060100000001')", + "pretty-print": " d " + }, + "_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000013e0000006401000000090100000002')", + "pretty-print": " > d " + } + }, + "groups": {} + } + } + }, + "plugin__rqt_image_view__ImageView__1": { + "keys": {}, + "groups": { + "dock_widget__ImageViewWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "dynamic_range": { + "type": "repr", + "repr": "False" + }, + "max_range": { + "type": "repr", + "repr": "10.0" + }, + "topic": { + "type": "repr", + "repr": "u'/svo/image'" + }, + "zoom1": { + "type": "repr", + "repr": "False" + } + }, + "groups": {} + } + } + }, + "plugin__svo_rqt__Svo__1": { + "keys": {}, + "groups": { + "dock_widget__SvoWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + }, + "dock_widget__SvoUi": { + "keys": { + "dockable": { + "type": "repr", + "repr": "u'true'" + }, + "parent": { + "type": "repr", + "repr": "None" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_shell__Shell__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "shell_type": { + "type": "repr", + "repr": "u'0'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_console__Console__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "show_highlighted_only": { + "type": "repr", + "repr": "u'false'" + }, + "message_limit": { + "type": "repr", + "repr": "u'20000'" + }, + "exclude_filters": { + "type": "repr", + "repr": "u'severity'" + }, + "table_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000004e000000ed01000000090100000002')", + "pretty-print": " N " + }, + "highlight_filters": { + "type": "repr", + "repr": "u'message'" + }, + "paused": { + "type": "repr", + "repr": "u'false'" + }, + "settings_exist": { + "type": "repr", + "repr": "u'true'" + }, + "filter_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000720000007201000000090100000002')", + "pretty-print": " r r " + } + }, + "groups": { + "exclude_filter_0": { + "keys": { + "enabled": { + "type": "repr", + "repr": "u'true'" + }, + "itemlist": { + "type": "repr", + "repr": "u''" + } + }, + "groups": {} + }, + "highlight_filter_0": { + "keys": { + "regex": { + "type": "repr", + "repr": "u'false'" + }, + "text": { + "type": "repr", + "repr": "u''" + }, + "enabled": { + "type": "repr", + "repr": "u'true'" + } + }, + "groups": {} + } + } + } + } + } + } + }, + "mainwindow": { + "keys": { + "geometry": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00010000000007a9fffffffc00001187000005a7000007b1000000180000117f0000059f000000000000')", + "pretty-print": " " + }, + "state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000009cf00000559fc0100000005fb0000002c00730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f005500690100000000000003890000000000000000fb0000006a007200710074005f0072006f0062006f0074005f007300740065006500720069006e0067005f005f0052006f0062006f0074005300740065006500720069006e0067005f005f0031005f005f0052006f0062006f0074005300740065006500720069006e00670055006901000000000000020b0000000000000000fc00000000000003530000022e00fffffffc0200000004fb0000003400730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f0057006900640067006500740100000019000000a30000009900fffffffb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c006500570069006400670065007401000000c3000003b50000000000000000fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000c2000002700000005100fffffffb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f00770069006400670065007401000003380000023a0000010f00fffffffb00000026007200710074005f007200760069007a005f005f005200560069007a005f005f0031005f005f010000035900000676000000a700fffffffb00000040007200710074005f007300680065006c006c005f005f005300680065006c006c005f005f0031005f005f0058005400650072006d005700690064006700650074010000091c000000b30000000000000000000009cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "pretty-print": " ,svo_rqt__Svo__1__SvoUi jrqt_robot_steering__RobotSteering__1__RobotSteeringUi S . p Q 8 : Y v " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} \ No newline at end of file diff --git a/GPU_version/vio/rviz_config.rviz b/GPU_version/vio/rviz_config.rviz new file mode 100644 index 0000000..95d7c0e --- /dev/null +++ b/GPU_version/vio/rviz_config.rviz @@ -0,0 +1,153 @@ +Panels: + - Class: rviz/Displays + Help Height: 108 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /Marker1 + - /Marker2 + - /Marker2/Namespaces1 + - /Image1 + Splitter Ratio: 0.5 + Tree Height: 680 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /svo/keyframes + Name: Marker + Namespaces: + cams: true + kfs: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /svo/points + Name: Marker + Namespaces: + pts: true + trajectory: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /svo/image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Fixed Frame: /world + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 5.48911 + Focal Point: + X: 0.777511 + Y: 0.689927 + Z: -0.445833 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.125204 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.45215 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1416 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002cc00000542fc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000355000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000383000001e70000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ce0000003efc0100000002fb0000000800540069006d00650000000000000003ce000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000006fd0000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2511 + X: 1962 + Y: 24 diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp new file mode 100644 index 0000000..d2327f4 --- /dev/null +++ b/GPU_version/vio/src/visualizer.cpp @@ -0,0 +1,127 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace svo { + +Visualizer:: +Visualizer() : + pnh_("~"), + trace_id_(0), + T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()) +{ + // Init ROS Marker Publishers + pub_frames_ = pnh_.advertise("keyframes", 10); + pub_points_ = pnh_.advertise("points", 1000); + pub_pose_ = pnh_.advertise("pose",10); + pub_info_ = pnh_.advertise("info", 10); +} + +void Visualizer::publishMinimal( + const cv::Mat& img, + const FramePtr& frame, + const FrameHandlerMono& slam, + const double timestamp) +{ + ++trace_id_; + std_msgs::Header header_msg; + header_msg.frame_id = "/cam"; + header_msg.seq = trace_id_; + header_msg.stamp = ros::Time(timestamp); + + // publish info msg. + if(pub_info_.getNumSubscribers() > 0) + { + vio_gpu::Info msg_info; + msg_info.header = header_msg; + 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) + { + Quaterniond q; + Vector3d p; + Matrix Cov; + // publish cam in world frame (Estimated odometry in the worls frame) + SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera + q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose()); + p = T_world_from_cam.translation(); + Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj(); + geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped); + msg_pose->header = header_msg; + msg_pose->pose.pose.position.x = p[0]; + msg_pose->pose.pose.position.y = p[1]; + msg_pose->pose.pose.position.z = p[2]; + msg_pose->pose.pose.orientation.x = q.x(); + msg_pose->pose.pose.orientation.y = q.y(); + msg_pose->pose.pose.orientation.z = q.z(); + msg_pose->pose.pose.orientation.w = q.w(); + for(size_t i=0; i<36; ++i) + msg_pose->pose.covariance[i] = Cov(i%6, i/6); + pub_pose_.publish(msg_pose); + } +} + +void Visualizer::visualizeMarkers( + const FramePtr& frame, + const set& core_kfs, + const Map& map) +{ + if(frame == NULL) + return; + + vk::output_helper::publishTfTransform( + frame->T_f_w_*T_world_from_vision_.inverse(), + ros::Time(frame->timestamp_), "cam_pos", "world", br_); + + if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0) + { + vk::output_helper::publishPointMarker( + pub_points_, T_world_from_vision_*frame->pos(), "trajectory", + ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5)); + removeDeletedPts(map); + } +} + +void Visualizer::removeDeletedPts(const Map& map) +{ + if(pub_points_.getNumSubscribers() > 0) + { + for(list::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it) + vk::output_helper::publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d()); + } +} + +} // end namespace gpu_svo diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp new file mode 100644 index 0000000..c11c6a0 --- /dev/null +++ b/GPU_version/vio/src/vo_node.cpp @@ -0,0 +1,148 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace svo { + +/// SVO Interface +class VoNode +{ +public: + svo::FrameHandlerMono* vo_; + svo::Visualizer visualizer_; + bool publish_markers_; //!< publish only the minimal amount of info (choice for embedded devices) + vk::AbstractCamera* cam_; + FILE* logtime; + bool quit_; + boost::thread* imu_th; + VoNode(); + ~VoNode(); + void imgCb(const sensor_msgs::ImageConstPtr& msg); + void imuCb(const sensor_msgs::ImuPtr& imu); + void imu(); +private: + double* imu_; +}; + +VoNode::VoNode() : + vo_(NULL), + publish_markers_(vk::getParam("vio/publish_markers", true)), + cam_(NULL), + quit_(false) +{ + // Create Camera + imu_=(double *)calloc(static_cast(6), sizeof(double)); + if(!vk::camera_loader::loadFromRosNs("vio", cam_)) + throw std::runtime_error("Camera model not correctly specified."); + // Get initial position and orientation + visualizer_.T_world_from_vision_ = Sophus::SE3( + vk::rpy2dcm(Vector3d(vk::getParam("vio/init_rx", 0.0), + vk::getParam("vio/init_ry", 0.0), + vk::getParam("vio/init_rz", 0.0))), + Eigen::Vector3d(vk::getParam("vio/init_tx", 0.0), + vk::getParam("vio/init_ty", 0.0), + vk::getParam("vio/init_tz", 0.0))); + // Init VO and start + vo_ = new svo::FrameHandlerMono(cam_,visualizer_.T_world_from_vision_); + vo_->start(); + imu_th=new boost::thread(&VoNode::imu,this); + logtime = fopen((std::string(getenv("HOME"))+"/time.txt").c_str(), "w+"); + +} + +VoNode::~VoNode() +{ + quit_=true; + fclose(logtime); + imu_th->join(); + delete imu_th; + delete logtime; + delete vo_; + delete cam_; +} + +void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) +{ + try { + auto start=std::chrono::steady_clock::now(); + cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image; + vo_->addImage(img, msg->header.stamp.toSec()); + visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec()); + if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED) + visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map()); + if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED) + usleep(100000); + fprintf(logtime, "%s\n",std::to_string(std::chrono::duration(std::chrono::steady_clock::now()-start).count()).c_str()); + + } catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + } +} +void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) { + double imu_in[6]; + imu_in[0] = 0.1*imu_[0]+0.9*imu->linear_acceleration.x; + imu_in[1] = 0.1*imu_[1]+0.9*imu->linear_acceleration.y; + imu_in[2] = 0.1*imu_[2]+0.9*imu->linear_acceleration.z; + imu_in[3] = 0.1*imu_[3]+0.9*imu->angular_velocity.x; + imu_in[4] = 0.1*imu_[4]+0.9*imu->angular_velocity.y; + imu_in[5] = 0.1*imu_[5]+0.9*imu->angular_velocity.z; + memcpy(imu_, imu_in, static_cast(6*sizeof(double))); + vo_->imu_integPtr_->update(imu_in); +} +void VoNode::imu(){ + ros::NodeHandle nh; + // subscribe to cam msgs + image_transport::ImageTransport it(nh); + image_transport::Subscriber it_sub = it.subscribe(vk::getParam("vio/cam_topic", "camera/image_raw"), 1, &svo::VoNode::imgCb, this); + // start processing callbacks + while(ros::ok() && !quit_) + { + ros::spinOnce(); + } + +} +} // namespace svo + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "vio"); + svo::VoNode vo_node; + ros::NodeHandle nh; + ros::Subscriber imu_sub=nh.subscribe(vk::getParam("vio/imu_topic", "imu/raw"),1,&svo::VoNode::imuCb, &vo_node); + // start processing callbacks + while(ros::ok() && !vo_node.quit_) + { + ros::spinOnce(); + usleep(1000); + // TODO check when last image was processed. when too long ago. publish warning that no msgs are received! + } + printf("SVO terminated.\n"); + return 0; +}