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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
VahdPourAmin
2021-07-24 22:01:17 +02:00
parent a2802878f4 ed5a9139ff
commit a83bba4399
38 changed files with 19 additions and 17196 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/gpu_svo/abstract_camera.h
-77
View File
@@ -1,77 +0,0 @@
/*
* abstract_camera.h
*
* Created on: Jul 23, 2012
* Author: cforster
*/
#ifndef ABSTRACT_CAMERA_H_
#define ABSTRACT_CAMERA_H_
#include <Eigen/Core>
namespace vk
{
using namespace std;
using namespace Eigen;
class AbstractCamera
{
protected:
int width_; // TODO cannot be const because of omni-camera model
int height_;
public:
AbstractCamera() {}; // need this constructor for omni camera
AbstractCamera(int width, int height) : width_(width), height_(height) {};
virtual ~AbstractCamera() {};
/// Project from pixels to world coordiantes. Returns a bearing vector of unit length.
virtual Vector3d
cam2world(const double& x, const double& y) const = 0;
/// Project from pixels to world coordiantes. Returns a bearing vector of unit length.
virtual Vector3d
cam2world(const Vector2d& px) const = 0;
virtual Vector2d
world2cam(const Vector3d& xyz_c) const = 0;
/// projects unit plane coordinates to camera coordinates
virtual Vector2d
world2cam(const Vector2d& uv) const = 0;
virtual double
errorMultiplier2() const = 0;
virtual double
errorMultiplier() const = 0;
inline int width() const { return width_; }
inline int height() const { return height_; }
inline bool isInFrame(const Vector2i & obs, int boundary=0) const
{
if(obs[0]>=boundary && obs[0]<width()-boundary
&& obs[1]>=boundary && obs[1]<height()-boundary)
return true;
return false;
}
inline bool isInFrame(const Vector2i &obs, int boundary, int level) const
{
if(obs[0] >= boundary && obs[0] < width()/(1<<level)-boundary
&& obs[1] >= boundary && obs[1] <height()/(1<<level)-boundary)
return true;
return false;
}
};
} // end namespace CSfM
#endif /* ABSTRACT_CAMERA_H_ */
GPU_version/vio/include/gpu_svo/aligned_mem.h
-127
View File
@@ -1,127 +0,0 @@
/*
* aligned_mem.h
*
* Created on: May 14, 2013
* Author: cforster
*
* Code from libcvd: cvd/internal/aligned_mem.h
* https://github.com/edrosten/libcvd
* Licence: LGPL 2.1
*/
#ifndef VIKIT_ALIGNED_MEM_H_
#define VIKIT_ALIGNED_MEM_H_
#include <string> // memset
#include <string.h> // memset
#include <cassert>
#include <cstdlib>
namespace vk {
namespace aligned_mem {
/// Check if the pointer is aligned to the specified byte granularity
inline bool
is_aligned8(const void* ptr)
{
return ((reinterpret_cast<size_t>(ptr)) & 0x7) == 0;
}
inline bool
is_aligned16(const void* ptr)
{
return ((reinterpret_cast<size_t>(ptr)) & 0xF) == 0;
}
template <class T, int N=20> struct placement_delete
{
enum { Size = (1<<N) };
struct Array {
T data[Size];
};
static inline void destruct(T* buf)
{
(*(Array*)buf).~Array();
}
static inline void free(T* buf, size_t M)
{
if (M >= Size) {
placement_delete<T,N>::free(buf+Size,M-Size);
placement_delete<T,N>::destruct(buf);
} else {
placement_delete<T,N-1>::free(buf, M);
}
}
};
template <class T> struct placement_delete<T,-1>
{
static inline void free(T*, size_t ) {}
};
inline void * aligned_alloc(size_t count, size_t alignment){
void * mem = NULL;
assert(posix_memalign(&mem, alignment, count) == 0);
return mem;
}
inline void aligned_free(void * memory) {
free(memory);
}
template <class T>
inline T * aligned_alloc(size_t count, size_t alignment){
void * data = aligned_alloc(sizeof(T)* count, alignment);
return new (data) T[count];
}
template <class T>
inline void aligned_free(T * memory, size_t count){
placement_delete<T>::free(memory, count);
aligned_free(memory);
}
template<class T> inline void memfill(T* data, int n, const T val)
{
T* de = data + n;
for(;data < de; data++)
*data=val;
}
template<> inline void memfill(unsigned char* data, int n, const unsigned char val)
{
memset(data, val, n);
}
template<> inline void memfill(signed char* data, int n, const signed char val)
{
memset(data, val, n);
}
template<> inline void memfill(char* data, int n, const char val)
{
memset(data, val, n);
}
template <class T, int N>
struct AlignedMem {
T* mem;
size_t count;
AlignedMem(size_t c) : count(c) {
mem = aligned_alloc<T>(count, N);
}
~AlignedMem() {
aligned_free<T>(mem, count);
}
T* data() { return mem; }
const T* data() const { return mem; }
};
} // namespace aligned_mem
} // namespace vikit
#endif // VIKIT_ALIGNED_MEM_H_
GPU_version/vio/include/gpu_svo/atan_camera.h
-91
View File
@@ -1,91 +0,0 @@
/*
* atan_camera.h
*
* Created on: Aug 21, 2012
* Author: cforster
*
* This class implements the FOV distortion model of Deverneay and Faugeras,
* Straight lines have to be straight, 2001.
*
* The code is an implementation of the ATAN class in PTAM by Georg Klein using Eigen.
*/
#ifndef ATAN_CAMERA_H_
#define ATAN_CAMERA_H_
#include <stdlib.h>
#include <string>
#include <Eigen/Eigen>
#include <gpu_svo/abstract_camera.h>
#include <gpu_svo/math_utils.h>
namespace vk {
using namespace std;
using namespace Eigen;
class ATANCamera : public AbstractCamera {
private:
double fx_, fy_; //!< focal length
double fx_inv_, fy_inv_; //!< inverse focal length
double cx_, cy_; //!< projection center
double s_, s_inv_; //!< distortion model coeff
double tans_; //!< distortion model coeff
double tans_inv_; //!< distortion model coeff
bool distortion_; //!< use distortion model?
//! Radial distortion transformation factor: returns ration of distorted / undistorted radius.
inline double rtrans_factor(double r) const
{
if(r < 0.001 || s_ == 0.0)
return 1.0;
else
return (s_inv_* atan(r * tans_) / r);
};
//! Inverse radial distortion: returns un-distorted radius from distorted.
inline double invrtrans(double r) const
{
if(s_ == 0.0)
return r;
return (tan(r * s_) * tans_inv_);
};
public:
ATANCamera(double width, double height, double fx, double fy, double dx, double dy, double s);
~ATANCamera();
virtual Vector3d
cam2world(const double& x, const double& y) const;
virtual Vector3d
cam2world(const Vector2d& px) const;
virtual Vector2d
world2cam(const Vector3d& xyz_c) const;
virtual Vector2d
world2cam(const Vector2d& uv) const;
const Vector2d focal_length() const
{
return Vector2d(fx_, fy_);
}
virtual double errorMultiplier2() const
{
return fx_;
}
virtual double errorMultiplier() const
{
return 4*fx_*fy_;
}
};
} // end namespace vk
#endif /* ATAN_CAMERA_H_ */
GPU_version/vio/include/gpu_svo/bundle_adjustment.h
-109
View File
@@ -1,109 +0,0 @@
// 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/vio/include/gpu_svo/camera_loader.h
-46
View File
@@ -1,46 +0,0 @@
/*
* camera_loader.h
*
* Created on: Feb 11, 2014
* Author: cforster
*/
#ifndef VIKIT_CAMERA_LOADER_H_
#define VIKIT_CAMERA_LOADER_H_
#include <string>
#include <gpu_svo/abstract_camera.h>
#include <gpu_svo/atan_camera.h>
#include <gpu_svo/params_helper.h>
namespace vk {
namespace camera_loader {
/// Load from ROS Namespace
bool loadFromRosNs(const std::string& ns, vk::AbstractCamera*& cam)
{
bool res = true;
std::string cam_model(getParam<std::string>(ns+"/cam_model"));
if(cam_model == "ATAN")
{
cam = new vk::ATANCamera(
getParam<int>(ns+"/cam_width"),
getParam<int>(ns+"/cam_height"),
getParam<double>(ns+"/cam_fx"),
getParam<double>(ns+"/cam_fy"),
getParam<double>(ns+"/cam_cx"),
getParam<double>(ns+"/cam_cy"),
getParam<double>(ns+"/cam_d0"));
}
else
{
cam = NULL;
res = false;
}
return res;
}
} // namespace camera_loader
} // namespace vk
#endif // VIKIT_CAMERA_LOADER_H_
GPU_version/vio/include/gpu_svo/cl.hpp
-12947
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/gpu_svo/cl_class.h
-191
View File
@@ -1,191 +0,0 @@
//
// Created by root on 4/27/21.
//
#ifndef VIO_OPENCL_CL_CLASS_H
#define VIO_OPENCL_CL_CLASS_H
#include <gpu_svo/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/vio/include/gpu_svo/config.h
-182
View File
@@ -1,182 +0,0 @@
// 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; }
/// Gro 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; }
static double& Svo_Ekf() { return getInstance().svo_ekf; }
static double& X_off() { return getInstance().acc_x_off; }
static double& Y_off() { return getInstance().acc_y_off; }
static double& Teta_off() { return getInstance().gyr_z_off; }
static double& Cmd_Cov() { return getInstance().cmd_cov; }
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,svo_ekf,acc_x_off,acc_y_off,gyr_z_off,cmd_cov;
};
} // namespace svo
#endif // SVO_CONFIG_H_
GPU_version/vio/include/gpu_svo/depth_filter.h
-170
View File
@@ -1,170 +0,0 @@
// 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(10),
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, 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.
Matcher matcher_;
uint seeds_cont_=0;
/// 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/vio/include/gpu_svo/feature.h
-75
View File
@@ -1,75 +0,0 @@
// 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/vio/include/gpu_svo/feature_alignment.h
-63
View File
@@ -1,63 +0,0 @@
// 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/vio/include/gpu_svo/feature_detection.h
-111
View File
@@ -1,111 +0,0 @@
// 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/vio/include/gpu_svo/for_it.hpp
-87
View File
@@ -1,87 +0,0 @@
//
// Created by root on 5/12/21.
//
#ifndef GPU_SVO_FOR_IT_H
#define GPU_SVO_FOR_IT_H
#include <iostream>
#include <vector>
#include <tuple>
//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop
// Wrapper class
template <typename T>
class for_impl
{
public:
// The return value of the operator* of the iterator, this
// is what you will get inside of the for loop
struct item
{
size_t index;
typename T::value_type & item;
};
typedef item value_type;
// Custom iterator with minimal interface
struct iterator
{
iterator(typename T::iterator _it, size_t counter=0) :
it(_it), counter(counter)
{}
iterator operator++()
{
return iterator(++it, ++counter);
}
bool operator!=(iterator other)
{
return it != other.it;
}
typename T::iterator::value_type item()
{
return *it;
}
value_type operator*()
{
return value_type{counter, *it};
}
size_t index()
{
return counter;
}
private:
typename T::iterator it;
size_t counter;
};
for_impl(T & t) : container(t) {}
iterator begin()
{
return iterator(container.begin());
}
iterator end()
{
return iterator(container.end());
}
private:
T & container;
};
// A templated free function allows you to create the wrapper class
// conveniently
template <typename T>
for_impl<T> _for(T & t)
{
return for_impl<T>(t);
}
#endif //GPU_SVO_FOR_IT_H
GPU_version/vio/include/gpu_svo/frame.h
-185
View File
@@ -1,185 +0,0 @@
// 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 <gpu_svo/math_utils.h>
#include <gpu_svo/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.
SE2_5 T_f_w_; //!< Transform (f)rame from (w)orld.
Matrix<double, 3, 3> 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_.se3()*xyz_w);
}
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); }
/// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); }
/// 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 Vector3d(T_f_w_.se3()*xyz_w);
}
/// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
inline Vector3d f2w(const Vector3d& f) const {
return getSE3Inv() * 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 Vector2d pos() const { return T_f_w_.se2().translation(); }
inline SE3 se3() const{
return T_f_w_.se3();
}
inline SE3 getSE3Inv() const{
Eigen::Matrix<double,3,3> R;
R<<T_f_w_.inverse().rotation_matrix()(0,0),1e-19,T_f_w_.inverse().rotation_matrix()(0,1),
1e-19,1.0,1e-19,
T_f_w_.inverse().rotation_matrix()(1,0),1e-19,T_f_w_.inverse().rotation_matrix()(1,1);
SE3 out(R,Vector3d(T_f_w_.inverse().translation()(0),1e-19,T_f_w_.inverse().translation()(1)));
return out;
}
/// 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,3>& 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) = x*z_inv_2; // x/z^2
J(0,2) = -(1.0 + pow(x,2)/z_inv_2); // -(1.0 + x^2/z^2)
J(1,0) = 1e-19; // 0
J(1,1) = y*z_inv_2; // y/z^2
J(1,2) = -x*y/z_inv_2; // -x*y/z^2
}
inline static void jacobian_l2uv(
const Matrix<double,3,1>& p,
Matrix<double,2,3>& J){
double A = -1.0*sqrt(p.z());
double z_inv = -1/p.z();
J(0,0) = z_inv;
J(0,1) = 1e-19;
J(0,2) = p.x()*A;
J(1,0) = 1e-19;
J(1,1) = z_inv;
J(1,2) =A*p.y();
}
};
/// 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/vio/include/gpu_svo/frame_handler_base.h
-118
View File
@@ -1,118 +0,0 @@
// 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 <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.
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/vio/include/gpu_svo/frame_handler_mono.h
-109
View File
@@ -1,109 +0,0 @@
// 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 <gpu_svo/abstract_camera.h>
#include <gpu_svo/frame_handler_base.h>
#include <gpu_svo/reprojector.h>
#include <gpu_svo/initialization.h>
#include <gpu_svo/ukf.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,Eigen::Matrix<double,3,1>& init);
virtual ~FrameHandlerMono();
/// Provide an image.
void addImage(const cv::Mat& img, double timestamp,const ros::Time& time);
/// 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::shared_ptr<Imu_Integration> imu_integPtr_; //Integration of IMU
void UpdateIMU(double* value,const ros::Time& time);
void UpdateCmd(double* value,const ros::Time& time);
UKF ukfPtr_;
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_;
ros::Time time_;
/// 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/vio/include/gpu_svo/global.h
-120
View File
@@ -1,120 +0,0 @@
// 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/se2.h>
#include <sophus/se3.h>
#include <boost/shared_ptr.hpp>
#include<Eigen/StdVector>
#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
#endif
#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)
namespace svo
{
using namespace Eigen;
using namespace Sophus;
const double EPS = 0.0000000001;
const double PI = 3.14159265;
class SE2_5: public SE2{
public:
SE2_5(SE2&& se2):T2_(se2){
}
SE2_5(SE2& se2):T2_(se2){
}
SE2_5(SE3&& se3){
Quaterniond q=se3.unit_quaternion().normalized();
double pitch=asin(2.0*(q.w()*q.y()-q.z()*q.x()));
if (std::abs(pitch) >= 1)
pitch = std::copysign(M_PI / 2.0, pitch); // use 90 degrees if out of range
else
pitch = std::asin(pitch);
Eigen::Matrix<double, 2,2> R;
R<<cos(pitch),sin(pitch),
-sin(pitch),cos(pitch);
T2_ = SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
}
SE2_5(SE3& se3){
Quaterniond q=se3.unit_quaternion().normalized();
double pitch=asin(2.0*(q.w()*q.y()-q.z()*q.x()));
if (std::abs(pitch) >= 1)
pitch = std::copysign(M_PI / 2.0, pitch); // use 90 degrees if out of range
else
pitch = std::asin(pitch);
Eigen::Matrix<double, 2,2> R;
R<<cos(pitch),sin(pitch),
-sin(pitch),cos(pitch);
T2_=SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
}
SE2_5(double x,double z,double pitch){
Eigen::Matrix<double, 2,2> R;
R<<cos(pitch),sin(pitch),
-sin(pitch),cos(pitch);
T2_=SE2(R,Vector2d(x,z));
};
SE2 se2() const{
return T2_;
}
SE2 inverse() const{
double yaw=atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0));
if(yaw<0.0)
yaw=M_PI-yaw;
else
yaw=yaw-M_PI;
return SE2(yaw,-1.0*T2_.translation());
}
double pitch()const{
return atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0));
}
SE3 se3() const{
Eigen::Matrix<double,3,3> R;
R<<T2_.rotation_matrix()(0,0),0.0,T2_.rotation_matrix()(0,1),
0.0,1.0,0.0,
T2_.rotation_matrix()(1,0),0.0,T2_.rotation_matrix()(1,1);
return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
}
~SE2_5(){}
private:
SE2 T2_;
};
class Frame;
typedef boost::shared_ptr<Frame> FramePtr;
} // namespace svo
#endif // SVO_GLOBAL_H_
GPU_version/vio/include/gpu_svo/homography.h
-85
View File
@@ -1,85 +0,0 @@
/*
* homography.cpp
* Adaptation of PTAM-GPL HomographyInit class.
* https://github.com/Oxford-PTAM/PTAM-GPL
* Licence: GPLv3
* Copyright 2008 Isis Innovation Limited
*
* Created on: Sep 2, 2012
* by: cforster
*
* This class implements the homography decomposition of Faugeras and Lustman's
* 1988 tech report. Code converted to Eigen from PTAM.
*
*/
#ifndef HOMOGRAPHY_H_
#define HOMOGRAPHY_H_
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/SVD>
#include <gpu_svo/math_utils.h>
namespace vk {
using namespace Eigen;
using namespace std;
struct HomographyDecomposition
{
Vector3d t;
Matrix3d R;
double d;
Vector3d n;
// Resolved Composition
Sophus::SE3 T; //!< second from first
int score;
};
class Homography
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Homography (const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
const vector<Vector2d, aligned_allocator<Vector2d> >& _fts2,
double _error_multiplier2,
double _thresh_in_px);
void
calcFromPlaneParams (const Vector3d & normal,
const Vector3d & point_on_plane);
void
calcFromMatches ();
size_t
computeMatchesInliers ();
bool
computeSE3fromMatches ();
bool
decompose ();
void
findBestDecomposition ();
double thresh;
double error_multiplier2;
const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c1; //!< Features on first image on unit plane
const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c2; //!< Features on second image on unit plane
vector<bool> inliers;
SE3 T_c2_from_c1; //!< Relative translation and rotation of two images
Matrix3d H_c2_from_c1; //!< Homography
vector<HomographyDecomposition> decompositions;
};
} /* end namespace vk */
#endif /* HOMOGRAPHY_H_ */
GPU_version/vio/include/gpu_svo/initialization.h
-87
View File
@@ -1,87 +0,0 @@
// 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>
//#include "../../gtsam/imu_integration.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_),T_cur_from_ref_(0.0,0.0,0.0) {};
~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.
SE2_5 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,
SE2_5& T_cur_from_ref);
} // namespace initialization
} // namespace svo
#endif // SVO_INITIALIZATION_H
GPU_version/vio/include/gpu_svo/map.h
-137
View File
@@ -1,137 +0,0 @@
// 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();
};
static bool syn_=false;
/// 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 Vector2d& 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 Matrix2d& R, const Vector2d& 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/vio/include/gpu_svo/matcher.h
-130
View File
@@ -1,130 +0,0 @@
// 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);
bool 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/vio/include/gpu_svo/math_utils.h
-171
View File
@@ -1,171 +0,0 @@
/*
* math_utils.h
*
* Created on: Jul 20, 2012
* Author: cforster
*/
#ifndef MATH_UTILS_H_
#define MATH_UTILS_H_
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <sophus/se3.h>
namespace vk
{
using namespace Eigen;
using namespace std;
using namespace Sophus;
Vector3d triangulateFeatureNonLin(
const Matrix3d& R,
const Vector3d& t,
const Vector3d& feature1,
const Vector3d& feature2);
/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e.
/// perfect triangulation without noise!
bool depthFromTriangulationExact(
const Matrix3d& R_r_c,
const Vector3d& t_r_c,
const Vector3d& f_r,
const Vector3d& f_c,
double& depth_in_r,
double& depth_in_c);
double reprojError(
const Vector3d& f1,
const Vector3d& f2,
double error_multiplier2);
double computeInliers(
const vector<Vector3d>& features1,
const vector<Vector3d>& features2,
const Matrix3d& R,
const Vector3d& t,
const double reproj_thresh,
double error_multiplier2,
vector<Vector3d>& xyz_vec,
vector<int>& inliers,
vector<int>& outliers);
void computeInliersOneView(
const vector<Vector3d> & feature_sphere_vec,
const vector<Vector3d> & xyz_vec,
const Matrix3d &R,
const Vector3d &t,
const double reproj_thresh,
const double error_multiplier2,
vector<int>& inliers,
vector<int>& outliers);
//! Direct Cosine Matrix to Roll Pitch Yaw
Vector3d dcm2rpy(const Matrix3d &R);
//! Roll Pitch Yaw to Direct Cosine Matrix
Matrix3d rpy2dcm(const Vector3d &rpy);
//! Angle Axis parametrization to Quaternion
Quaterniond angax2quat(const Vector3d& n, const double& angle);
//! Angle Axis parametrization to Matrix representation
Matrix3d angax2dcm(const Vector3d& n, const double& angle);
double sampsonusError(
const Vector2d &v2Dash,
const Matrix3d& m3Essential,
const Vector2d& v2);
inline Matrix3d sqew(const Vector3d& v)
{
Matrix3d v_sqew;
v_sqew << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
return v_sqew;
}
inline double norm_max(const Eigen::VectorXd & v)
{
double max = -1;
for (int i=0; i<v.size(); i++)
{
double abs = fabs(v[i]);
if(abs>max){
max = abs;
}
}
return max;
}
inline Vector2d project2d(const Vector3d& v)
{
return v.head<2>()/v[2];
}
inline Vector3d unproject2d(const Vector2d& v)
{
return Vector3d(v[0], v[1], 1.0);
}
inline Vector3d project3d(const Vector4d& v)
{
return v.head<3>()/v[3];
}
inline Vector4d unproject3d(const Vector3d& v)
{
return Vector4d(v[0], v[1], v[2], 1.0);
}
template<class T>
T getMedian(vector<T>& data_vec)
{
assert(!data_vec.empty());
typename vector<T>::iterator it = data_vec.begin()+floor(data_vec.size()/2);
nth_element(data_vec.begin(), it, data_vec.end());
return *it;
}
inline double pyrFromZero_d(double x_0, int level)
{
return x_0/(1<<level); // = 1 / 2^level
}
inline Vector2d pyrFromZero_2d(const Vector2d& uv_0, int level)
{
return Vector2d(pyrFromZero_d(uv_0[0], level),
pyrFromZero_d(uv_0[1], level));
}
inline void
frameJac_xyz2uv(const Vector3d & xyz,
const double & focal_length,
Matrix<double,2,6> & frame_jac)
{
const double x = xyz[0];
const double y = xyz[1];
const double z = xyz[2];
const double z_2 = z*z;
frame_jac(0,0) = -1./z *focal_length;
frame_jac(0,1) = 0;
frame_jac(0,2) = x/z_2 *focal_length;
frame_jac(0,3) = x*y/z_2 * focal_length;
frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length;
frame_jac(0,5) = y/z *focal_length;
frame_jac(1,0) = 0;
frame_jac(1,1) = -1./z *focal_length;
frame_jac(1,2) = y/z_2 *focal_length;
frame_jac(1,3) = (1+y*y/z_2) *focal_length;
frame_jac(1,4) = -x*y/z_2 *focal_length;
frame_jac(1,5) = -x/z *focal_length;
}
} // end namespace vk
#endif /* MATH_UTILS_H_ */
GPU_version/vio/include/gpu_svo/nlls_solver.h
-169
View File
@@ -1,169 +0,0 @@
/*
* Abstract Nonlinear Least-Squares Solver Class
*
* nlls_solver.h
*
* Created on: Nov 5, 2012
* Author: cforster
*/
#ifndef LM_SOLVER_H_
#define LM_SOLVER_H_
#include <stdint.h>
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/robust_cost.h>
namespace vk {
using namespace std;
using namespace Eigen;
/**
* \brief Abstract Class for solving nonlinear least-squares (NLLS) problems.
*
* The function implements two algorithms: Levenberg Marquardt and Gauss Newton
*
* Example implementations of this function can be found in the rpl_examples
* package: img_align_2d.cpp, img_align_3d.cpp
*
* Template Parameters:
* D : dimension of the residual
* T : type of the model, e.g. SE2, SE3
*/
template <int D, typename T>
class NLLSSolver {
public:
typedef T ModelType;
enum Method{GaussNewton, LevenbergMarquardt};
enum ScaleEstimatorType{UnitScale, TDistScale, MADScale, NormalScale};
enum WeightFunctionType{UnitWeight, TDistWeight, TukeyWeight, HuberWeight};
protected:
Matrix<double, D, D> H_; //!< Hessian approximation
Matrix<double, D, 1> Jres_; //!< Jacobian x Residual
Matrix<double, D, 1> x_; //!< update step
bool have_prior_;
ModelType prior_;
Matrix<double, D, D> I_prior_; //!< Prior information matrix (inverse covariance)
double chi2_;
double rho_;
Method method_;
/// If the flag linearize_system is set, the function must also compute the
/// Jacobian and set the member variables H_, Jres_
virtual double
computeResiduals (const ModelType& model,
bool linearize_system,
bool compute_weight_scale) = 0;
/// Solve the linear system H*x = Jres. This function must set the update
/// step in the member variable x_. Must return true if the system could be
/// solved and false if it was singular.
virtual int
solve () = 0;
virtual void
update (const ModelType& old_model, ModelType& new_model) = 0;
virtual void
applyPrior (const ModelType& current_model) { }
virtual void
startIteration () { }
virtual void
finishIteration () { }
virtual void
finishTrial () { }
public:
/// Damping parameter. If mu > 0, coefficient matrix is positive definite, this
/// ensures that x is a descent direction. If mu is large, x is a short step in
/// the steepest direction. This is good if the current iterate is far from the
/// solution. If mu is small, LM approximates gauss newton iteration and we
/// have (almost) quadratic convergence in the final stages.
double mu_init_, mu_;
double nu_init_, nu_; //!< Increase factor of mu after fail
size_t n_iter_init_, n_iter_; //!< Number of Iterations
size_t n_trials_; //!< Number of trials
size_t n_trials_max_; //!< Max number of trials
size_t n_meas_; //!< Number of measurements
bool stop_; //!< Stop flag
bool verbose_; //!< Output Statistics
double eps_; //!< Stop if update norm is smaller than eps
size_t iter_; //!< Current Iteration
// robust least squares
bool use_weights_;
float scale_;
robust_cost::ScaleEstimatorPtr scale_estimator_;
robust_cost::WeightFunctionPtr weight_function_;
NLLSSolver() :
have_prior_(false),
method_(LevenbergMarquardt),
mu_init_(0.01f),
mu_(mu_init_),
nu_init_(2.0),
nu_(nu_init_),
n_iter_init_(15),
n_iter_(n_iter_init_),
n_trials_(0),
n_trials_max_(5),
n_meas_(0),
stop_(false),
verbose_(true),
eps_(0.0000000001),
iter_(0),
use_weights_(false),
scale_(0.0),
scale_estimator_(NULL),
weight_function_(NULL)
{ }
virtual ~NLLSSolver() {}
/// Calls the GaussNewton or LevenbergMarquardt optimization strategy
void optimize(ModelType& model);
/// Gauss Newton optimization strategy
void optimizeGaussNewton(ModelType& model);
/// Levenberg Marquardt optimization strategy
void optimizeLevenbergMarquardt(ModelType& model);
/// Specify the robust cost that should be used and the appropriate scale estimator
void setRobustCostFunction(
ScaleEstimatorType scale_estimator,
WeightFunctionType weight_function);
/// Add prior to optimization.
void setPrior(
const ModelType& prior,
const Matrix<double, D, D>& Information);
/// Reset all parameters to restart the optimization
void reset();
/// Get the squared error
const double& getChi2() const;
/// The Information matrix is equal to the inverse covariance matrix.
const Matrix<double, D, D>& getInformationMatrix() const;
};
} // end namespace vk
#include "gpu_svo/nlls_solver_impl.hpp"
#endif /* LM_SOLVER_H_ */
GPU_version/vio/include/gpu_svo/nlls_solver_impl.hpp
-348
View File
@@ -1,348 +0,0 @@
/*
* Abstract Nonlinear Least-Squares Solver Class
*
* nlls_solver.h
*
* Created on: Nov 5, 2012
* Author: cforster
*/
#ifndef LM_SOLVER_IMPL_HPP_
#define LM_SOLVER_IMPL_HPP_
#include <stdexcept>
template <int D, typename T>
void vk::NLLSSolver<D, T>::optimize(ModelType& model)
{
if(method_ == GaussNewton)
optimizeGaussNewton(model);
else if(method_ == LevenbergMarquardt)
optimizeLevenbergMarquardt(model);
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::optimizeGaussNewton(ModelType& model)
{
// Compute weight scale
if(use_weights_)
computeResiduals(model, false, true);
// Save the old model to rollback in case of unsuccessful update
ModelType old_model(model);
// perform iterative estimation
for (iter_ = 0; iter_<n_iter_; ++iter_)
{
rho_ = 0;
startIteration();
H_.setZero();
Jres_.setZero();
// compute initial error
n_meas_ = 0;
double new_chi2 = computeResiduals(model, true, false);
// add prior
if(have_prior_)
applyPrior(model);
// solve the linear system
if(!solve())
{
// matrix was singular and could not be computed
std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
std::cout << "H = " << H_ << std::endl;
std::cout << "Jres = " << Jres_ << std::endl;
stop_ = true;
}
// check if error increased since last optimization
if((iter_ > 0 && new_chi2 > chi2_) || stop_)
{
if(verbose_)
{
std::cout << "It. " << iter_
<< "\t Failure"
<< "\t new_chi2 = " << new_chi2
<< "\t Error increased. Stop optimizing.\n";
// << "H = " << H_ << std::endl
//<< "Jres = " << Jres_ << std::endl;
}
model = old_model; // rollback
break;
}
// update the model
ModelType new_model;
update(model, new_model);
old_model = model;
model = new_model;
chi2_ = new_chi2;
if(verbose_)
{
std::cout << "It. " << iter_
<< "\t Success"
<< "\t new_chi2 = " << new_chi2
<< "\t n_meas = " << n_meas_
<< "\t x_norm = " << vk::norm_max(x_)<<'\n';
//<< "\n H = " << H_ << std::endl
//<< "Jres = " << Jres_ << std::endl;
}
finishIteration();
// stop when converged, i.e. update step too small
if(vk::norm_max(x_)<=eps_)
break;
}
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::optimizeLevenbergMarquardt(ModelType& model)
{
// Compute weight scale
if(use_weights_)
computeResiduals(model, false, true);
// compute the initial error
chi2_ = computeResiduals(model, true, false);
if(verbose_)
cout << "init chi2 = " << chi2_
<< "\t n_meas = " << n_meas_
<< endl;
// TODO: compute initial lambda
// Hartley and Zisserman: "A typical init value of lambda is 10^-3 times the
// average of the diagonal elements of J'J"
// Compute Initial Lambda
if(mu_ < 0)
{
double H_max_diag = 0;
double tau = 1e-4;
for(size_t j=0; j<D; ++j)
H_max_diag = max(H_max_diag, fabs(H_(j,j)));
mu_ = tau*H_max_diag;
}
// perform iterative estimation
for (iter_ = 0; iter_<n_iter_; ++iter_)
{
rho_ = 0;
startIteration();
bool sign=false;
// try to compute and update, if it fails, try with increased mu
n_trials_ = 0;
do
{
// init variables
ModelType new_model;
double new_chi2 = -1;
H_.setZero();
//H_ = mu_ * Matrix<double,D,D>::Identity(D,D);
Jres_.setZero();
// compute initial error
n_meas_ = 0;
computeResiduals(model, true, false);
// add damping term:
H_ += (H_.diagonal()*mu_).asDiagonal();
// add prior
if(have_prior_)
applyPrior(model);
// solve the linear system
if(solve())
{
// update the model
update(model, new_model);
// compute error with new model and compare to old error
n_meas_ = 0;
new_chi2 = computeResiduals(new_model, true, false);
rho_ = chi2_-new_chi2;
}
else
{
// matrix was singular and could not be computed
cout << "Matrix is close to singular!" << endl;
cout << "H = " << H_ << endl;
cout << "Jres = " << Jres_ << endl;
rho_ = -1;
}
if(rho_>0)
{
// update decrased the error -> success
model = new_model;
chi2_ = new_chi2;
stop_ = vk::norm_max(x_)<=eps_ ? true : false;
mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.));
nu_ = 2.;
if(verbose_)
{
cout << "It. " << iter_
<< "\t Trial " << n_trials_
<< "\t Success"
<< "\t n_meas = " << n_meas_
<< "\t new_chi2 = " << new_chi2
<< "\t mu = " << mu_
<< "\t nu = " << nu_
<< "\t x_norm = " << vk::norm_max(x_)
<< endl;
}
}
else
{
if(sign){
if(verbose_)
{
cout << "It. " << iter_
<< "\t Trial " << n_trials_
<< "\t Failure"
<< "\t n_meas = " << n_meas_
<< "\t new_chi2 = " << new_chi2
<< "\t mu = " << mu_
<< "\t nu = " << nu_
<< "\t x_norm = " << vk::norm_max(x_)
<< endl;
}
mu_ /= nu_;
nu_ /= 2.;
++n_trials_;
if (n_trials_ >= n_trials_max_)
stop_ = true;
sign=false;
}else{
if(verbose_)
{
cout << "It. " << iter_
<< "\t Trial " << n_trials_
<< "\t Failure"
<< "\t n_meas = " << n_meas_
<< "\t new_chi2 = " << new_chi2
<< "\t mu = " << mu_
<< "\t nu = " << nu_
<< "\t x_norm = " << vk::norm_max(x_)
<< endl;
}
mu_ *= nu_;
nu_ *= 2.;
++n_trials_;
if (n_trials_ >= n_trials_max_)
stop_ = true;
sign=true;
}
}
finishTrial();
} while(!(rho_>0 || stop_));
if (stop_)
break;
finishIteration();
}
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::setRobustCostFunction(
ScaleEstimatorType scale_estimator,
WeightFunctionType weight_function)
{
switch(scale_estimator)
{
case TDistScale:
if(verbose_)
printf("Using TDistribution Scale Estimator\n");
scale_estimator_.reset(new robust_cost::TDistributionScaleEstimator());
use_weights_=true;
break;
case MADScale:
if(verbose_)
printf("Using MAD Scale Estimator\n");
scale_estimator_.reset(new robust_cost::MADScaleEstimator());
use_weights_=true;
break;
case NormalScale:
if(verbose_)
printf("Using Normal Scale Estimator\n");
scale_estimator_.reset(new robust_cost::NormalDistributionScaleEstimator());
use_weights_=true;
break;
default:
if(verbose_)
printf("Using Unit Scale Estimator\n");
scale_estimator_.reset(new robust_cost::UnitScaleEstimator());
use_weights_=false;
}
switch(weight_function)
{
case TDistWeight:
if(verbose_)
printf("Using TDistribution Weight Function\n");
weight_function_.reset(new robust_cost::TDistributionWeightFunction());
break;
case TukeyWeight:
if(verbose_)
printf("Using Tukey Weight Function\n");
weight_function_.reset(new robust_cost::TukeyWeightFunction());
break;
case HuberWeight:
if(verbose_)
printf("Using Huber Weight Function\n");
weight_function_.reset(new robust_cost::HuberWeightFunction());
break;
default:
if(verbose_)
printf("Using Unit Weight Function\n");
weight_function_.reset(new robust_cost::UnitWeightFunction());
}
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::setPrior(
const T& prior,
const Matrix<double, D, D>& Information)
{
have_prior_ = true;
prior_ = prior;
I_prior_ = Information;
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::reset()
{
have_prior_ = false;
chi2_ = 1e10;
mu_ = mu_init_;
nu_ = nu_init_;
n_meas_ = 0;
n_iter_ = n_iter_init_;
iter_ = 0;
stop_ = false;
}
template <int D, typename T>
inline const double& vk::NLLSSolver<D, T>::getChi2() const
{
return chi2_;
}
template <int D, typename T>
inline const vk::Matrix<double, D, D>& vk::NLLSSolver<D, T>::getInformationMatrix() const
{
return H_;
}
#endif /* LM_SOLVER_IMPL_HPP_ */
GPU_version/vio/include/gpu_svo/output_helper.h
-90
View File
@@ -1,90 +0,0 @@
/*
* output_helper.h
*
* Created on: Jan 20, 2013
* Author: cforster
*/
#ifndef VIKIT_OUTPUT_HELPER_H_
#define VIKIT_OUTPUT_HELPER_H_
#include <string>
#include <ros/ros.h>
#include <Eigen/Core>
#include <sophus/se3.h>
#include <tf/transform_broadcaster.h>
using namespace std;
using namespace Eigen;
void
publishTfTransform (const Sophus::SE3& T, const ros::Time& stamp,
const string& frame_id, const string& child_frame_id,
tf::TransformBroadcaster& br);
void
publishPointMarker (ros::Publisher pub,
const Vector3d& pos,
const string& ns,
const ros::Time& timestamp,
int id,
int action,
double marker_scale,
const Vector3d& color,
ros::Duration lifetime = ros::Duration(0.0));
void
publishLineMarker (ros::Publisher pub,
const Vector3d& start,
const Vector3d& end,
const string& ns,
const ros::Time& timestamp,
int id,
int action,
double marker_scale,
const Vector3d& color,
ros::Duration lifetime = ros::Duration(0.0));
void
publishArrowMarker (ros::Publisher pub,
const Vector3d& pos,
const Vector3d& dir,
double scale,
const string& ns,
const ros::Time& timestamp,
int id,
int action,
double marker_scale,
const Vector3d& color);
void
publishHexacopterMarker (ros::Publisher pub,
const string& frame_id,
const string& ns,
const ros::Time& timestamp,
int id,
int action,
double marker_scale,
const Vector3d& color);
void
publishCameraMarker(ros::Publisher pub,
const string& frame_id,
const string& ns,
const ros::Time& timestamp,
int id,
double marker_scale,
const Vector3d& color);
void
publishFrameMarker (ros::Publisher pub,
const Matrix3d& rot,
const Vector3d& pos,
const string& ns,
const ros::Time& timestamp,
int id,
int action,
double marker_scale,
ros::Duration lifetime = ros::Duration(0.0));
#endif /* VIKIT_OUTPUT_HELPER_H_ */
GPU_version/vio/include/gpu_svo/params_helper.h
-57
View File
@@ -1,57 +0,0 @@
//
// Created by root on 5/24/21.
//
#ifndef GPU_SVO_PARAMS_HELPER_H
#define GPU_SVO_PARAMS_HELPER_H
/*
* ros_params_helper.h
*
* Created on: Feb 22, 2013
* Author: cforster
*
* from libpointmatcher_ros
*/
#include <string>
#include <ros/ros.h>
namespace vk {
inline
bool hasParam(const std::string& name)
{
return ros::param::has(name);
}
template<typename T>
T getParam(const std::string& name, const T& defaultValue)
{
T v;
if(ros::param::get(name, v))
{
ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v);
return v;
}
else
ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue);
return defaultValue;
}
template<typename T>
T getParam(const std::string& name)
{
T v;
if(ros::param::get(name, v))
{
ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v);
return v;
}
else
ROS_ERROR_STREAM("Cannot find value for parameter: " << name);
return T();
}
} // namespace vk
#endif //GPU_SVO_PARAMS_HELPER_H
GPU_version/vio/include/gpu_svo/patch_score.h
-225
View File
@@ -1,225 +0,0 @@
/*
* patch_score.h
*
* Created on: Dec 5, 2013
* Author: cforster
*/
#ifndef VIKIT_PATCH_SCORE_H_
#define VIKIT_PATCH_SCORE_H_
#include <stdint.h>
#if __SSE2__
#include <tmmintrin.h>
#endif
namespace vk {
namespace patch_score {
#if __SSE2__
// Horizontal sum of uint16s stored in an XMM register
inline int SumXMM_16(__m128i &target)
{
unsigned short int sums_store[8];
_mm_storeu_si128((__m128i*)sums_store, target);
return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3] +
sums_store[4] + sums_store[5] + sums_store[6] + sums_store[7];
}
// Horizontal sum of uint32s stored in an XMM register
inline int SumXMM_32(__m128i &target)
{
unsigned int sums_store[4];
_mm_storeu_si128((__m128i*)sums_store, target);
return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3];
}
#endif
/// Zero Mean Sum of Squared Differences Cost
template<int HALF_PATCH_SIZE>
class ZMSSD {
public:
static const int patch_size_ = 2*HALF_PATCH_SIZE;
static const int patch_area_ = patch_size_*patch_size_;
static const int threshold_ = 2000*patch_area_;
uint8_t* ref_patch_;
int sumA_, sumAA_;
ZMSSD(uint8_t* ref_patch) :
ref_patch_(ref_patch)
{
uint32_t sumA_uint=0, sumAA_uint=0;
for(int r = 0; r < patch_area_; r++)
{
uint8_t n = ref_patch_[r];
sumA_uint += n;
sumAA_uint += n*n;
}
sumA_ = sumA_uint;
sumAA_ = sumAA_uint;
}
static int threshold() { return threshold_; }
int computeScore(uint8_t* cur_patch) const
{
uint32_t sumB_uint = 0;
uint32_t sumBB_uint = 0;
uint32_t sumAB_uint = 0;
for(int r = 0; r < patch_area_; r++)
{
const uint8_t cur_pixel = cur_patch[r];
sumB_uint += cur_pixel;
sumBB_uint += cur_pixel*cur_pixel;
sumAB_uint += cur_pixel * ref_patch_[r];
}
const int sumB = sumB_uint;
const int sumBB = sumBB_uint;
const int sumAB = sumAB_uint;
return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_;
}
int computeScore(uint8_t* cur_patch, int stride) const
{
int sumB, sumBB, sumAB;
#if __SSE2__
if(patch_size_ == 8)
{
// From PTAM-GPL, Copyright 2008 Isis Innovation Limited
__m128i xImageAsEightBytes;
__m128i xImageAsWords;
__m128i xTemplateAsEightBytes;
__m128i xTemplateAsWords;
__m128i xZero;
__m128i xImageSums; // These sums are 8xuint16
__m128i xImageSqSums; // These sums are 4xint32
__m128i xCrossSums; // These sums are 4xint32
__m128i xProduct;
xImageSums = _mm_setzero_si128();
xImageSqSums = _mm_setzero_si128();
xCrossSums = _mm_setzero_si128();
xZero = _mm_setzero_si128();
uint8_t* imagepointer = cur_patch;
uint8_t* templatepointer = ref_patch_;
long unsigned int cur_stride = stride;
xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
imagepointer += cur_stride;
xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer);
templatepointer += 16;
xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero);
xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
imagepointer += cur_stride;
xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero);
xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
imagepointer += cur_stride;
xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer);
templatepointer += 16;
xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero);
xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
imagepointer += cur_stride;
xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero);
xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
imagepointer += cur_stride;
xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer);
templatepointer += 16;
xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero);
xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
imagepointer += cur_stride;
xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero);
xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
imagepointer += cur_stride;
xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer);
templatepointer += 16;
xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero);
xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero);
xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
sumB = SumXMM_16(xImageSums);
sumAB = SumXMM_32(xCrossSums);
sumBB = SumXMM_32(xImageSqSums);
}
else
#endif
{
uint32_t sumB_uint = 0;
uint32_t sumBB_uint = 0;
uint32_t sumAB_uint = 0;
for(int y=0, r=0; y < patch_size_; ++y)
{
uint8_t* cur_patch_ptr = cur_patch + y*stride;
for(int x=0; x < patch_size_; ++x, ++r)
{
const uint8_t cur_px = cur_patch_ptr[x];
sumB_uint += cur_px;
sumBB_uint += cur_px * cur_px;
sumAB_uint += cur_px * ref_patch_[r];
}
}
sumB = sumB_uint;
sumBB = sumBB_uint;
sumAB = sumAB_uint;
}
return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_;
}
};
} // namespace patch_score
} // namespace vk
#endif // VIKIT_PATCH_SCORE_H_
GPU_version/vio/include/gpu_svo/point.h
-108
View File
@@ -1,108 +0,0 @@
// 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 Vector2d& 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/vio/include/gpu_svo/pose_optimizer.h
-50
View File
@@ -1,50 +0,0 @@
// 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/vio/include/gpu_svo/reprojector.h
-101
View File
@@ -1,101 +0,0 @@
// 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/vio/include/gpu_svo/robust_cost.h
-156
View File
@@ -1,156 +0,0 @@
/**
* This file is part of dvo.
*
* Copyright 2012 Christian Kerl <[email protected]> (Technical University of Munich)
* For more information see <http://vision.in.tum.de/data/software/dvo>.
*
* dvo is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* dvo is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with dvo. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef VIKIT_ROBUST_COST_H_
#define VIKIT_ROBUST_COST_H_
#include <vector>
#include <stdlib.h>
#include <memory>
namespace vk {
namespace robust_cost {
// interface for scale estimators
class ScaleEstimator
{
public:
virtual ~ScaleEstimator() {};
virtual float compute(std::vector<float>& errors) const = 0;
};
typedef std::shared_ptr<ScaleEstimator> ScaleEstimatorPtr;
class UnitScaleEstimator : public ScaleEstimator
{
public:
UnitScaleEstimator() {}
virtual ~UnitScaleEstimator() {}
virtual float compute(std::vector<float>& errors) const { return 1.0f; };
};
// estimates scale by fitting a t-distribution to the data with the given degrees of freedom
class TDistributionScaleEstimator : public ScaleEstimator
{
public:
TDistributionScaleEstimator(const float dof = DEFAULT_DOF);
virtual ~TDistributionScaleEstimator() {};
virtual float compute(std::vector<float>& errors) const;
static const float DEFAULT_DOF;
static const float INITIAL_SIGMA;
protected:
float dof_;
float initial_sigma_;
};
// estimates scale by computing the median absolute deviation
class MADScaleEstimator : public ScaleEstimator
{
public:
MADScaleEstimator() {};
virtual ~MADScaleEstimator() {};
virtual float compute(std::vector<float>& errors) const;
private:
static const float NORMALIZER;;
};
// estimates scale by computing the standard deviation
class NormalDistributionScaleEstimator : public ScaleEstimator
{
public:
NormalDistributionScaleEstimator() {};
virtual ~NormalDistributionScaleEstimator() {};
virtual float compute(std::vector<float>& errors) const;
private:
};
/**
* Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)).
* The errors are assumed to be normalized to unit variance.
*
* See:
* "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54)
*/
class WeightFunction
{
public:
virtual ~WeightFunction() {};
virtual float value(const float& x) const = 0;
virtual void configure(const float& param) {};
};
typedef std::shared_ptr<WeightFunction> WeightFunctionPtr;
class UnitWeightFunction : public WeightFunction
{
public:
UnitWeightFunction() {};
virtual ~UnitWeightFunction() {};
virtual float value(const float& x) const { return 1.0f; };
};
/**
* Tukey's hard re-descending function.
*
* See:
* http://en.wikipedia.org/wiki/Redescending_M-estimator
*/
class TukeyWeightFunction : public WeightFunction
{
public:
TukeyWeightFunction(const float b = DEFAULT_B);
virtual ~TukeyWeightFunction() {};
virtual float value(const float& x) const;
virtual void configure(const float& param);
static const float DEFAULT_B;
private:
float b_square;
};
class TDistributionWeightFunction : public WeightFunction
{
public:
TDistributionWeightFunction(const float dof = DEFAULT_DOF);
virtual ~TDistributionWeightFunction() {};
virtual float value(const float& x) const;
virtual void configure(const float& param);
static const float DEFAULT_DOF;
private:
float dof_;
float normalizer_;
};
class HuberWeightFunction : public WeightFunction
{
public:
HuberWeightFunction(const float k = DEFAULT_K);
virtual ~HuberWeightFunction() {};
virtual float value(const float& x) const;
virtual void configure(const float& param);
static const float DEFAULT_K;
private:
float k;
};
} // namespace robust_cost
} // namespace vk
#endif // VIKIT_ROBUST_COST_H_
GPU_version/vio/include/gpu_svo/sparse_img_align.h
-83
View File
@@ -1,83 +0,0 @@
// 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 <gpu_svo/nlls_solver.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<3, SE2>
{
static const int patch_halfsize_ = 12;
static const int patch_size_ = 12*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, 3, 3> 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, 3, 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 SE2& 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/vio/include/gpu_svo/timer.h
-81
View File
@@ -1,81 +0,0 @@
#ifndef TIMER_H
#define TIMER_H
#include <sys/time.h>
#include <stdio.h>
#include <unistd.h>
namespace vk
{
class Timer
{
private:
timeval start_time_;
double time_;
double accumulated_;
public:
/// The constructor directly starts the timer.
Timer() :
time_(0.0),
accumulated_(0.0)
{
start();
}
~Timer()
{}
inline void start()
{
accumulated_ = 0.0;
gettimeofday(&start_time_, NULL);
}
inline void resume()
{
gettimeofday(&start_time_, NULL);
}
inline double stop()
{
timeval end_time;
gettimeofday(&end_time, NULL);
long seconds = end_time.tv_sec - start_time_.tv_sec;
long useconds = end_time.tv_usec - start_time_.tv_usec;
time_ = ((seconds) + useconds*0.000001) + accumulated_;
accumulated_ = time_;
return time_;
}
inline double getTime() const
{
return time_;
}
inline void reset()
{
time_ = 0.0;
accumulated_ = 0.0;
}
static double getCurrentTime()
{
timeval time_now;
gettimeofday(&time_now, NULL);
return time_now.tv_sec + time_now.tv_usec*0.000001;
}
static double getCurrentSecond()
{
timeval time_now;
gettimeofday(&time_now, NULL);
return time_now.tv_sec;
}
};
} // end namespace vk
#endif
GPU_version/vio/include/gpu_svo/ukf.h
-155
View File
@@ -1,155 +0,0 @@
//
// Created by root on 6/8/21.
//
#ifndef GPU_SVO_UKF_H
#define GPU_SVO_UKF_H
#include <iostream>
#include <fstream>
#include "gpu_svo/config.h"
#include <Eigen/Dense>
class Base{
public:
Base(const Eigen::Matrix<double, 3, 1> &init) {
state_=init;
cov_.setIdentity();
cov_*=0.0001;
};
virtual ~Base(){
};
void predict(double dx,double dz,double dpitch,double time){
if(t_==0.0){
t_=time;
return;
}
double dt=time-t_;
t_=time;
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2));
state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2));
Eigen::Matrix<double,3,3> R;
R<<svo::Config::Cmd_Cov(),0,0,
0,svo::Config::Cmd_Cov(),0,
0,0,svo::Config::Cmd_Cov();
Eigen::Matrix<double,3,3> G;
G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)),
0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)),
0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
predict_up=true;
}
void correct(double ddx, double ddz, double dpitch, double time){
Eigen::Matrix<double,3,3> H;
Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k;
double dt=time-t_;
t_=time;
H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
0,0,1.0;
Q<<svo::Config::ACC_Noise(),0,0,
0,svo::Config::ACC_Noise(),0,
0,0,svo::Config::GYO_Noise();
E(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
E(2)=state_(2)+dt*dpitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E;
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)));
state_h_(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)));
Eigen::Matrix<double,3,3> R;
R<<svo::Config::ACC_Noise(),0,0,
0,svo::Config::ACC_Noise(),0,
0,0,svo::Config::GYO_Noise();
Eigen::Matrix<double,3,3> G;
G<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
yaw_t_1_=state_(2);
}
void correct(double x, double z,double pitch,size_t match,double time){
Eigen::Matrix<double,3,3> H;
Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k;
t_=time;
++match;
H<<1.0,0,0,
0,1.0,0,
0,0,1.0;
Q<<svo::Config::Svo_Ekf(),0,0,
0,svo::Config::Svo_Ekf(),0,
0,0,svo::Config::Svo_Ekf();
E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+(k*E);
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
yaw_t_1_=state_(2);
predict_up=false;
}
Eigen::Matrix<double,3,3> cov_;
Eigen::Matrix<double, 3, 1> state_;
bool predict_up=false;
private:
double t_=0.0;
Eigen::Matrix<double,3,3> cov_h_;
Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
double yaw_t_1_=0.0;
};
class UKF {
public:
UKF(const Eigen::Matrix<double, 3, 1>& init) : filter_(new Base(init)) {
};
virtual ~UKF() {
delete filter_;
};
void UpdateIMU(double x,double z,double pitch,const ros::Time& time) {
while(lock)usleep(5);
lock=true;
if(abs(x)<1.0)x=pow(x,3);//picked up from your code
if(abs(z)<1.0)z=pow(z,3);//picked up from your code
if(filter_->predict_up)filter_->correct(x,z,pitch,1e-9*time.toNSec());
lock=false;
};
void UpdateCmd(double x,double z,double pitch,const ros::Time& time) {
while(lock)usleep(5);
lock=true;
filter_->predict(x,z,pitch,1e-9*time.toNSec());
lock=false;
};
std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
while(lock)usleep(5);
lock=true;
if(pitch<0.0)
pitch=M_PI-pitch;
else
pitch=pitch-M_PI;
if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec());
lock=false;
double pitch_f;
if(filter_->state_(0)<0.0)
pitch_f=M_PI-filter_->state_(2);
else
pitch_f=filter_->state_(2)-M_PI;
return std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
};
std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5> get_location(){
double pitch_f;
if(filter_->state_(0)<0.0)
pitch_f=M_PI-filter_->state_(2);
else
pitch_f=filter_->state_(2)-M_PI;
return std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
}
private:
Base* filter_= nullptr;
bool lock=false;
};
#endif //GPU_SVO_UKF_H
GPU_version/vio/include/gpu_svo/vision.h
-80
View File
@@ -1,80 +0,0 @@
/*
* vision.h
*
* Created on: May 14, 2013
* Author: cforster
*/
#ifndef VIKIT_VISION_H_
#define VIKIT_VISION_H_
#include <gpu_svo/aligned_mem.h>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
namespace vk
{
//! Return value between 0 and 1
//! WARNING This function does not check whether the x/y is within the border
inline float
interpolateMat_32f(const cv::Mat& mat, float u, float v)
{
assert(mat.type()==CV_32F);
float x = floor(u);
float y = floor(v);
float subpix_x = u-x;
float subpix_y = v-y;
float wx0 = 1.0-subpix_x;
float wx1 = subpix_x;
float wy0 = 1.0-subpix_y;
float wy1 = subpix_y;
float val00 = mat.at<float>(y,x);
float val10 = mat.at<float>(y,x+1);
float val01 = mat.at<float>(y+1,x);
float val11 = mat.at<float>(y+1,x+1);
return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11;
}
//! Return value between 0 and 255
//! WARNING This function does not check whether the x/y is within the border
inline float
interpolateMat_8u(const cv::Mat& mat, float u, float v)
{
if(mat.empty())return 0.0;
if(mat.type()!=CV_8U)return 0.0;
int x = floor(u);
int y = floor(v);
float subpix_x = u-x;
float subpix_y = v-y;
float w00 = (1.0f-subpix_x)*(1.0f-subpix_y);
float w01 = (1.0f-subpix_x)*subpix_y;
float w10 = subpix_x*(1.0f-subpix_y);
float w11 = 1.0f - w00 - w01 - w10;
const int stride = mat.step.p[0];
unsigned char* ptr = mat.data + y*stride + x;
if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0;
if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0;
return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1];
}
void halfSample(const cv::Mat& in, cv::Mat& out);
float shiTomasiScore(const cv::Mat& img, int u, int v);
void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst);
#ifdef __SSE2__
/// Used to convert a Kinect depthmap
/// Code by Christian Kerl DVO, GPL Licence
void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale);
#endif
} // namespace vk
#endif // VIKIT_VISION_H_
GPU_version/vio/include/gpu_svo/visualizer.h
-62
View File
@@ -1,62 +0,0 @@
// 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>
#include <gpu_svo/ukf.h>
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_with_cov_;
Visualizer();
~Visualizer() {};
void publishMinimal(
UKF& ukf,
const double timestamp);
};
} // end namespace svo
#endif /* SVO_VISUALIZER_H_ */
GPU_version/vio/src/frame_handler_mono.cpp
+8 -6
View File
@@ -52,18 +52,20 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
gpu_fast_->write_buf(1,3,3,pose); gpu_fast_->write_buf(1,3,3,pose);
cl_double3 f[300]={0}; cl_double3 f[300]={0};
gpu_fast_->write_buf(1,4,300,f); gpu_fast_->write_buf(1,4,300,f);
cl_double2 px[300]={0};
gpu_fast_->write_buf(1,5,300,px);
int level=0; int level=0;
gpu_fast_->write_buf(1,5,level); gpu_fast_->write_buf(1,6,level);
double c[5]={0}; double c[5]={0};
gpu_fast_->write_buf(1,6,5,c); gpu_fast_->write_buf(1,7,5,c);
float e[300]={0}; float e[300]={0};
gpu_fast_->write_buf(1,7,300,e); gpu_fast_->write_buf(1,8,300,e);
float H[6]={0}; float H[6]={0};
gpu_fast_->write_buf(1,8,6,H); gpu_fast_->write_buf(1,9,6,H);
float J[3]={0}; float J[3]={0};
gpu_fast_->write_buf(1,9,3,J); gpu_fast_->write_buf(1,10,3,J);
double chi2=0.0; double chi2=0.0;
gpu_fast_->write_buf(1,10,chi2); gpu_fast_->write_buf(1,11,chi2);
imageAlign_=new SparseImgAlignGpu(Config::kltMaxLevel(), Config::kltMinLevel(), imageAlign_=new SparseImgAlignGpu(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::GaussNewton, false,gpu_fast_); 30, SparseImgAlign::GaussNewton, false,gpu_fast_);
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
GPU_version/vio/src/sparse_img_align_gpu.cpp
+11 -7
View File
@@ -45,8 +45,9 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
double cur_pos[3]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()}; double cur_pos[3]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()};
residual_->reload_buf(1,2,cur_pos); residual_->reload_buf(1,2,cur_pos);
residual_->reload_buf(1,3,ref_pos); residual_->reload_buf(1,3,ref_pos);
residual_->reload_buf(1,6,camera); residual_->reload_buf(1,7,camera);
cl_double3* features; cl_double3* features;
cl_double2* featue_px;
feature_counter_ = 0; // is used to compute the index of the cached jacobian feature_counter_ = 0; // is used to compute the index of the cached jacobian
std::vector<bool>::iterator visiblity_it = visible_fts_.begin(); std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end(); for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();
@@ -57,9 +58,12 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
features[feature_counter_].x=xyz_ref(0); features[feature_counter_].x=xyz_ref(0);
features[feature_counter_].y=xyz_ref(1); features[feature_counter_].y=xyz_ref(1);
features[feature_counter_].z=xyz_ref(2); features[feature_counter_].z=xyz_ref(2);
featue_px[feature_counter_].x=(*it)->px(0);
featue_px[feature_counter_].y=(*it)->px(1);
++feature_counter_; ++feature_counter_;
} }
residual_->reload_buf(1,4,features); residual_->reload_buf(1,4,features);
residual_->reload_buf(1,5,featue_px);
SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it
for(level_=max_level_; level_>=min_level_; --level_) for(level_=max_level_; level_>=min_level_; --level_)
{ {
@@ -67,7 +71,7 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
cv::Mat& ref_img = ref_frame->img_pyr_.at(level_); cv::Mat& ref_img = ref_frame->img_pyr_.at(level_);
residual_->reload_buf(1,0,cur_img); residual_->reload_buf(1,0,cur_img);
residual_->reload_buf(1,1,ref_img); residual_->reload_buf(1,1,ref_img);
residual_->reload_buf(1,5,&level_); residual_->reload_buf(1,6,&level_);
mu_ = 1.0; mu_ = 1.0;
optimize(T_cur); optimize(T_cur);
} }
@@ -81,16 +85,16 @@ double SparseImgAlignGpu::computeResiduals(
bool compute_weight_scale) bool compute_weight_scale)
{ {
double chi2=0.0; double chi2=0.0;
residual_->reload_buf(1,10,&chi2); residual_->reload_buf(1,11,&chi2);
/// TODO GPU compute based on the feature /// TODO GPU compute based on the feature
residual_->run(1,feature_counter_); residual_->run(1,feature_counter_);
if(compute_weight_scale && iter_ == 0) { if(compute_weight_scale && iter_ == 0) {
float error[feature_counter_]; float error[feature_counter_];
residual_->read(1,7,error); residual_->read(1,8,error);
std::vector<float> errors(error,error+feature_counter_); std::vector<float> errors(error,error+feature_counter_);
scale_ = scale_estimator_->compute(errors); scale_ = scale_estimator_->compute(errors);
} }
residual_->read(1,10,&chi2); residual_->read(1,11,&chi2);
return chi2; return chi2;
} }
@@ -98,8 +102,8 @@ int SparseImgAlignGpu::solve()
{ {
double* H; double* H;
double* J; double* J;
residual_->read(1,8,H); residual_->read(1,9,H);
residual_->read(1,9,J); residual_->read(1,10,J);
x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Matrix<double,3,1>(J)); x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Matrix<double,3,1>(J));
if((bool) std::isnan((double) x_[0])) if((bool) std::isnan((double) x_[0]))
return 0; return 0;