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/include/gpu_svo/bundle_adjustment.h
+109
View File
@@ -0,0 +1,109 @@
// 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_BUNDLE_ADJUSTMENT_H_
#define SVO_BUNDLE_ADJUSTMENT_H_
#include <gpu_svo/global.h>
namespace g2o {
class EdgeProjectXYZ2UV;
class SparseOptimizer;
class VertexSE3Expmap;
class VertexSBAPointXYZ;
}
namespace svo {
typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
typedef g2o::VertexSE3Expmap g2oFrameSE3;
typedef g2o::VertexSBAPointXYZ g2oPoint;
class Frame;
class Point;
class Feature;
class Map;
/// Local, global and 2-view bundle adjustment with g2o
namespace ba {
/// Temporary container to hold the g2o edge with reference to frame and point.
struct EdgeContainerSE3
{
g2oEdgeSE3* edge;
Frame* frame;
Feature* feature;
bool is_deleted;
EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) :
edge(e), frame(frame), feature(feature), is_deleted(false)
{}
};
/// Optimize two camera frames and their observed 3D points.
/// Is used after initialization.
void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map);
/// Local bundle adjustment.
/// Optimizes core_kfs and their observed map points while keeping the
/// neighbourhood fixed.
void localBA(
Frame* center_kf,
set<FramePtr>* core_kfs,
Map* map,
size_t& n_incorrect_edges_1,
size_t& n_incorrect_edges_2,
double& init_error,
double& final_error);
/// Global bundle adjustment.
/// Optimizes the whole map. Is currently not used in SVO.
void globalBA(Map* map);
/// Initialize g2o with solver type, optimization strategy and camera model.
void setupG2o(g2o::SparseOptimizer * optimizer);
/// Run the optimization on the provided graph.
void runSparseBAOptimizer(
g2o::SparseOptimizer* optimizer,
unsigned int num_iter,
double& init_error,
double& final_error);
/// Create a g2o vertice from a keyframe object.
g2oFrameSE3* createG2oFrameSE3(
Frame* kf,
size_t id,
bool fixed);
/// Creates a g2o vertice from a mappoint object.
g2oPoint* createG2oPoint(
Vector3d pos,
size_t id,
bool fixed);
/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
g2oEdgeSE3* createG2oEdgeSE3(
g2oFrameSE3* v_kf,
g2oPoint* v_mp,
const Vector2d& f_up,
bool robust_kernel,
double huber_width,
double weight = 1);
} // namespace ba
} // namespace svo
#endif // SVO_BUNDLE_ADJUSTMENT_H_
GPU_version/svo/include/gpu_svo/cl_class.h
+191
View File
@@ -0,0 +1,191 @@
//
// Created by root on 4/27/21.
//
#ifndef VIO_OPENCL_CL_CLASS_H
#define VIO_OPENCL_CL_CLASS_H
#include <CL/cl.hpp>
#include <iostream>
#include <vector>
#include <algorithm>
#include <fstream>
#include <exception>
#include <CL/opencl.h>
#include <opencv2/opencv.hpp>
class kernel{
public:
kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());};
~kernel(){
_buffers.clear();
_images.clear();
};
template<typename T>
int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){
_buffers.push_back(std::pair<std::pair<cl::Buffer,size_t>,size_t>(std::pair<cl::Buffer,size_t>(cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(T) * buf_size),
buf_size),id));
cl_int err=queue->enqueueWriteBuffer(_buffers.back().first.first,CL_TRUE, 0, sizeof(T) * buf_size, buf);
if(err!=0){
std::cerr<<"Error C:Kernel, F:write, L:enqueueWriteBuffer\n"<<err<<'\n';
return err;
}
err=_kernel->setArg(id,_buffers.back().first.first);
if(err!=0){
std::cerr<<"Error C:Kernel, F:write, L3:setArg\n"<<err<<'\n';
return err;
}
return CL_SUCCESS;
};
int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){
_images.push_back(std::pair<cl::Image2D,size_t>(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)),id));
cl_int err=_kernel->setArg(id,_images.back().first);
if(err!=0){
std::cerr<<"Error C:Kernel, F:write image, L:setArg\n"<<err<<'\n';
return err;
}
return CL_SUCCESS;
};
template<typename T>
int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
try{
if(id>_buffers.size())return CL_MAP_FAILURE;
for(auto i:_buffers)if(i.second==id)
queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf);
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
}
};
int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
try{
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';
return CL_MAP_FAILURE;
}
};
cl::Kernel* _kernel = NULL;
std::pair<cl::Buffer,size_t> read(size_t id){
for(auto i:_buffers)if(i.second==id)return i.first;
}
cl::Image2D read_img(size_t id){
for(auto i:_images)if(i.second==id)return i.first;
}
private:
std::vector<std::pair<std::pair<cl::Buffer,size_t>,size_t>> _buffers;
std::vector<std::pair<cl::Image2D,size_t>> _images;
};
class read_cl{
public:
read_cl(std::string path){
src_str=(char *)malloc(0x100000);
FILE *fp;
fp = fopen(path.c_str(), "r");
size = fread(src_str, 1, 0x100000, fp);
fclose(fp);
}
void print(){
std::cerr<<src_str<<"\n";
}
char *src_str = nullptr;
size_t size;
};
class opencl{
public:
opencl();
~opencl();
int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));};
template<typename T>
int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) {
try{
_kernels.at(id1).write(id2,buf,queue,context,size);
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Write buffer on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
}
}
int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
try{
_kernels.at(id1).write(id2,buf,context);
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Write image on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
}
}
template<typename T>
int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
_kernels.at(id1)._kernel->setArg(id2,value);
};
template<typename T>
int32_t reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T* buf) {
try{
_kernels.at(id1).reload(id2,buf,queue);
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
}
}
int32_t reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
try{
_kernels.at(id1).reload(id2,buf,context);
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Write buffer on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
}
}
int32_t run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) {
cl_int err=0;
if(z>1 && y>1){
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/);
}else if(z<2 && y>1){
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/);
}else{
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/);
};
queue->finish();
if(err!=0){
std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_FUNCTION_NAME>();
std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_NUM_ARGS>()<<" ,Run out: "<<err<<'\n';
for(int i=0;i<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_NUM_ARGS>();++i)
std::cerr<<"argument type "<<i<<":"<<_kernels.at(id1)._kernel->getArgInfo<CL_KERNEL_ARG_TYPE_NAME>(i)<<'\n';
}
return err;
}
template<typename T>
void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, T* out){
queue->enqueueReadBuffer(_kernels.at(id1).read(id2).first, CL_TRUE, 0, sizeof(T) * _kernels.at(id1).read(id2).second, out);
}
void clear_buf();
private:
std::vector<kernel> _kernels;
cl::Context* context = NULL;
cl::Device device;
cl::Program* program = NULL;
cl::CommandQueue* queue=NULL;
};
#endif //VIO_OPENCL_CL_CLASS_H
GPU_version/svo/include/gpu_svo/config.h
+177
View File
@@ -0,0 +1,177 @@
// 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_CONFIG_H_
#define SVO_CONFIG_H_
#include <string>
#include <stdint.h>
#include <stdio.h>
namespace svo {
using std::string;
/// Global configuration file of SVO.
/// Implements the Singleton design pattern to allow global access and to ensure
/// that only one instance exists.
class Config
{
public:
static Config& getInstance();
/// Base-name of the tracefiles.
static string& traceName() { return getInstance().trace_name; }
/// Directory where the tracefiles are saved.
static string& traceDir() { return getInstance().trace_dir; }
/// Number of pyramid levels used for features.
static size_t& nPyrLevels() { return getInstance().n_pyr_levels; }
/// Use the IMU to get relative rotations.
static bool& useImu() { return getInstance().use_imu; }
/// Number of keyframes in the core. The core-kfs are optimized through bundle adjustment.
static size_t& coreNKfs() { return getInstance().core_n_kfs; }
/// Initial scale of the map. Depends on the distance the camera is moved for the initialization.
static double& mapScale() { return getInstance().map_scale; }
/// Feature grid size of a cell in [px].
static size_t& gridSize() { return getInstance().grid_size; }
/// Initialization: Minimum required disparity between the first two frames.
static double& initMinDisparity() { return getInstance().init_min_disparity; }
/// Initialization: Minimum number of tracked features.
static size_t& initMinTracked() { return getInstance().init_min_tracked; }
/// Initialization: Minimum number of inliers after RANSAC.
static size_t& initMinInliers() { return getInstance().init_min_inliers; }
/// Maximum level of the Lucas Kanade tracker.
static size_t& kltMaxLevel() { return getInstance().klt_max_level; }
/// Minimum level of the Lucas Kanade tracker.
static size_t& kltMinLevel() { return getInstance().klt_min_level; }
/// Reprojection threshold [px].
static double& reprojThresh() { return getInstance().reproj_thresh; }
/// Reprojection threshold after pose optimization.
static double& poseOptimThresh() { return getInstance().poseoptim_thresh; }
/// Number of iterations in local bundle adjustment.
static size_t& poseOptimNumIter() { return getInstance().poseoptim_num_iter; }
/// Maximum number of points to optimize at every iteration.
static size_t& structureOptimMaxPts() { return getInstance().structureoptim_max_pts; }
/// Number of iterations in structure optimization.
static size_t& structureOptimNumIter() { return getInstance().structureoptim_num_iter; }
/// Reprojection threshold after bundle adjustment.
static double& lobaThresh() { return getInstance().loba_thresh; }
/// Threshold for the robust Huber kernel of the local bundle adjustment.
static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; }
/// Number of iterations in the local bundle adjustment.
static size_t& lobaNumIter() { return getInstance().loba_num_iter; }
/// Minimum distance between two keyframes. Relative to the average height in the map.
static double& kfSelectMinDist() { return getInstance().kfselect_mindist; }
/// Select only features with a minimum Harris corner score for triangulation.
static double& triangMinCornerScore() { return getInstance().triang_min_corner_score; }
/// Subpixel refinement of reprojection and triangulation. Set to 0 if no subpix refinement required!
static size_t& subpixNIter() { return getInstance().subpix_n_iter; }
/// 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.
static size_t& maxNKfs() { return getInstance().max_n_kfs; }
/// How much (in milliseconds) is the camera delayed with respect to the imu.
static double& imgImuDelay() { return getInstance().img_imu_delay; }
/// Maximum number of features that should be tracked.
static size_t& maxFts() { return getInstance().max_fts; }
/// If the number of tracked features drops below this threshold. Tracking quality is bad.
static size_t& qualityMinFts() { return getInstance().quality_min_fts; }
/// If within one frame, this amount of features are dropped. Tracking quality is bad.
static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; }
/// acc white noise in continuous.
static double& ACC_Noise() { return getInstance().ACC_noise; }
/// acc white noise in continuous.
static double& GYO_Noise() { return getInstance().GYO_noise; }
/// integration uncertainty continuous
static double& IUC() { return getInstance().iuc; }
// acc bias in continuous
static double& ABC() { return getInstance().abc; }
// gyro bias in continuous
static double& GBC() { return getInstance().gbc; }
// error in the bias used for preintegration
static double& EBP() { return getInstance().ebp; }
// the noise used for projection
static double& NRP() { return getInstance().nrp; }
private:
Config();
Config(Config const&);
void operator=(Config const&);
string trace_name;
string trace_dir;
size_t n_pyr_levels;
bool use_imu;
size_t core_n_kfs;
double map_scale;
size_t grid_size;
double init_min_disparity;
size_t init_min_tracked;
size_t init_min_inliers;
size_t klt_max_level;
size_t klt_min_level;
double reproj_thresh;
double poseoptim_thresh;
size_t poseoptim_num_iter;
size_t structureoptim_max_pts;
size_t structureoptim_num_iter;
double loba_thresh;
double loba_robust_huber_width;
size_t loba_num_iter;
double kfselect_mindist;
double triang_min_corner_score;
size_t triang_half_patch_size;
size_t subpix_n_iter;
size_t max_n_kfs;
double img_imu_delay;
size_t max_fts;
size_t quality_min_fts;
int quality_max_drop_fts;
double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise;
};
} // namespace svo
#endif // SVO_CONFIG_H_
GPU_version/svo/include/gpu_svo/depth_filter.h
+171
View File
@@ -0,0 +1,171 @@
// 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_DEPTH_FILTER_H_
#define SVO_DEPTH_FILTER_H_
#include <queue>
#include <boost/thread.hpp>
#include <boost/function.hpp>
#include <vikit/performance_monitor.h>
#include <gpu_svo/global.h>
#include <gpu_svo/feature_detection.h>
#include <gpu_svo/matcher.h>
namespace svo {
class Frame;
class Feature;
class Point;
/// A seed is a probabilistic depth estimate for a single pixel.
struct Seed
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static int batch_counter;
static int seed_counter;
int batch_id; //!< Batch id is the id of the keyframe for which the seed was created.
int id; //!< Seed ID, only used for visualization.
Feature* ftr; //!< Feature in the keyframe for which the depth should be computed.
float a; //!< a of Beta distribution: When high, probability of inlier is large.
float b; //!< b of Beta distribution: When high, probability of outlier is large.
float mu; //!< Mean of normal distribution.
float z_range; //!< Max range of the possible depth.
float sigma2; //!< Variance of normal distribution.
Matrix2d patch_cov; //!< Patch covariance in reference image.
Seed(Feature* ftr, float depth_mean, float depth_min);
};
/// Depth filter implements the Bayesian Update proposed in:
/// "Video-based, Real-Time Multi View Stereo" by G. Vogiatzis and C. Hernández.
/// In Image and Vision Computing, 29(7):434-441, 2011.
///
/// The class uses a callback mechanism such that it can be used also by other
/// algorithms than nslam and for simplified testing.
class DepthFilter
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef boost::unique_lock<boost::mutex> lock_t;
typedef boost::function<void ( Point*, double )> callback_t;
/// Depth-filter config parameters
struct Options
{
bool check_ftr_angle; //!< gradient features are only updated if the epipolar line is orthogonal to the gradient.
bool epi_search_1d; //!< restrict Gauss Newton in the epipolar search to the epipolar line.
bool verbose; //!< display output.
bool use_photometric_disparity_error; //!< use photometric disparity error instead of 1px error in tau computation.
int max_n_kfs; //!< maximum number of keyframes for which we maintain seeds.
double sigma_i_sq; //!< image noise.
double seed_convergence_sigma2_thresh; //!< threshold on depth uncertainty for convergence.
Options()
: check_ftr_angle(false),
epi_search_1d(false),
verbose(false),
use_photometric_disparity_error(false),
max_n_kfs(3),
sigma_i_sq(5e-4),
seed_convergence_sigma2_thresh(200.0)
{}
} options_;
DepthFilter(
feature_detection::DetectorPtr feature_detector,
callback_t seed_converged_cb);
virtual ~DepthFilter();
/// Start this thread when seed updating should be in a parallel thread.
void startThread();
/// Stop the parallel thread that is running.
void stopThread();
/// Add frame to the queue to be processed.
void addFrame(FramePtr frame);
/// Add new keyframe to the queue
void addKeyframe(FramePtr frame, double depth_mean, double depth_min);
/// Remove all seeds which are initialized from the specified keyframe. This
/// function is used to make sure that no seeds points to a non-existent frame
/// when a frame is removed from the map.
void removeKeyframe(FramePtr frame);
/// If the map is reset, call this function such that we don't have pointers
/// to old frames.
void reset();
/// Returns a copy of the seeds belonging to frame. Thread-safe.
/// Can be used to compute the Next-Best-View in parallel.
/// IMPORTANT! Make sure you hold a valid reference counting pointer to frame
/// so it is not being deleted while you use it.
void getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds);
/// Return a reference to the seeds. This is NOT THREAD SAFE!
//std::list<Seed>& getSeeds() { return seeds_; } Bug 238
std::list<Seed, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
/// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
static void updateSeed(
const float x,
const float tau2,
Seed* seed);
/// Compute the uncertainty of the measurement.
static double computeTau(
const SE3& T_ref_cur,
const Vector3d& f,
const double z,
const double px_error_angle);
protected:
feature_detection::DetectorPtr feature_detector_;
callback_t seed_converged_cb_;
//std::list<Seed> seeds_;
std::list<Seed, Eigen::aligned_allocator<Seed>> seeds_;
boost::mutex seeds_mut_;
bool seeds_updating_halt_; //!< Set this value to true when seeds updating should be interrupted.
boost::thread* thread_;
std::queue<FramePtr> frame_queue_;
boost::mutex frame_queue_mut_;
boost::condition_variable frame_queue_cond_;
FramePtr new_keyframe_; //!< Next keyframe to extract new seeds.
bool new_keyframe_set_; //!< Do we have a new keyframe to process?.
double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds.
double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds.
vk::PerformanceMonitor permon_; //!< Separate performance monitor since the DepthFilter runs in a parallel thread.
Matcher matcher_;
/// Initialize new seeds from a frame.
void initializeSeeds(FramePtr frame);
/// Update all seeds with a new measurement frame.
virtual void updateSeeds(FramePtr frame);
/// When a new keyframe arrives, the frame queue should be cleared.
void clearFrameQueue();
/// A thread that is continuously updating the seeds.
void updateSeedsLoop();
};
} // namespace svo
#endif // SVO_DEPTH_FILTER_H_
GPU_version/svo/include/gpu_svo/feature.h
+75
View File
@@ -0,0 +1,75 @@
// 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_FEATURE_H_
#define SVO_FEATURE_H_
#include <gpu_svo/frame.h>
namespace svo {
/// A salient image region that is tracked across frames.
struct Feature
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum FeatureType {
CORNER,
EDGELET
};
FeatureType type; //!< Type can be corner or edgelet.
Frame* frame; //!< Pointer to frame in which the feature was detected.
Vector2d px; //!< Coordinates in pixels on pyramid level 0.
Vector3d f; //!< Unit-bearing vector of the feature.
int level; //!< Image pyramid level where feature was extracted.
Point* point; //!< Pointer to 3D point which corresponds to the feature.
Vector2d grad; //!< Dominant gradient direction for edglets, normalized.
Feature(Frame* _frame, const Vector2d& _px, int _level) :
type(CORNER),
frame(_frame),
px(_px),
f(frame->cam_->cam2world(px)),
level(_level),
point(NULL),
grad(1.0,0.0)
{}
Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
type(CORNER),
frame(_frame),
px(_px),
f(_f),
level(_level),
point(NULL),
grad(1.0,0.0)
{}
Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
type(CORNER),
frame(_frame),
px(_px),
f(_f),
level(_level),
point(_point),
grad(1.0,0.0)
{}
};
} // namespace svo
#endif // SVO_FEATURE_H_
GPU_version/svo/include/gpu_svo/feature_alignment.h
+63
View File
@@ -0,0 +1,63 @@
// 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_FEATURE_ALIGNMENT_H_
#define SVO_FEATURE_ALIGNMENT_H_
#include <gpu_svo/global.h>
namespace svo {
/// Subpixel refinement of a reference feature patch with the current image.
/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on"
/// paper by Baker.
namespace feature_alignment {
bool align1D(
const cv::Mat& cur_img,
const Vector2f& dir, // direction in which the patch is allowed to move
uint8_t* ref_patch_with_border,
uint8_t* ref_patch,
const int n_iter,
Vector2d& cur_px_estimate,
double& h_inv);
bool align2D(
const cv::Mat& cur_img,
uint8_t* ref_patch_with_border,
uint8_t* ref_patch,
const int n_iter,
Vector2d& cur_px_estimate,
bool no_simd = false);
bool align2D_SSE2(
const cv::Mat& cur_img,
uint8_t* ref_patch_with_border,
uint8_t* ref_patch,
const int n_iter,
Vector2d& cur_px_estimate);
bool align2D_NEON(
const cv::Mat& cur_img,
uint8_t* ref_patch_with_border,
uint8_t* ref_patch,
const int n_iter,
Vector2d& cur_px_estimate);
} // namespace feature_alignment
} // namespace svo
#endif // SVO_FEATURE_ALIGNMENT_H_
GPU_version/svo/include/gpu_svo/feature_detection.h
+111
View File
@@ -0,0 +1,111 @@
// 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_FEATURE_DETECTION_H_
#define SVO_FEATURE_DETECTION_H_
#include <gpu_svo/global.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/cl_class.h>
namespace svo {
/// Implementation of various feature detectors.
namespace feature_detection {
/// Temporary container used for corner detection. Features are initialized from these.
struct Corner
{
int x; //!< x-coordinate of corner in the image.
int y; //!< y-coordinate of corner in the image.
int level; //!< pyramid level of the corner.
float score; //!< shi-tomasi score of the corner.
float angle; //!< for gradient-features: dominant gradient angle.
Corner(int x, int y, float score, int level, float angle) :
x(x), y(y), level(level), score(score), angle(angle)
{}
};
typedef vector<Corner> Corners;
/// All detectors should derive from this abstract class.
class AbstractDetector
{
public:
AbstractDetector(
const int img_width,
const int img_height,
const int cell_size,
const int n_pyr_levels);
virtual ~AbstractDetector() {
};
virtual void detect(
Frame* frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts) = 0;
/// Flag the grid cell as occupied
void setGridOccpuancy(const Vector2d& px);
/// Set grid cells of existing features as occupied
void setExistingFeatures(const Features& fts);
protected:
static const int border_ = 8; //!< no feature should be within 8px of border.
const int cell_size_;
const int n_pyr_levels_;
const int grid_n_cols_;
const int grid_n_rows_;
vector<bool> grid_occupancy_;
void resetGrid();
inline int getCellIndex(int x, int y, int level)
{
const int scale = (1<<level);
return (scale*y)/cell_size_*grid_n_cols_ + (scale*x)/cell_size_;
}
};
typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
/// FAST detector by Edward Rosten.
class FastDetector : public AbstractDetector
{
public:
FastDetector(
const int img_width,
const int img_height,
const int cell_size,
opencl* gpu_fast_,
const int n_pyr_levels);
virtual ~FastDetector() {}
virtual void detect(
Frame* frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts);
opencl* gpu_fast_;
};
} // namespace feature_detection
} // namespace svo
#endif // SVO_FEATURE_DETECTION_H_
GPU_version/svo/include/gpu_svo/frame.h
+154
View File
@@ -0,0 +1,154 @@
// 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_FRAME_H_
#define SVO_FRAME_H_
#include <sophus/se3.h>
#include <vikit/math_utils.h>
#include <vikit/abstract_camera.h>
#include <boost/noncopyable.hpp>
#include <gpu_svo/global.h>
namespace g2o {
class VertexSE3Expmap;
}
typedef g2o::VertexSE3Expmap g2oFrameSE3;
namespace svo {
class Point;
struct Feature;
typedef list<Feature*> Features;
typedef vector<cv::Mat> ImgPyr;
/// A frame saves the image, the associated features and the estimated pose.
class Frame : boost::noncopyable
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id.
int id_; //!< Unique id of the frame.
double timestamp_; //!< Timestamp of when the image was recorded.
vk::AbstractCamera* cam_; //!< Camera model.
Sophus::SE3 T_f_w_; //!< Transform (f)rame from (w)orld.
Matrix<double, 6, 6> Cov_; //!< Covariance.
ImgPyr img_pyr_; //!< Image Pyramid.
Features fts_; //!< List of features in the image.
vector<Feature*> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
bool is_keyframe_; //!< Was this frames selected as keyframe?
g2oFrameSE3* v_kf_; //!< Temporary pointer to the g2o node object of the keyframe.
int last_published_ts_; //!< Timestamp of last publishing.
Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);
~Frame();
/// Initialize new frame and create image pyramid.
void initFrame(const cv::Mat& img);
/// Select this frame as keyframe.
void setKeyframe();
/// Add a feature to the image
void addFeature(Feature* ftr);
/// The KeyPoints are those five features which are closest to the 4 image corners
/// and to the center and which have a 3D point assigned. These points are used
/// to quickly check whether two frames have overlapping field of view.
void setKeyPoints();
/// Check if we can select five better key-points.
void checkKeyPoints(Feature* ftr);
/// If a point is deleted, we must remove the corresponding key-point.
void removeKeyPoint(Feature* ftr);
/// Return number of point observations.
inline size_t nObs() const { return fts_.size(); }
/// Check if a point in (w)orld coordinate frame is visible in the image.
bool isVisible(const Vector3d& xyz_w) const;
/// Full resolution image stored in the frame.
inline const cv::Mat& img() const { return img_pyr_[0]; }
/// Was this frame selected as keyframe?
inline bool isKeyframe() const { return is_keyframe_; }
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); }
/// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); }
/// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); }
/// Transforms point coordinates in world-frame (w) to camera-frams (f).
inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; }
/// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; }
/// Projects Point from unit sphere (f) in camera pixels (c).
inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
/// Return the pose of the frame in the (w)orld coordinate frame.
inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
/// Frame jacobian for projection of 3D point in (f)rame coordinate to
/// unit plane coordinates uv (focal length = 1).
inline static void jacobian_xyz2uv(
const Vector3d& xyz_in_f,
Matrix<double,2,6>& J)
{
const double x = xyz_in_f[0];
const double y = xyz_in_f[1];
const double z_inv = 1./xyz_in_f[2];
const double z_inv_2 = z_inv*z_inv;
J(0,0) = -z_inv; // -1/z
J(0,1) = 0.0; // 0
J(0,2) = x*z_inv_2; // x/z^2
J(0,3) = y*J(0,2); // x*y/z^2
J(0,4) = -(1.0 + x*J(0,2)); // -(1.0 + x^2/z^2)
J(0,5) = y*z_inv; // y/z
J(1,0) = 0.0; // 0
J(1,1) = -z_inv; // -1/z
J(1,2) = y*z_inv_2; // y/z^2
J(1,3) = 1.0 + y*J(1,2); // 1.0 + y^2/z^2
J(1,4) = -J(0,3); // -x*y/z^2
J(1,5) = -x*z_inv; // x/z
}
};
/// Some helper functions for the frame object.
namespace frame_utils {
/// Creates an image pyramid of half-sampled images.
void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr);
/// Get the average depth of the features in the image.
bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min);
} // namespace frame_utils
} // namespace svo
#endif // SVO_FRAME_H_
GPU_version/svo/include/gpu_svo/frame_handler_base.h
+123
View File
@@ -0,0 +1,123 @@
// 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_FRAME_HANDLER_BASE_H_
#define SVO_FRAME_HANDLER_BASE_H_
#include <queue>
#include <vikit/timer.h>
#include <vikit/ringbuffer.h>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <gpu_svo/global.h>
#include <gpu_svo/map.h>
namespace vk
{
class AbstractCamera;
class PerformanceMonitor;
}
namespace svo
{
class Point;
class Matcher;
class DepthFilter;
/// Base class for various VO pipelines. Manages the map and the state machine.
class FrameHandlerBase : boost::noncopyable
{
public:
enum Stage {
STAGE_PAUSED,
STAGE_FIRST_FRAME,
STAGE_SECOND_FRAME,
STAGE_DEFAULT_FRAME,
STAGE_RELOCALIZING
};
enum TrackingQuality {
TRACKING_INSUFFICIENT,
TRACKING_BAD,
TRACKING_GOOD
};
enum UpdateResult {
RESULT_NO_KEYFRAME,
RESULT_IS_KEYFRAME,
RESULT_FAILURE
};
FrameHandlerBase();
virtual ~FrameHandlerBase();
/// Get the current map.
const Map& map() const { return map_; }
/// Will reset the map as soon as the current frame is finished processing.
void reset() { set_reset_ = true; }
/// Start processing.
void start() { set_start_ = true; }
/// Get the current stage of the algorithm.
Stage stage() const { return stage_; }
/// Get tracking quality.
TrackingQuality trackingQuality() const { return tracking_quality_; }
/// Get the processing time of the previous iteration.
double lastProcessingTime() const { return timer_.getTime(); }
/// Get the number of feature observations of the last frame.
size_t lastNumObservations() const { return num_obs_last_; }
protected:
Stage stage_; //!< Current stage of the algorithm.
bool set_reset_; //!< Flag that the user can set. Will reset the system before the next iteration.
bool set_start_; //!< Flag the user can set to start the system when the next image is received.
Map map_; //!< Map of keyframes created by the slam system.
vk::Timer timer_; //!< Stopwatch to measure time to process frame.
vk::RingBuffer<double> acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
vk::RingBuffer<size_t> acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
size_t num_obs_last_; //!< Number of observations in the previous frame.
TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features.
/// Before a frame is processed, this function is called.
bool startFrameProcessingCommon(const double timestamp);
/// When a frame is finished processing, this function is called.
int finishFrameProcessingCommon(
const size_t update_id,
const UpdateResult dropout,
const size_t num_observations);
/// Reset the map and frame handler to start from scratch.
void resetCommon();
/// Reset the frame handler. Implement in derived class.
virtual void resetAll() { resetCommon(); }
/// Set the tracking quality based on the number of tracked features.
virtual void setTrackingQuality(const size_t num_observations);
/// Optimize some of the observed 3D points.
virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
};
} // namespace nslam
#endif // SVO_FRAME_HANDLER_BASE_H_
GPU_version/svo/include/gpu_svo/frame_handler_mono.h
+105
View File
@@ -0,0 +1,105 @@
// 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_FRAME_HANDLER_H_
#define SVO_FRAME_HANDLER_H_
#include <set>
#include <vikit/abstract_camera.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 {
/// Monocular Visual Odometry Pipeline as described in the SVO paper.
class FrameHandlerMono : public FrameHandlerBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init);
virtual ~FrameHandlerMono();
/// Provide an image.
void addImage(const cv::Mat& img, double timestamp);
/// Set the first frame (used for synthetic datasets in benchmark node)
void setFirstFrame(const FramePtr& first_frame);
/// Get the last frame that has been processed.
FramePtr lastFrame() { return last_frame_; }
/// Get the set of spatially closest keyframes of the last frame.
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_; }
/// Access the depth filter.
DepthFilter* depthFilter() const { return depth_filter_; }
/// An external place recognition module may know where to relocalize.
bool relocalizeFrameAtPose(
const int keyframe_id,
const SE3& T_kf_f,
const cv::Mat& img,
const double timestamp);
std::unique_ptr<Imu_Integration> imu_integPtr_; //Integration of IMU
protected:
vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
Reprojector reprojector_; //!< Projects points from other keyframes into the current frame
FramePtr new_frame_; //!< Current frame.
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.
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();
/// Processes the first frame and sets it as a keyframe.
virtual UpdateResult processFirstFrame();
/// Processes all frames after the first frame until a keyframe is selected.
virtual UpdateResult processSecondFrame();
/// Processes all frames after the first two keyframes.
virtual UpdateResult processFrame();
/// Try relocalizing the frame at relative position to provided keyframe.
virtual UpdateResult relocalizeFrame(
const SE3& T_cur_ref,
FramePtr ref_keyframe);
/// Reset the frame handler. Implement in derived class.
virtual void resetAll();
/// Keyframe selection criterion.
virtual bool needNewKf(double scene_depth_mean);
void setCoreKfs(size_t n_closest);
};
} // namespace svo
#endif // SVO_FRAME_HANDLER_H_
GPU_version/svo/include/gpu_svo/global.h
+101
View File
@@ -0,0 +1,101 @@
// 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_GLOBAL_H_
#define SVO_GLOBAL_H_
#include <list>
#include <vector>
#include <string>
#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <sophus/se3.h>
#include <vikit/performance_monitor.h>
#include <boost/shared_ptr.hpp>
#include<Eigen/StdVector>
#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
#endif
#ifdef SVO_USE_ROS
#include <ros/console.h>
#define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
#define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
#define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
#define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
#define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
#else
#define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "<<x<<"\033[0;0m"<<std::endl;
#ifdef SVO_DEBUG_OUTPUT
#define SVO_DEBUG_STREAM(x) SVO_INFO_STREAM(x)
#else
#define SVO_DEBUG_STREAM(x)
#endif
#define SVO_WARN_STREAM(x) std::cerr<<"\033[0;33m[WARN] "<<x<<"\033[0;0m"<<std::endl;
#define SVO_ERROR_STREAM(x) std::cerr<<"\033[1;31m[ERROR] "<<x<<"\033[0;0m"<<std::endl;
#include <chrono> // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc.
#define SVO_WARN_STREAM_THROTTLE(rate, x) \
do { \
static double __log_stream_throttle__last_hit__ = 0.0; \
std::chrono::time_point<std::chrono::system_clock> __log_stream_throttle__now__ = \
std::chrono::system_clock::now(); \
if (__log_stream_throttle__last_hit__ + rate <= \
std::chrono::duration_cast<std::chrono::seconds>( \
__log_stream_throttle__now__.time_since_epoch()).count()) { \
__log_stream_throttle__last_hit__ = \
std::chrono::duration_cast<std::chrono::seconds>( \
__log_stream_throttle__now__.time_since_epoch()).count(); \
SVO_WARN_STREAM(x); \
} \
} while(0)
#endif
namespace svo
{
using namespace Eigen;
using namespace Sophus;
const double EPS = 0.0000000001;
const double PI = 3.14159265;
#ifdef SVO_TRACE
extern vk::PerformanceMonitor* g_permon;
#define SVO_LOG(value) g_permon->log(std::string((#value)),(value))
#define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2)
#define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3)
#define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4)
#define SVO_START_TIMER(name) g_permon->startTimer((name))
#define SVO_STOP_TIMER(name) g_permon->stopTimer((name))
#else
#define SVO_LOG(v)
#define SVO_LOG2(v1, v2)
#define SVO_LOG3(v1, v2, v3)
#define SVO_LOG4(v1, v2, v3, v4)
#define SVO_START_TIMER(name)
#define SVO_STOP_TIMER(name)
#endif
class Frame;
typedef boost::shared_ptr<Frame> FramePtr;
} // namespace svo
#endif // SVO_GLOBAL_H_
GPU_version/svo/include/gpu_svo/imu_integration.h
+121
View File
@@ -0,0 +1,121 @@
//
// Created by gufe on 4/9/21.
//
#ifndef SVO_IMU_INTEGRATION_H
#define SVO_IMU_INTEGRATION_H
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <sophus/se3.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)
using gtsam::symbol_shorthand::P; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::C; // Camera pose
//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop
// Wrapper class
template <typename T>
class enumerate_impl
{
public:
// The return value of the operator* of the iterator, this
// is what you will get inside of the for loop
struct item
{
size_t index;
typename T::value_type & item;
};
typedef item value_type;
// Custom iterator with minimal interface
struct iterator
{
iterator(typename T::iterator _it, size_t counter=0) :
it(_it), counter(counter)
{}
iterator operator++()
{
return iterator(++it, ++counter);
}
bool operator!=(iterator other)
{
return it != other.it;
}
typename T::iterator::value_type item()
{
return *it;
}
value_type operator*()
{
return value_type{counter, *it};
}
size_t index()
{
return counter;
}
private:
typename T::iterator it;
size_t counter;
};
enumerate_impl(T & t) : container(t) {}
iterator begin()
{
return iterator(container.begin());
}
iterator end()
{
return iterator(container.end());
}
private:
T & container;
};
// A templated free function allows you to create the wrapper class
// conveniently
template <typename T>
enumerate_impl<T> enumerate(T & t)
{
return enumerate_impl<T>(t);
}
class Imu_Integration{
public:
Imu_Integration(Sophus::SE3& SE_init);
~Imu_Integration();
bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
bool update(double* imu= nullptr);
bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
private:
std::shared_ptr<gtsam::NavState> statePtr;
std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
std::shared_ptr<gtsam::Values> valuesPtr;
std::shared_ptr<gtsam::NonlinearFactorGraph> graphPtr;
boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> preintegratedPtr;
boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
std::chrono::steady_clock::time_point t_1;
std::uint32_t imu_factor_id=0;
uint imu_n=0;
std::shared_ptr<gtsam::ISAM2Params> optimizerParamPtr;
bool syn=false;
};
#endif //SVO_IMU_INTEGRATION_H
GPU_version/svo/include/gpu_svo/initialization.h
+86
View File
@@ -0,0 +1,86 @@
// 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_INITIALIZATION_H
#define SVO_INITIALIZATION_H
#include <gpu_svo/global.h>
#include <gpu_svo/cl_class.h>
namespace svo {
class FrameHandlerMono;
/// Bootstrapping the map from the first two views.
namespace initialization {
enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS };
/// Tracks features using Lucas-Kanade tracker and then estimates a homography.
class KltHomographyInit {
friend class svo::FrameHandlerMono;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FramePtr frame_ref_;
KltHomographyInit(opencl* gpu_fast_):gpu_fast_(gpu_fast_) {};
~KltHomographyInit() {};
InitResult addFirstFrame(FramePtr frame_ref);
InitResult addSecondFrame(FramePtr frame_ref);
void reset();
protected:
vector<cv::Point2f> px_ref_; //!< keypoints to be tracked in reference frame.
vector<cv::Point2f> px_cur_; //!< tracked keypoints in current frame.
vector<Vector3d> f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image.
vector<Vector3d> f_cur_; //!< bearing vectors corresponding to the keypoints in the current image.
vector<double> disparities_; //!< disparity between first and second frame.
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,
opencl* gpu_fast);
/// Compute optical flow (Lucas Kanade) for selected keypoints.
void trackKlt(
FramePtr frame_ref,
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vector<double>& disparities);
void computeHomography(
const vector<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref);
} // namespace initialization
} // namespace svo
#endif // SVO_INITIALIZATION_H
GPU_version/svo/include/gpu_svo/map.h
+136
View File
@@ -0,0 +1,136 @@
// 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_MAP_H_
#define SVO_MAP_H_
#include <queue>
#include <boost/noncopyable.hpp>
#include <boost/thread.hpp>
#include <gpu_svo/global.h>
namespace svo {
class Point;
class Feature;
class Seed;
/// Container for converged 3D points that are not already assigned to two keyframes.
class MapPointCandidates
{
public:
typedef pair<Point*, Feature*> PointCandidate;
typedef list<PointCandidate> PointCandidateList;
/// The depth-filter is running in a parallel thread and fills the canidate list.
/// This mutex controls concurrent access to point_candidates.
boost::mutex mut_;
/// Candidate points are created from converged seeds.
/// Until the next keyframe, these points can be used for reprojection and pose optimization.
PointCandidateList candidates_;
list< Point* > trash_points_;
MapPointCandidates();
~MapPointCandidates();
/// Add a candidate point.
void newCandidatePoint(Point* point, double depth_sigma2);
/// Adds the feature to the frame and deletes candidate from list.
void addCandidatePointToFrame(FramePtr frame);
/// Remove a candidate point from the list of candidates.
bool deleteCandidatePoint(Point* point);
/// Remove all candidates that belong to a frame.
void removeFrameCandidates(FramePtr frame);
/// Reset the candidate list, remove and delete all points.
void reset();
void deleteCandidate(PointCandidate& c);
void emptyTrash();
};
/// Map object which saves all keyframes which are in a map.
class Map : boost::noncopyable
{
public:
list< FramePtr > keyframes_; //!< List of keyframes in the map.
list< Point* > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
MapPointCandidates point_candidates_;
Map();
~Map();
/// Reset the map. Delete all keyframes and reset the frame and point counters.
void reset();
/// Delete a point in the map and remove all references in keyframes to it.
void safeDeletePoint(Point* pt);
/// Moves the point to the trash queue which is cleaned now and then.
void deletePoint(Point* pt);
/// Moves the frame to the trash queue which is cleaned now and then.
bool safeDeleteFrame(FramePtr frame);
/// Remove the references between a point and a frame.
void removePtFrameRef(Frame* frame, Feature* ftr);
/// Add a new keyframe to the map.
void addKeyframe(FramePtr new_keyframe);
/// Given a frame, return all keyframes which have an overlapping field of view.
void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const;
/// Return the keyframe which is spatially closest and has overlapping field of view.
FramePtr getClosestKeyframe(const FramePtr& frame) const;
/// Return the keyframe which is furthest apart from pos.
FramePtr getFurthestKeyframe(const Vector3d& pos) const;
bool getKeyframeById(const int id, FramePtr& frame) const;
/// Transform the whole map with rotation R, translation t and scale s.
void transform(const Matrix3d& R, const Vector3d& t, const double& s);
/// Empty trash bin of deleted keyframes and map points. We don't delete the
/// points immediately to ensure proper cleanup and to provide the visualizer
/// a list of objects which must be removed.
void emptyTrash();
/// Return the keyframe which was last inserted in the map.
inline FramePtr lastKeyframe() { return keyframes_.back(); }
/// Return the number of keyframes in the map
inline size_t size() const { return keyframes_.size(); }
};
/// A collection of debug functions to check the data consistency.
namespace map_debug {
void mapStatistics(Map* map);
void mapValidation(Map* map, int id);
void frameValidation(Frame* frame, int id);
void pointValidation(Point* point, int id);
} // namespace map_debug
} // namespace svo
#endif // SVO_MAP_H_
GPU_version/svo/include/gpu_svo/matcher.h
+130
View File
@@ -0,0 +1,130 @@
// 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_MATCHER_H_
#define SVO_MATCHER_H_
#include <gpu_svo/global.h>
namespace vk {
class AbstractCamera;
namespace patch_score {
template<int HALF_PATCH_SIZE> class ZMSSD;
}
}
namespace svo {
class Point;
class Frame;
class Feature;
/// Warp a patch from the reference view to the current view.
namespace warp {
void getWarpMatrixAffine(
const vk::AbstractCamera& cam_ref,
const vk::AbstractCamera& cam_cur,
const Vector2d& px_ref,
const Vector3d& f_ref,
const double depth_ref,
const SE3& T_cur_ref,
const int level_ref,
Matrix2d& A_cur_ref);
int getBestSearchLevel(
const Matrix2d& A_cur_ref,
const int max_level);
void warpAffine(
const Matrix2d& A_cur_ref,
const cv::Mat& img_ref,
const Vector2d& px_ref,
const int level_ref,
const int level_cur,
const int halfpatch_size,
uint8_t* patch);
} // namespace warp
/// Patch-matcher for reprojection-matching and epipolar search in triangulation.
class Matcher
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static const int halfpatch_size_ = 4;
static const int patch_size_ = 8;
typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;
struct Options
{
bool align_1d; //!< in epipolar search: align patch 1D along epipolar line
int align_max_iter; //!< number of iterations for aligning the feature patches in gauss newton
double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align
size_t max_epi_search_steps;//!< max number of evaluations along epipolar line
bool subpix_refinement; //!< do gauss newton feature patch alignment after epipolar search
bool epi_search_edgelet_filtering;
double epi_search_edgelet_max_angle;
Options() :
align_1d(false),
align_max_iter(10),
max_epi_length_optim(2.0),
max_epi_search_steps(1000),
subpix_refinement(true),
epi_search_edgelet_filtering(true),
epi_search_edgelet_max_angle(0.7)
{}
} options_;
uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16)));
uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
Matrix2d A_cur_ref_; //!< affine warp matrix
Vector2d epi_dir_;
double epi_length_; //!< length of epipolar line segment in pixels (only used for epipolar search)
double h_inv_; //!< hessian of 1d image alignment along epipolar line
int search_level_;
bool reject_;
Feature* ref_ftr_;
Vector2d px_cur_;
Matcher() = default;
~Matcher() = default;
/// Find a match by directly applying subpix refinement.
/// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result!
bool findMatchDirect(
const Point& pt,
const Frame& frame,
Vector2d& px_cur);
/// Find a match by searching along the epipolar line without using any features.
bool findEpipolarMatchDirect(
const Frame& ref_frame,
const Frame& cur_frame,
const Feature& ref_ftr,
const double d_estimate,
const double d_min,
const double d_max,
double& depth);
void createPatchFromPatchWithBorder();
};
} // namespace svo
#endif // SVO_MATCHER_H_
GPU_version/svo/include/gpu_svo/point.h
+108
View File
@@ -0,0 +1,108 @@
// 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_POINT_H_
#define SVO_POINT_H_
#include <boost/noncopyable.hpp>
#include <gpu_svo/global.h>
namespace g2o {
class VertexSBAPointXYZ;
}
typedef g2o::VertexSBAPointXYZ g2oPoint;
namespace svo {
class Feature;
typedef Matrix<double, 2, 3> Matrix23d;
/// A 3D point on the surface of the scene.
class Point : boost::noncopyable
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum PointType {
TYPE_DELETED,
TYPE_CANDIDATE,
TYPE_UNKNOWN,
TYPE_GOOD
};
static int point_counter_; //!< Counts the number of created points. Used to set the unique id.
int id_; //!< Unique ID of the point.
Vector3d pos_; //!< 3d pos of the point in the world coordinate frame.
Vector3d normal_; //!< Surface normal at point.
Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation.
bool normal_set_; //!< Flag whether the surface normal was estimated or not.
list<Feature*> obs_; //!< References to keyframes which observe the point.
size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
int last_published_ts_; //!< Timestamp of last publishing.
int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice.
PointType type_; //!< Quality of the point.
int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point.
int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point.
int last_structure_optim_; //!< Timestamp of last point optimization
Point(const Vector3d& pos);
Point(const Vector3d& pos, Feature* ftr);
~Point();
/// Add a reference to a frame.
void addFrameRef(Feature* ftr);
/// Remove reference to a frame.
bool deleteFrameRef(Frame* frame);
/// Initialize point normal. The inital estimate will point towards the frame.
void initNormal();
/// Check whether mappoint has reference to a frame.
Feature* findFrameRef(Frame* frame);
/// Get Frame with similar viewpoint.
bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const;
/// Get number of observations.
inline size_t nRefs() const { return obs_.size(); }
/// Optimize point position through minimizing the reprojection error.
void optimize(const size_t n_iter);
/// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
inline static void jacobian_xyz2uv(
const Vector3d& p_in_f,
const Matrix3d& R_f_w,
Matrix23d& point_jac)
{
const double z_inv = 1.0/p_in_f[2];
const double z_inv_sq = z_inv*z_inv;
point_jac(0, 0) = z_inv;
point_jac(0, 1) = 0.0;
point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
point_jac(1, 0) = 0.0;
point_jac(1, 1) = z_inv;
point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
point_jac = - point_jac * R_f_w;
}
};
} // namespace svo
#endif // SVO_POINT_H_
GPU_version/svo/include/gpu_svo/pose_optimizer.h
+50
View File
@@ -0,0 +1,50 @@
// 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_POSE_OPTIMIZER_H_
#define SVO_POSE_OPTIMIZER_H_
#include <gpu_svo/global.h>
namespace svo {
using namespace Eigen;
using namespace Sophus;
using namespace std;
typedef Matrix<double,6,6> Matrix6d;
typedef Matrix<double,2,6> Matrix26d;
typedef Matrix<double,6,1> Vector6d;
class Point;
/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame.
namespace pose_optimizer {
void optimizeGaussNewton(
const double reproj_thresh,
const size_t n_iter,
const bool verbose,
FramePtr& frame,
double& estimated_scale,
double& error_init,
double& error_final,
size_t& num_obs);
} // namespace pose_optimizer
} // namespace svo
#endif // SVO_POSE_OPTIMIZER_H_
GPU_version/svo/include/gpu_svo/reprojector.h
+101
View File
@@ -0,0 +1,101 @@
// 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_REPROJECTION_H_
#define SVO_REPROJECTION_H_
#include <gpu_svo/global.h>
#include <gpu_svo/matcher.h>
namespace vk {
class AbstractCamera;
}
namespace svo {
class Map;
class Point;
/// Project points from the map into the image and find the corresponding
/// feature (corner). We don't search a match for every point but only for one
/// point per cell. Thereby, we achieve a homogeneously distributed set of
/// matched features and at the same time we can save processing time by not
/// projecting all points.
class Reprojector
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Reprojector config parameters
struct Options {
size_t max_n_kfs; //!< max number of keyframes to reproject from
bool find_match_direct;
Options()
: max_n_kfs(10),
find_match_direct(true)
{}
} options_;
size_t n_matches_;
size_t n_trials_;
Reprojector(vk::AbstractCamera* cam, Map& map);
~Reprojector();
/// Project points from the map into the image. First finds keyframes with
/// overlapping field of view and projects only those map-points.
void reprojectMap(
FramePtr frame,
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs);
private:
/// A candidate is a point that projects into the image plane and for which we
/// will search a maching feature in the image.
struct Candidate {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Point* pt; //!< 3D point.
Vector2d px; //!< projected 2D pixel location.
Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
};
typedef std::list<Candidate > Cell;
typedef std::vector<Cell*> CandidateGrid;
/// The grid stores a set of candidate matches. For every grid cell we try to find one match.
struct Grid
{
CandidateGrid cells;
vector<int> cell_order;
int cell_size;
int grid_n_cols;
int grid_n_rows;
};
Grid grid_;
Matcher matcher_;
Map& map_;
static bool pointQualityComparator(Candidate& lhs, Candidate& rhs);
void initializeGrid(vk::AbstractCamera* cam);
void resetGrid();
bool reprojectCell(Cell& cell, FramePtr frame);
bool reprojectPoint(FramePtr frame, Point* point);
};
} // namespace svo
#endif // SVO_REPROJECTION_H_
GPU_version/svo/include/gpu_svo/sparse_img_align.h
+83
View File
@@ -0,0 +1,83 @@
// 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_SPARSE_IMG_ALIGN_H_
#define SVO_SPARSE_IMG_ALIGN_H_
#include <vikit/nlls_solver.h>
#include <vikit/performance_monitor.h>
#include <gpu_svo/global.h>
namespace vk {
class AbstractCamera;
}
namespace svo {
class Feature;
/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
class SparseImgAlign : public vk::NLLSSolver<6, SE3>
{
static const int patch_halfsize_ = 2;
static const int patch_size_ = 2*patch_halfsize_;
static const int patch_area_ = patch_size_*patch_size_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
cv::Mat resimg_;
SparseImgAlign(
int n_levels,
int min_level,
int n_iter,
Method method,
bool display,
bool verbose);
size_t run(
FramePtr ref_frame,
FramePtr cur_frame);
/// Return fisher information matrix, i.e. the Hessian of the log-likelihood
/// at the converged state.
Matrix<double, 6, 6> getFisherInformation();
protected:
FramePtr ref_frame_; //!< reference frame, has depth for gradient pixels.
FramePtr cur_frame_; //!< only the image is known!
int level_; //!< current pyramid level on which the optimization runs.
bool display_; //!< display residual image.
int max_level_; //!< coarsest pyramid level for the alignment.
int min_level_; //!< finest pyramid level for the alignment.
// cache:
Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_;
bool have_ref_patch_cache_;
cv::Mat ref_patch_cache_;
std::vector<bool> visible_fts_;
void precomputeReferencePatches();
virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false);
virtual int solve();
virtual void update (const ModelType& old_model, ModelType& new_model);
virtual void startIteration();
virtual void finishIteration();
};
} // namespace svo
#endif // SVO_SPARSE_IMG_ALIGN_H_
GPU_version/svo/include/gpu_svo/svoVisionFactor.h
+70
View File
@@ -0,0 +1,70 @@
//
// Created by root on 4/13/21.
//
#ifndef SVO_SVOVISIONFACTOR_H
#define SVO_SVOVISIONFACTOR_H
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose3.h>
#include <vikit/robust_cost.h>
#include <vikit/math_utils.h>
#include <gpu_svo/frame.h>
namespace svo {
class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
private:
// measurement information
gtsam::Point3 feature; ///<Unit-bearing vector of the feature.
gtsam::Point3 landmark; ///<Unit-bearing vector of the feature.
boost::optional<gtsam::Point3> body_P_sensor_; ///< TODO The pose of the sensor in the body frame
int level; ///< Image pyramid level where feature was extracted.
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<VisionFactor> shared_ptr;
/**
* Constructor
* @param feature Unit-bearing vector of the feature.
* @param model is the standard deviation
* @param pointKey is the index of the landmark
* @param poseKey is the index of the camera pose
* @param landmark 3D position of the landmark
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param level Image pyramid level where feature was extracted.
*/
VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp,
gtsam::Key poseKey,boost::optional<gtsam::Point3> body_P_sensor = boost::none) :
gtsam::NoiseModelFactor1<gtsam::Pose3>(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){}
virtual ~VisionFactor(){}
/// error function
/// @param p the Estimated pose in Pose3
/// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> Hp = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
gtsam::Matrix26 J;
gtsam::Point3 point(p*landmark);
svo::Frame::jacobian_xyz2uv(point, J);
Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J * 1.0 / (1<<level)).finished();
return e;
}
// The second is a 'clone' function that allows the factor to be copied. Under most
// circumstances, the following code that employs the default copy constructor should
// work fine.
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); }
};
} // namespace gtsamexamples
#endif //SVO_SVOVISIONFACTOR_H