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

debug gpu version done

Browse Source
This commit is contained in:
gfuse
2021-05-08 11:53:15 +02:00
parent 5a693259d6
commit 9dbc540a5d
60 changed files with 1249 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/svo/CMakeLists.txt
+1
View File
@@ -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\")
GPU_version/svo/include/svo/bundle_adjustment.h → GPU_version/svo/include/gpu_svo/bundle_adjustment.h
+1 -1
View File
@@ -17,7 +17,7 @@
#ifndef SVO_BUNDLE_ADJUSTMENT_H_
#define SVO_BUNDLE_ADJUSTMENT_H_
#include <svo/global.h>
#include <gpu_svo/global.h>
namespace g2o {
class EdgeProjectXYZ2UV;
GPU_version/svo/include/svo/cl_class.h → GPU_version/svo/include/gpu_svo/cl_class.h
+4 -2
View File
@@ -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<uchar*>(buf.data));
}
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
@@ -127,7 +129,7 @@ public:
_kernels.at(id1).write(id2,buf,context);
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Write buffer on GPU failed\n"<<err.what()<<'\n';
std::cerr<<"Write image on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
}
}
GPU_version/svo/include/svo/config.h → GPU_version/svo/include/gpu_svo/config.h
View File
GPU_version/svo/include/svo/depth_filter.h → GPU_version/svo/include/gpu_svo/depth_filter.h
+3 -3
View File
@@ -21,9 +21,9 @@
#include <boost/thread.hpp>
#include <boost/function.hpp>
#include <vikit/performance_monitor.h>
#include <svo/global.h>
#include <svo/feature_detection.h>
#include <svo/matcher.h>
#include <gpu_svo/global.h>
#include <gpu_svo/feature_detection.h>
#include <gpu_svo/matcher.h>
namespace svo {
GPU_version/svo/include/svo/feature.h → GPU_version/svo/include/gpu_svo/feature.h
+1 -1
View File
@@ -17,7 +17,7 @@
#ifndef SVO_FEATURE_H_
#define SVO_FEATURE_H_
#include <svo/frame.h>
#include <gpu_svo/frame.h>
namespace svo {
GPU_version/svo/include/svo/feature_alignment.h → GPU_version/svo/include/gpu_svo/feature_alignment.h
+1 -1
View File
@@ -17,7 +17,7 @@
#ifndef SVO_FEATURE_ALIGNMENT_H_
#define SVO_FEATURE_ALIGNMENT_H_
#include <svo/global.h>
#include <gpu_svo/global.h>
namespace svo {
GPU_version/svo/include/svo/feature_detection.h → GPU_version/svo/include/gpu_svo/feature_detection.h
+5 -6
View File
@@ -17,9 +17,9 @@
#ifndef SVO_FEATURE_DETECTION_H_
#define SVO_FEATURE_DETECTION_H_
#include <svo/global.h>
#include <svo/frame.h>
#include <svo/cl_class.h>
#include <gpu_svo/global.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/cl_class.h>
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
GPU_version/svo/include/svo/frame.h → GPU_version/svo/include/gpu_svo/frame.h
+1 -1
View File
@@ -21,7 +21,7 @@
#include <vikit/math_utils.h>
#include <vikit/abstract_camera.h>
#include <boost/noncopyable.hpp>
#include <svo/global.h>
#include <gpu_svo/global.h>
namespace g2o {
class VertexSE3Expmap;
GPU_version/svo/include/svo/frame_handler_base.h → GPU_version/svo/include/gpu_svo/frame_handler_base.h
+2 -2
View File
@@ -23,8 +23,8 @@
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <svo/global.h>
#include <svo/map.h>
#include <gpu_svo/global.h>
#include <gpu_svo/map.h>
namespace vk
{
GPU_version/svo/include/svo/frame_handler_mono.h → GPU_version/svo/include/gpu_svo/frame_handler_mono.h
+9 -8
View File
@@ -19,10 +19,11 @@
#include <set>
#include <vikit/abstract_camera.h>
#include <svo/frame_handler_base.h>
#include <svo/reprojector.h>
#include <svo/initialization.h>
#include <svo/imu_integration.h>
#include <gpu_svo/frame_handler_base.h>
#include <gpu_svo/reprojector.h>
#include <gpu_svo/initialization.h>
#include <gpu_svo/imu_integration.h>
#include <gpu_svo/cl_class.h>
namespace svo {
@@ -48,8 +49,8 @@ public:
const set<FramePtr>& coreKeyframes() { return core_kfs_; }
/// Return the feature track to visualize the KLT tracking during initialization.
const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; }
const vector<cv::Point2f>& 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<FramePtr> core_kfs_; //!< Keyframes in the closer neighbourhood.
vector< pair<FramePtr,size_t> > 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();
GPU_version/svo/include/svo/global.h → GPU_version/svo/include/gpu_svo/global.h
View File
GPU_version/svo/include/svo/imu_integration.h → GPU_version/svo/include/gpu_svo/imu_integration.h
+1 -1
View File
@@ -13,7 +13,7 @@
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <sophus/se3.h>
#include <svo/frame.h>
#include <gpu_svo/frame.h>
using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot)
GPU_version/svo/include/svo/initialization.h → GPU_version/svo/include/gpu_svo/initialization.h
+6 -3
View File
@@ -17,7 +17,8 @@
#ifndef SVO_INITIALIZATION_H
#define SVO_INITIALIZATION_H
#include <svo/global.h>
#include <gpu_svo/global.h>
#include <gpu_svo/cl_class.h>
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<int> inliers_; //!< inliers after the geometric check (e.g., Homography).
vector<Vector3d> 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<cv::Point2f>& px_vec,
vector<Vector3d>& f_vec);
vector<Vector3d>& f_vec,
opencl* gpu_fast);
/// Compute optical flow (Lucas Kanade) for selected keypoints.
void trackKlt(
GPU_version/svo/include/svo/map.h → GPU_version/svo/include/gpu_svo/map.h
+1 -1
View File
@@ -20,7 +20,7 @@
#include <queue>
#include <boost/noncopyable.hpp>
#include <boost/thread.hpp>
#include <svo/global.h>
#include <gpu_svo/global.h>
namespace svo {
GPU_version/svo/include/svo/matcher.h → GPU_version/svo/include/gpu_svo/matcher.h
+1 -1
View File
@@ -17,7 +17,7 @@
#ifndef SVO_MATCHER_H_
#define SVO_MATCHER_H_
#include <svo/global.h>
#include <gpu_svo/global.h>
namespace vk {
class AbstractCamera;
GPU_version/svo/include/svo/point.h → GPU_version/svo/include/gpu_svo/point.h
+1 -1
View File
@@ -18,7 +18,7 @@
#define SVO_POINT_H_
#include <boost/noncopyable.hpp>
#include <svo/global.h>
#include <gpu_svo/global.h>
namespace g2o {
class VertexSBAPointXYZ;
GPU_version/svo/include/svo/pose_optimizer.h → GPU_version/svo/include/gpu_svo/pose_optimizer.h
+1 -1
View File
@@ -17,7 +17,7 @@
#ifndef SVO_POSE_OPTIMIZER_H_
#define SVO_POSE_OPTIMIZER_H_
#include <svo/global.h>
#include <gpu_svo/global.h>
namespace svo {
GPU_version/svo/include/svo/reprojector.h → GPU_version/svo/include/gpu_svo/reprojector.h
+2 -2
View File
@@ -17,8 +17,8 @@
#ifndef SVO_REPROJECTION_H_
#define SVO_REPROJECTION_H_
#include <svo/global.h>
#include <svo/matcher.h>
#include <gpu_svo/global.h>
#include <gpu_svo/matcher.h>
namespace vk {
class AbstractCamera;
GPU_version/svo/include/svo/sparse_img_align.h → GPU_version/svo/include/gpu_svo/sparse_img_align.h
+1 -1
View File
@@ -19,7 +19,7 @@
#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <svo/global.h>
#include <gpu_svo/global.h>
namespace vk {
class AbstractCamera;
GPU_version/svo/include/svo/svoVisionFactor.h → GPU_version/svo/include/gpu_svo/svoVisionFactor.h
+1 -1
View File
@@ -12,7 +12,7 @@
#include <gtsam/geometry/Pose3.h>
#include <vikit/robust_cost.h>
#include <vikit/math_utils.h>
#include <svo/frame.h>
#include <gpu_svo/frame.h>
namespace svo {
GPU_version/svo/src/bundle_adjustment.cpp
+6 -6
View File
@@ -26,12 +26,12 @@
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#include <svo/bundle_adjustment.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.h>
#include <svo/map.h>
#include <gpu_svo/bundle_adjustment.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/config.h>
#include <gpu_svo/map.h>
#define SCHUR_TRICK 1
GPU_version/svo/src/cl_class.cpp
+3 -3
View File
@@ -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<cl::Platform> all_platforms;
cl::Platform::get(&all_platforms);
@@ -12,7 +12,7 @@ opencl::opencl() {
//get default device of the default platform
std::vector<cl::Device> all_devices;
for(auto i:all_platforms){
std::cout << "Find platform: " << i.getInfo<CL_PLATFORM_NAME>() << "\n";
std::cout << "Find platform number:"<< i.getInfo<CL_PLATFORM_NAME>() << "\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)
GPU_version/svo/src/config.cpp
+1 -1
View File
@@ -17,7 +17,7 @@
#ifdef SVO_USE_ROS
#include <vikit/params_helper.h>
#endif
#include <svo/config.h>
#include <gpu_svo/config.h>
namespace svo {
GPU_version/svo/src/depth_filter.cpp
+8 -8
View File
@@ -20,14 +20,14 @@
#include <vikit/vision.h>
#include <boost/bind.hpp>
#include <boost/math/distributions/normal.hpp>
#include <svo/global.h>
#include <svo/depth_filter.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/matcher.h>
#include <svo/config.h>
#include <svo/feature_detection.h>
#include <gpu_svo/global.h>
#include <gpu_svo/depth_filter.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/point.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/matcher.h>
#include <gpu_svo/config.h>
#include <gpu_svo/feature_detection.h>
namespace svo {
GPU_version/svo/src/feature_alignment.cpp
+1 -1
View File
@@ -20,7 +20,7 @@
#ifdef __ARM_NEON__
#include <arm_neon.h>
#endif
#include <svo/feature_alignment.h>
#include <gpu_svo/feature_alignment.h>
namespace svo {
namespace feature_alignment {
GPU_version/svo/src/feature_detection.cpp
+15 -19
View File
@@ -14,8 +14,8 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <svo/feature_detection.h>
#include <svo/feature.h>
#include <gpu_svo/feature_detection.h>
#include <gpu_svo/feature.h>
#include <vikit/vision.h>
namespace svo {
@@ -32,14 +32,6 @@ AbstractDetector::AbstractDetector(
grid_n_rows_(ceil(static_cast<double>(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; L<n_pyr_levels_; ++L)
{
const int scale = (1<<L);
gpu_fast_->reload_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<int> 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;i<count[0];++i)
{
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;
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<int>((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_
+ static_cast<int>((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);
}
GPU_version/svo/src/frame.cpp
+4 -4
View File
@@ -15,10 +15,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <stdexcept>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/config.h>
#include <vikit/math_utils.h>
#include <vikit/vision.h>
#include <vikit/performance_monitor.h>
GPU_version/svo/src/frame_handler_base.cpp
+6 -6
View File
@@ -19,12 +19,12 @@
#include <Eigen/StdVector>
#include <boost/bind.hpp>
#include <fstream>
#include <svo/frame_handler_base.h>
#include <svo/config.h>
#include <svo/feature.h>
#include <svo/matcher.h>
#include <svo/map.h>
#include <svo/point.h>
#include <gpu_svo/frame_handler_base.h>
#include <gpu_svo/config.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/matcher.h>
#include <gpu_svo/map.h>
#include <gpu_svo/point.h>
namespace svo
{
GPU_version/svo/src/frame_handler_mono.cpp
+24 -14
View File
@@ -14,18 +14,18 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <svo/config.h>
#include <svo/frame_handler_mono.h>
#include <svo/map.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/pose_optimizer.h>
#include <svo/sparse_img_align.h>
#include <gpu_svo/config.h>
#include <gpu_svo/frame_handler_mono.h>
#include <gpu_svo/map.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/pose_optimizer.h>
#include <gpu_svo/sparse_img_align.h>
#include <vikit/performance_monitor.h>
#include <svo/depth_filter.h>
#include <gpu_svo/depth_filter.h>
#ifdef USE_BUNDLE_ADJUSTMENT
#include <svo/bundle_adjustment.h>
#include <gpu_svo/bundle_adjustment.h>
#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<Imu_Integration>(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;
}
GPU_version/svo/src/imu_integration.cpp
+5 -5
View File
@@ -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 <memory>
#include <utility>
#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 <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose3.h>
#include <vikit/params_helper.h>
#include <svo/config.h>
#include <gpu_svo/config.h>
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
GPU_version/svo/src/initialization.cpp
+10 -9
View File
@@ -14,12 +14,12 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <svo/config.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/initialization.h>
#include <svo/feature_detection.h>
#include <gpu_svo/config.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/point.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/initialization.h>
#include <gpu_svo/feature_detection.h>
#include <vikit/math_utils.h>
#include <vikit/homography.h>
@@ -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<cv::Point2f>& px_vec,
vector<Vector3d>& f_vec)
vector<Vector3d>& 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
GPU_version/svo/src/map.cpp
+4 -4
View File
@@ -15,10 +15,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <set>
#include <svo/map.h>
#include <svo/point.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <gpu_svo/map.h>
#include <gpu_svo/point.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <boost/bind.hpp>
namespace svo {
GPU_version/svo/src/matcher.cpp
+6 -6
View File
@@ -19,12 +19,12 @@
#include <vikit/vision.h>
#include <vikit/math_utils.h>
#include <vikit/patch_score.h>
#include <svo/matcher.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.h>
#include <svo/feature_alignment.h>
#include <gpu_svo/matcher.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/config.h>
#include <gpu_svo/feature_alignment.h>
namespace svo {
GPU_version/svo/src/point.cpp
+3 -3
View File
@@ -16,9 +16,9 @@
#include <stdexcept>
#include <vikit/math_utils.h>
#include <svo/point.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
namespace svo {
GPU_version/svo/src/pose_optimizer.cpp
+4 -4
View File
@@ -15,10 +15,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <stdexcept>
#include <svo/pose_optimizer.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <gpu_svo/pose_optimizer.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <vikit/robust_cost.h>
#include <vikit/math_utils.h>
GPU_version/svo/src/reprojector.cpp
+6 -6
View File
@@ -16,12 +16,12 @@
#include <algorithm>
#include <stdexcept>
#include <svo/reprojector.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/map.h>
#include <svo/config.h>
#include <gpu_svo/reprojector.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/point.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/map.h>
#include <gpu_svo/config.h>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <vikit/abstract_camera.h>
GPU_version/svo/src/sparse_img_align.cpp
+5 -5
View File
@@ -15,11 +15,11 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <svo/sparse_img_align.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/config.h>
#include <svo/point.h>
#include <gpu_svo/sparse_img_align.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/config.h>
#include <gpu_svo/point.h>
#include <vikit/abstract_camera.h>
#include <vikit/vision.h>
#include <vikit/math_utils.h>
GPU_version/vio/.idea/.gitignore
Generated
+8
View File
@@ -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/
GPU_version/vio/.idea/misc.xml
Generated
+4
View File
@@ -0,0 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
</project>
GPU_version/vio/.idea/modules.xml
Generated
+8
View File
@@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/svo_ros.iml" filepath="$PROJECT_DIR$/.idea/svo_ros.iml" />
</modules>
</component>
</project>
GPU_version/vio/.idea/vcs.xml
Generated
+6
View File
@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$/.." vcs="Git" />
</component>
</project>
GPU_version/vio/CMakeLists.txt
+92
View File
@@ -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})
GPU_version/vio/include/vio_gpu/visualizer.h
+73
View File
@@ -0,0 +1,73 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// 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 <http://www.gnu.org/licenses/>.
#ifndef SVO_VISUALIZER_H_
#define SVO_VISUALIZER_H_
#include <queue>
#include <ros/ros.h>
#include <gpu_svo/global.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_broadcaster.h>
#include <image_transport/image_transport.h>
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
namespace svo {
class Frame;
class Point;
class Map;
class FrameHandlerMono;
typedef boost::shared_ptr<Frame> 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<FramePtr>& core_kfs,
const Map& map);
void removeDeletedPts(const Map& map);
};
} // end namespace svo
#endif /* SVO_VISUALIZER_H_ */
GPU_version/vio/launch/Px30.launch
+22
View File
@@ -0,0 +1,22 @@
<launch>
<node pkg="vio_gpu" type="vio_gpu" name="vio" clear_params="true" output="screen">
<!-- Camera topic to subscribe to -->
<param name="cam_topic" value="/CoreNode/grey_img" type="str" />
<param name="imu_topic" value="/SensorNode/imu" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio_gpu)/param/px30.yaml" />
<!-- Initial camera orientation, make it point downwards -->
<param name="init_rx" value="3.14" />
<param name="init_ry" value="0.00" />
<param name="init_rz" value="0.00" />
</node>
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio_gpu)/param/vo_fast.yaml" />
</launch>
GPU_version/vio/launch/live.launch
+16
View File
@@ -0,0 +1,16 @@
<launch>
<node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
<!-- Camera topic to subscribe to -->
<param name="cam_topic" value="/camera/image_raw" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find svo_ros)/param/camera_atan.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
</node>
</launch>
GPU_version/vio/launch/test.launch
+22
View File
@@ -0,0 +1,22 @@
<launch>
<node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
<!-- Camera topic to subscribe to -->
<param name="cam_topic" value="/camera/image_raw" type="str" />
<param name="imu_topic" value="/SensorNode/imu" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio_svo)/param/camera_atan.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
<!-- Initial camera orientation, make it point downwards -->
<param name="init_rx" value="3.14" />
<param name="init_ry" value="0.00" />
<param name="init_rz" value="0.00" />
</node>
</launch>
GPU_version/vio/launch/unity.launch
+22
View File
@@ -0,0 +1,22 @@
<launch>
<node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
<!-- Camera topic to subscribe to -->
<param name="cam_topic" value="/image_converter/output_image " type="str" />
<param name="imu_topic" value="/imu_unity/imu" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio_svo)/param/unity.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
<!-- Initial camera orientation, make it point downwards -->
<param name="init_rx" value="0.00" />
<param name="init_ry" value="0.00" />
<param name="init_rz" value="0.00" />
</node>
</launch>
GPU_version/vio/msg/Info.msg
+6
View File
@@ -0,0 +1,6 @@
Header header
float32 processing_time
int32[] keyframes
int32 num_matches
int32 tracking_quality
int32 stage
GPU_version/vio/package.xml
+43
View File
@@ -0,0 +1,43 @@
<?xml version="1.0"?>
<package>
<name>vio_gpu</name>
<version>0.1.0</version>
<description>
Monocular Visual Odometry - ROS Nodes
</description>
<author>Majid Geravand</author>
<maintainer email="[email protected]">Majid Geravand</maintainer>
<license>GPLv3</license>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<build_depend>roscpp</build_depend>
<build_depend>cmake_modules</build_depend> <!-- for FindEigen.cmake module -->
<build_depend>nav_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>tf</build_depend>
<build_depend>gpu_svo</build_depend>
<build_depend>vikit_common</build_depend>
<build_depend>vikit_ros</build_depend>
<build_depend>geometry_msgs</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>tf</run_depend>
<run_depend>vikit_common</run_depend>
<run_depend>vikit_ros</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>gpu_svo</run_depend>
</package>
GPU_version/vio/param/camera_atan.yaml
+8
View File
@@ -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
GPU_version/vio/param/camera_pinhole.yaml
+11
View File
@@ -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
GPU_version/vio/param/px30.yaml
+8
View File
@@ -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
GPU_version/vio/param/unity.yaml
+8
View File
@@ -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
GPU_version/vio/param/vo_accurate.yaml
+4
View File
@@ -0,0 +1,4 @@
grid_size: 20
max_n_kfs: 50
loba_num_iter: 10
max_fts: 200
GPU_version/vio/param/vo_fast.yaml
+13
View File
@@ -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
GPU_version/vio/rqt.perspective
+293
View File
@@ -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": {}
}
}
}
}
}
GPU_version/vio/rviz_config.rviz
+153
View File
@@ -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: <Fixed 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: <Fixed 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
GPU_version/vio/src/visualizer.cpp
+127
View File
@@ -0,0 +1,127 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// 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 <http://www.gnu.org/licenses/>.
#include <vio_gpu/visualizer.h>
#include <gpu_svo/frame_handler_mono.h>
#include <gpu_svo/point.h>
#include <gpu_svo/map.h>
#include <gpu_svo/feature.h>
#include <vio_gpu/Info.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <vikit/timer.h>
#include <vikit/output_helper.h>
#include <deque>
namespace svo {
Visualizer::
Visualizer() :
pnh_("~"),
trace_id_(0),
T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
{
// Init ROS Marker Publishers
pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
pub_info_ = pnh_.advertise<vio_gpu::Info>("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<FramePtr>::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<int>(slam.stage());
msg_info.tracking_quality = static_cast<int>(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<double,6,6> 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<FramePtr>& 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<Point*>::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
GPU_version/vio/src/vo_node.cpp
+148
View File
@@ -0,0 +1,148 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// 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 <http://www.gnu.org/licenses/>.
#include <ros/package.h>
#include <string>
#include <gpu_svo/frame_handler_mono.h>
#include <vio_gpu/visualizer.h>
#include <vikit/params_helper.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <image_transport/image_transport.h>
#include <boost/thread.hpp>
#include <cv_bridge/cv_bridge.h>
#include <Eigen/Core>
#include <vikit/abstract_camera.h>
#include <vikit/camera_loader.h>
#include <gpu_svo/frame.h>
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<bool>("vio/publish_markers", true)),
cam_(NULL),
quit_(false)
{
// Create Camera
imu_=(double *)calloc(static_cast<std::size_t>(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<double>("vio/init_rx", 0.0),
vk::getParam<double>("vio/init_ry", 0.0),
vk::getParam<double>("vio/init_rz", 0.0))),
Eigen::Vector3d(vk::getParam<double>("vio/init_tx", 0.0),
vk::getParam<double>("vio/init_ty", 0.0),
vk::getParam<double>("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<double>(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<std::size_t>(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<std::string>("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<std::string>("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;
}