Merge remote-tracking branch 'origin/master'
This commit is contained in:
VahdPourAmin
@@ -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_ */
|
||||
@@ -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_
|
||||
@@ -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_ */
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_ */
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_ */
|
||||
@@ -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_ */
|
||||
@@ -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_ */
|
||||
@@ -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_ */
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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_
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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_
|
||||
@@ -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_ */
|
||||
@@ -52,18 +52,20 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
|
||||
gpu_fast_->write_buf(1,3,3,pose);
|
||||
cl_double3 f[300]={0};
|
||||
gpu_fast_->write_buf(1,4,300,f);
|
||||
cl_double2 px[300]={0};
|
||||
gpu_fast_->write_buf(1,5,300,px);
|
||||
int level=0;
|
||||
gpu_fast_->write_buf(1,5,level);
|
||||
gpu_fast_->write_buf(1,6,level);
|
||||
double c[5]={0};
|
||||
gpu_fast_->write_buf(1,6,5,c);
|
||||
gpu_fast_->write_buf(1,7,5,c);
|
||||
float e[300]={0};
|
||||
gpu_fast_->write_buf(1,7,300,e);
|
||||
gpu_fast_->write_buf(1,8,300,e);
|
||||
float H[6]={0};
|
||||
gpu_fast_->write_buf(1,8,6,H);
|
||||
gpu_fast_->write_buf(1,9,6,H);
|
||||
float J[3]={0};
|
||||
gpu_fast_->write_buf(1,9,3,J);
|
||||
gpu_fast_->write_buf(1,10,3,J);
|
||||
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(),
|
||||
30, SparseImgAlign::GaussNewton, false,gpu_fast_);
|
||||
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
|
||||
|
||||
@@ -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()};
|
||||
residual_->reload_buf(1,2,cur_pos);
|
||||
residual_->reload_buf(1,3,ref_pos);
|
||||
residual_->reload_buf(1,6,camera);
|
||||
residual_->reload_buf(1,7,camera);
|
||||
cl_double3* features;
|
||||
cl_double2* featue_px;
|
||||
feature_counter_ = 0; // is used to compute the index of the cached jacobian
|
||||
std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
|
||||
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_].y=xyz_ref(1);
|
||||
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_;
|
||||
}
|
||||
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
|
||||
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_);
|
||||
residual_->reload_buf(1,0,cur_img);
|
||||
residual_->reload_buf(1,1,ref_img);
|
||||
residual_->reload_buf(1,5,&level_);
|
||||
residual_->reload_buf(1,6,&level_);
|
||||
mu_ = 1.0;
|
||||
optimize(T_cur);
|
||||
}
|
||||
@@ -81,16 +85,16 @@ double SparseImgAlignGpu::computeResiduals(
|
||||
bool compute_weight_scale)
|
||||
{
|
||||
double chi2=0.0;
|
||||
residual_->reload_buf(1,10,&chi2);
|
||||
residual_->reload_buf(1,11,&chi2);
|
||||
/// TODO GPU compute based on the feature
|
||||
residual_->run(1,feature_counter_);
|
||||
if(compute_weight_scale && iter_ == 0) {
|
||||
float error[feature_counter_];
|
||||
residual_->read(1,7,error);
|
||||
residual_->read(1,8,error);
|
||||
std::vector<float> errors(error,error+feature_counter_);
|
||||
scale_ = scale_estimator_->compute(errors);
|
||||
}
|
||||
residual_->read(1,10,&chi2);
|
||||
residual_->read(1,11,&chi2);
|
||||
return chi2;
|
||||
}
|
||||
|
||||
@@ -98,8 +102,8 @@ int SparseImgAlignGpu::solve()
|
||||
{
|
||||
double* H;
|
||||
double* J;
|
||||
residual_->read(1,8,H);
|
||||
residual_->read(1,9,J);
|
||||
residual_->read(1,9,H);
|
||||
residual_->read(1,10,J);
|
||||
x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Matrix<double,3,1>(J));
|
||||
if((bool) std::isnan((double) x_[0]))
|
||||
return 0;
|
||||
|
||||
Reference in New Issue
Block a user