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

Merge pull request #1 from Gfuse/client/master

Browse Source 
Client/master
This commit is contained in:
Majid Geravand
2021-08-12 09:05:05 +02:00
committed by GitHub
parent 7a355486b3 d0bac30af9
commit 281422e338
77 changed files with 876 additions and 403 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+50
View File
@@ -0,0 +1,50 @@
# These are some examples of commonly ignored file patterns.
# You should customize this list as applicable to your project.
# Learn more about .gitignore:
# https://www.atlassian.com/git/tutorials/saving-changes/gitignore
# Node artifact files
node_modules/
dist/
# Compiled Java class files
*.class
# Compiled Python bytecode
*.py[cod]
# Log files
*.log
# Package files
*.jar
# Maven
target/
dist/
# JetBrains IDE
.idea/
# Unit test reports
TEST*.xml
# Generated by MacOS
.DS_Store
# Generated by Windows
Thumbs.db
# Applications
*.app
*.exe
*.war
# Large media files
*.mp4
*.tiff
*.avi
*.flv
*.mov
*.wmv
.idea/misc.xml
Generated
+1 -1
View File
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$/GPU_version/svo">
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$/GPU_version/vio">
<contentRoot DIR="$PROJECT_DIR$" />
</component>
</project>
GPU_version/vio/gtsam/imu_integration.cpp
+6 -6
View File
@@ -5,14 +5,14 @@
#include "imu_integration.h"
#include <memory>
#include <utility>
#include "gpu_svo/feature.h"
#include "gpu_svo/point.h"
#include "gpu_svo/svoVisionFactor.h"
#include "vio/feature.h"
#include "vio/point.h"
#include "vio/svoVisionFactor.h"
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose3.h>
#include <gpu_svo/params_helper.h>
#include <gpu_svo/config.h>
#include <gpu_svo/for_it.hpp>
#include <vio/params_helper.h>
#include <vio/config.h>
#include <vio/for_it.hpp>
#define debug 0
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
GPU_version/vio/gtsam/imu_integration.h
+1 -1
View File
@@ -13,7 +13,7 @@
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <sophus/se3.h>
#include <gpu_svo/frame.h>
#include <vio/frame.h>
using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot)
GPU_version/vio/gtsam/svoVisionFactor.h
+4 -4
View File
@@ -10,11 +10,11 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose3.h>
#include <gpu_svo/robust_cost.h>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/frame.h>
#include <vio/robust_cost.h>
#include <vio/math_utils.h>
#include <vio/frame.h>
namespace svo {
namespace vio {
class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
GPU_version/vio/gtsam/svoVisionFactorLandMark.h
+2 -2
View File
@@ -12,9 +12,9 @@
#include <gtsam/geometry/Pose3.h>
#include <vikit/robust_cost.h>
#include <vikit/math_utils.h>
#include <gpu_svo/frame.h>
#include <vio/frame.h>
namespace svo {
namespace vio {
class VisionFactorLandMark: public gtsam::NoiseModelFactor3<gtsam::Pose3,gtsam::Pose3,gtsam::Point3> {
GPU_version/vio/gtsam/svoVisionFactorSmall.h
+2 -2
View File
@@ -12,9 +12,9 @@
#include <gtsam/geometry/Pose3.h>
#include <vikit/robust_cost.h>
#include <vikit/math_utils.h>
#include <gpu_svo/frame.h>
#include <vio/frame.h>
namespace svo {
namespace vio {
class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
GPU_version/vio/include/gpu_svo/abstract_camera.h → GPU_version/vio/include/vio/abstract_camera.h
+1
View File
@@ -33,6 +33,7 @@ public:
/// Project from pixels to world coordiantes. Returns a bearing vector of unit length.
virtual Vector3d
cam2world(const double& x, const double& y) const = 0;
virtual double* params() const =0;
/// Project from pixels to world coordiantes. Returns a bearing vector of unit length.
virtual Vector3d
GPU_version/vio/include/gpu_svo/aligned_mem.h → GPU_version/vio/include/vio/aligned_mem.h
View File
GPU_version/vio/include/gpu_svo/atan_camera.h → GPU_version/vio/include/vio/atan_camera.h
+12 -2
View File
@@ -14,10 +14,11 @@
#define ATAN_CAMERA_H_
#include <stdlib.h>
#include <iostream>
#include <string>
#include <Eigen/Eigen>
#include <gpu_svo/abstract_camera.h>
#include <gpu_svo/math_utils.h>
#include <vio/abstract_camera.h>
#include <vio/math_utils.h>
namespace vk {
@@ -34,6 +35,7 @@ private:
double tans_; //!< distortion model coeff
double tans_inv_; //!< distortion model coeff
bool distortion_; //!< use distortion model?
double* param_;
//! Radial distortion transformation factor: returns ration of distorted / undistorted radius.
inline double rtrans_factor(double r) const
@@ -84,6 +86,14 @@ public:
{
return 4*fx_*fy_;
}
virtual double* params() const{
param_[0]=fx_;
param_[1]=fy_;
param_[2]=cx_;
param_[3]=cy_;
param_[4]=s_;
return param_;
}
};
} // end namespace vk
GPU_version/vio/include/gpu_svo/bundle_adjustment.h → GPU_version/vio/include/vio/bundle_adjustment.h
+3 -3
View File
@@ -17,7 +17,7 @@
#ifndef SVO_BUNDLE_ADJUSTMENT_H_
#define SVO_BUNDLE_ADJUSTMENT_H_
#include <gpu_svo/global.h>
#include <vio/global.h>
namespace g2o {
class EdgeProjectXYZ2UV;
@@ -26,7 +26,7 @@ class VertexSE3Expmap;
class VertexSBAPointXYZ;
}
namespace svo {
namespace vio {
typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
typedef g2o::VertexSE3Expmap g2oFrameSE3;
@@ -104,6 +104,6 @@ g2oEdgeSE3* createG2oEdgeSE3(
double weight = 1);
} // namespace ba
} // namespace svo
} // namespace vio
#endif // SVO_BUNDLE_ADJUSTMENT_H_
GPU_version/vio/include/gpu_svo/camera_loader.h → GPU_version/vio/include/vio/camera_loader.h
+3 -3
View File
@@ -9,9 +9,9 @@
#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>
#include <vio/abstract_camera.h>
#include <vio/atan_camera.h>
#include <vio/params_helper.h>
namespace vk {
namespace camera_loader {
GPU_version/vio/include/gpu_svo/cl.hpp → GPU_version/vio/include/vio/cl.hpp
View File
GPU_version/vio/include/gpu_svo/cl_class.h → GPU_version/vio/include/vio/cl_class.h
+40 -29
View File
@@ -4,7 +4,7 @@
#ifndef VIO_OPENCL_CL_CLASS_H
#define VIO_OPENCL_CL_CLASS_H
#include <gpu_svo/cl.hpp>
#include <vio/cl.hpp>
#include <iostream>
#include <vector>
#include <algorithm>
@@ -12,33 +12,32 @@
#include <exception>
#include <CL/opencl.h>
#include <opencv2/opencv.hpp>
#include <vio/abstract_camera.h>
class kernel{
public:
kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());};
~kernel(){
for(auto&& i:_images)i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
for(auto&& i:_buffers)i.first.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
_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),
_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 | CL_MEM_USE_HOST_PTR
, sizeof(T) * buf_size,buf),
buf_size),id));
cl_int err=queue->enqueueWriteBuffer(_buffers.back().first.first,CL_TRUE, 0, sizeof(T) * buf_size, buf);
cl_int err=_kernel->setArg(id,_buffers.back().first.first);
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';
std::cerr<<"Error C:Kernel, F:write, L3:setArg\t"<<id<<"\t"<<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,
_images.push_back(std::pair<cl::Image2D,size_t>(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR,
cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
buf.size().width,
buf.size().height,
@@ -47,7 +46,7 @@ public:
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';
std::cerr<<"Error C:Kernel, F:write image, L:setArg ID:\t"<<id<<"\t"<<err<<'\n';
return err;
}
return CL_SUCCESS;
@@ -55,27 +54,29 @@ public:
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;
for(auto&& i:_buffers)if(i.second==id){
T* Map_buf=(T*)queue->enqueueMapBuffer(i.first.first,CL_TRUE,CL_MAP_WRITE,0,sizeof(T) * i.first.second);
memcpy(Map_buf,buf,sizeof(T) * i.first.second);
if(queue->enqueueUnmapMemObject(i.first.first,Map_buf)!=CL_SUCCESS)std::cerr<<"Reload buffer on GPU failed"<<'\n';
}
return CL_SUCCESS;
};
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,
for(auto&& i:_images)if(i.second==id){
i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR,
cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
buf.size().width,
buf.size().height,
0,
reinterpret_cast<uchar*>(buf.data));
cl_int err=_kernel->setArg(id,i.first);
if(err!=0){
std::cerr<<"Error C:Kernel, F:write image, L:setArg ID:\t"<<id<<"\t"<<err<<'\n';
return err;
}
}
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
@@ -90,6 +91,9 @@ public:
for(auto i:_images)if(i.second==id)return i.first;
}
private:
static void notify(cl_mem *, void * user_data) {
//std::cerr << "Memory object was deleted." << std::endl;
}
std::vector<std::pair<std::pair<cl::Buffer,size_t>,size_t>> _buffers;
std::vector<std::pair<cl::Image2D,size_t>> _images;
@@ -111,7 +115,7 @@ public:
};
class opencl{
public:
opencl();
opencl(vk::AbstractCamera* cam);
~opencl();
int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));};
template<typename T>
@@ -158,6 +162,7 @@ public:
}
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;
queue->flush();
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){
@@ -165,27 +170,33 @@ public:
}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;
return queue->finish();
}
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);
}
template<typename T>
void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,size_t size/*size*/, T* out){
T* Map_buf=(T*)queue->enqueueMapBuffer(_kernels.at(id1).read(id2).first,CL_TRUE,CL_MAP_READ,0,sizeof(T) * size);
memcpy(out,Map_buf,sizeof(T) * size);
if(queue->enqueueUnmapMemObject(_kernels.at(id1).read(id2).first,Map_buf)!=CL_SUCCESS)std::cerr<<"Read buffer on GPU failed"<<'\n';
}
void clear_buf();
private:
std::vector<kernel> _kernels;
cl::Context* context = NULL;
cl::Device device;
cl::Program* program = NULL;
cl::CommandQueue* queue=NULL;
cl::Context* context = nullptr;
cl::Device* device = nullptr;
cl::Program* program = nullptr;
cl::CommandQueue* queue= nullptr;
vk::AbstractCamera* cam= nullptr;
};
#endif //VIO_OPENCL_CL_CLASS_H
GPU_version/vio/include/gpu_svo/config.h → GPU_version/vio/include/vio/config.h
+2 -2
View File
@@ -21,7 +21,7 @@
#include <stdint.h>
#include <stdio.h>
namespace svo {
namespace vio {
using std::string;
@@ -177,6 +177,6 @@ private:
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
} // namespace vio
#endif // SVO_CONFIG_H_
GPU_version/vio/include/gpu_svo/depth_filter.h → GPU_version/vio/include/vio/depth_filter.h
+5 -5
View File
@@ -21,11 +21,11 @@
#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>
#include <vio/global.h>
#include <vio/feature_detection.h>
#include <vio/matcher.h>
namespace svo {
namespace vio {
class Frame;
class Feature;
@@ -165,6 +165,6 @@ protected:
void updateSeedsLoop();
};
} // namespace svo
} // namespace vio
#endif // SVO_DEPTH_FILTER_H_
GPU_version/vio/include/gpu_svo/feature.h → GPU_version/vio/include/vio/feature.h
+3 -3
View File
@@ -17,9 +17,9 @@
#ifndef SVO_FEATURE_H_
#define SVO_FEATURE_H_
#include <gpu_svo/frame.h>
#include <vio/frame.h>
namespace svo {
namespace vio {
/// A salient image region that is tracked across frames.
struct Feature
@@ -70,6 +70,6 @@ struct Feature
{}
};
} // namespace svo
} // namespace vio
#endif // SVO_FEATURE_H_
GPU_version/vio/include/gpu_svo/feature_alignment.h → GPU_version/vio/include/vio/feature_alignment.h
+3 -3
View File
@@ -17,9 +17,9 @@
#ifndef SVO_FEATURE_ALIGNMENT_H_
#define SVO_FEATURE_ALIGNMENT_H_
#include <gpu_svo/global.h>
#include <vio/global.h>
namespace svo {
namespace vio {
/// Subpixel refinement of a reference feature patch with the current image.
/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on"
@@ -58,6 +58,6 @@ bool align2D_NEON(
Vector2d& cur_px_estimate);
} // namespace feature_alignment
} // namespace svo
} // namespace vio
#endif // SVO_FEATURE_ALIGNMENT_H_
GPU_version/vio/include/gpu_svo/feature_detection.h → GPU_version/vio/include/vio/feature_detection.h
+5 -5
View File
@@ -17,11 +17,11 @@
#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>
#include <vio/global.h>
#include <vio/frame.h>
#include <vio/cl_class.h>
namespace svo {
namespace vio {
/// Implementation of various feature detectors.
namespace feature_detection {
@@ -106,6 +106,6 @@ public:
};
} // namespace feature_detection
} // namespace svo
} // namespace vio
#endif // SVO_FEATURE_DETECTION_H_
GPU_version/vio/include/gpu_svo/for_it.hpp → GPU_version/vio/include/vio/for_it.hpp
+3 -3
View File
@@ -2,8 +2,8 @@
// Created by root on 5/12/21.
//
#ifndef GPU_SVO_FOR_IT_H
#define GPU_SVO_FOR_IT_H
#ifndef VIO_FOR_IT_H
#define VIO_FOR_IT_H
#include <iostream>
#include <vector>
#include <tuple>
@@ -84,4 +84,4 @@ for_impl<T> _for(T & t)
{
return for_impl<T>(t);
}
#endif //GPU_SVO_FOR_IT_H
#endif //VIO_FOR_IT_H
GPU_version/vio/include/gpu_svo/frame.h → GPU_version/vio/include/vio/frame.h
+5 -5
View File
@@ -18,17 +18,17 @@
#define SVO_FRAME_H_
#include <sophus/se3.h>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/abstract_camera.h>
#include <vio/math_utils.h>
#include <vio/abstract_camera.h>
#include <boost/noncopyable.hpp>
#include <gpu_svo/global.h>
#include <vio/global.h>
namespace g2o {
class VertexSE3Expmap;
}
typedef g2o::VertexSE3Expmap g2oFrameSE3;
namespace svo {
namespace vio {
class Point;
struct Feature;
@@ -180,6 +180,6 @@ void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr);
bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min);
} // namespace frame_utils
} // namespace svo
} // namespace vio
#endif // SVO_FRAME_H_
GPU_version/vio/include/gpu_svo/frame_handler_base.h → GPU_version/vio/include/vio/frame_handler_base.h
+3 -3
View File
@@ -21,8 +21,8 @@
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include <boost/thread.hpp>
#include <gpu_svo/global.h>
#include <gpu_svo/map.h>
#include <vio/global.h>
#include <vio/map.h>
namespace vk
{
@@ -30,7 +30,7 @@ class AbstractCamera;
//class PerformanceMonitor;
}
namespace svo
namespace vio
{
class Point;
class Matcher;
GPU_version/vio/include/gpu_svo/frame_handler_mono.h → GPU_version/vio/include/vio/frame_handler_mono.h
+10 -8
View File
@@ -18,14 +18,15 @@
#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>
#include <vio/abstract_camera.h>
#include <vio/frame_handler_base.h>
#include <vio/reprojector.h>
#include <vio/initialization.h>
#include <vio/ukf.h>
#include <vio/cl_class.h>
#include <vio/sparse_img_align_gpu.h>
namespace svo {
namespace vio {
/// Monocular Visual Odometry Pipeline as described in the SVO paper.
class FrameHandlerMono : public FrameHandlerBase
@@ -78,6 +79,7 @@ protected:
opencl* gpu_fast_;
ros::Time time_;
/// Initialize the visual odometry algorithm.
virtual void initialize();
@@ -104,6 +106,6 @@ protected:
void setCoreKfs(size_t n_closest);
};
} // namespace svo
} // namespace vio
#endif // SVO_FRAME_HANDLER_H_
GPU_version/vio/include/gpu_svo/global.h → GPU_version/vio/include/vio/global.h
+3 -3
View File
@@ -43,7 +43,7 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
#define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
namespace svo
namespace vio
{
using namespace Eigen;
using namespace Sophus;
@@ -90,7 +90,7 @@ namespace svo
return T2_;
}
SE2 inverse() const{
double yaw=atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0));
double yaw=atan(T2_.so2().unit_complex().imag()/T2_.so2().unit_complex().real());
if(yaw<0.0)
yaw=M_PI-yaw;
else
@@ -115,6 +115,6 @@ namespace svo
class Frame;
typedef boost::shared_ptr<Frame> FramePtr;
} // namespace svo
} // namespace vio
#endif // SVO_GLOBAL_H_
GPU_version/vio/include/gpu_svo/homography.h → GPU_version/vio/include/vio/homography.h
+1 -1
View File
@@ -19,7 +19,7 @@
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/SVD>
#include <gpu_svo/math_utils.h>
#include <vio/math_utils.h>
namespace vk {
GPU_version/vio/include/gpu_svo/initialization.h → GPU_version/vio/include/vio/initialization.h
+5 -5
View File
@@ -17,11 +17,11 @@
#ifndef SVO_INITIALIZATION_H
#define SVO_INITIALIZATION_H
#include <gpu_svo/global.h>
#include <gpu_svo/cl_class.h>
#include <vio/global.h>
#include <vio/cl_class.h>
//#include "../../gtsam/imu_integration.h"
namespace svo {
namespace vio {
class FrameHandlerMono;
@@ -32,7 +32,7 @@ enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS };
/// Tracks features using Lucas-Kanade tracker and then estimates a homography.
class KltHomographyInit {
friend class svo::FrameHandlerMono;
friend class vio::FrameHandlerMono;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -82,6 +82,6 @@ void computeHomography(
SE2_5& T_cur_from_ref);
} // namespace initialization
} // namespace svo
} // namespace vio
#endif // SVO_INITIALIZATION_H
GPU_version/vio/include/gpu_svo/map.h → GPU_version/vio/include/vio/map.h
+3 -3
View File
@@ -20,9 +20,9 @@
#include <queue>
#include <boost/noncopyable.hpp>
#include <boost/thread.hpp>
#include <gpu_svo/global.h>
#include <vio/global.h>
namespace svo {
namespace vio {
class Point;
class Feature;
@@ -132,6 +132,6 @@ void frameValidation(Frame* frame, int id);
void pointValidation(Point* point, int id);
} // namespace map_debug
} // namespace svo
} // namespace vio
#endif // SVO_MAP_H_
GPU_version/vio/include/gpu_svo/matcher.h → GPU_version/vio/include/vio/matcher.h
+3 -3
View File
@@ -17,7 +17,7 @@
#ifndef SVO_MATCHER_H_
#define SVO_MATCHER_H_
#include <gpu_svo/global.h>
#include <vio/global.h>
namespace vk {
class AbstractCamera;
@@ -26,7 +26,7 @@ namespace vk {
}
}
namespace svo {
namespace vio {
class Point;
class Frame;
@@ -125,6 +125,6 @@ public:
void createPatchFromPatchWithBorder();
};
} // namespace svo
} // namespace vio
#endif // SVO_MATCHER_H_
GPU_version/vio/include/gpu_svo/math_utils.h → GPU_version/vio/include/vio/math_utils.h
View File
GPU_version/vio/include/gpu_svo/nlls_solver.h → GPU_version/vio/include/vio/nlls_solver.h
+11 -6
View File
@@ -16,8 +16,8 @@
#include <math.h>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/robust_cost.h>
#include <vio/math_utils.h>
#include <vio/robust_cost.h>
namespace vk {
@@ -63,6 +63,9 @@ protected:
computeResiduals (const ModelType& model,
bool linearize_system,
bool compute_weight_scale) = 0;
virtual double
computeResiduals (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
@@ -72,6 +75,8 @@ protected:
virtual void
update (const ModelType& old_model, ModelType& new_model) = 0;
virtual void
update () = 0;
virtual void
applyPrior (const ModelType& current_model) { }
@@ -105,7 +110,7 @@ public:
// robust least squares
bool use_weights_;
float scale_;
double scale_=2.0;
robust_cost::ScaleEstimatorPtr scale_estimator_;
robust_cost::WeightFunctionPtr weight_function_;
@@ -126,7 +131,7 @@ public:
eps_(0.0000000001),
iter_(0),
use_weights_(false),
scale_(0.0),
scale_(2.0),
scale_estimator_(NULL),
weight_function_(NULL)
{ }
@@ -137,7 +142,7 @@ public:
void optimize(ModelType& model);
/// Gauss Newton optimization strategy
void optimizeGaussNewton(ModelType& model);
void optimizeGaussNewton();
/// Levenberg Marquardt optimization strategy
void optimizeLevenbergMarquardt(ModelType& model);
@@ -164,6 +169,6 @@ public:
} // end namespace vk
#include "gpu_svo/nlls_solver_impl.hpp"
#include "vio/nlls_solver_impl.hpp"
#endif /* LM_SOLVER_H_ */
GPU_version/vio/include/gpu_svo/nlls_solver_impl.hpp → GPU_version/vio/include/vio/nlls_solver_impl.hpp
+5 -38
View File
@@ -16,48 +16,30 @@ template <int D, typename T>
void vk::NLLSSolver<D, T>::optimize(ModelType& model)
{
if(method_ == GaussNewton)
optimizeGaussNewton(model);
optimizeGaussNewton();
else if(method_ == LevenbergMarquardt)
optimizeLevenbergMarquardt(model);
}
template <int D, typename T>
void vk::NLLSSolver<D, T>::optimizeGaussNewton(ModelType& model)
void vk::NLLSSolver<D, T>::optimizeGaussNewton()
{
// 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);
computeResiduals(false, true);
// 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);
double new_chi2 = computeResiduals(true, false);
// 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_)
{
@@ -67,34 +49,19 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton(ModelType& model)
<< "\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;
update();
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;
GPU_version/vio/include/gpu_svo/output_helper.h → GPU_version/vio/include/vio/output_helper.h
View File
GPU_version/vio/include/gpu_svo/params_helper.h → GPU_version/vio/include/vio/params_helper.h
+3 -3
View File
@@ -2,8 +2,8 @@
// Created by root on 5/24/21.
//
#ifndef GPU_SVO_PARAMS_HELPER_H
#define GPU_SVO_PARAMS_HELPER_H
#ifndef VIO_PARAMS_HELPER_H
#define VIO_PARAMS_HELPER_H
/*
* ros_params_helper.h
*
@@ -54,4 +54,4 @@ namespace vk {
} // namespace vk
#endif //GPU_SVO_PARAMS_HELPER_H
#endif //VIO_PARAMS_HELPER_H
GPU_version/vio/include/gpu_svo/patch_score.h → GPU_version/vio/include/vio/patch_score.h
View File
GPU_version/vio/include/gpu_svo/point.h → GPU_version/vio/include/vio/point.h
+3 -3
View File
@@ -18,14 +18,14 @@
#define SVO_POINT_H_
#include <boost/noncopyable.hpp>
#include <gpu_svo/global.h>
#include <vio/global.h>
namespace g2o {
class VertexSBAPointXYZ;
}
typedef g2o::VertexSBAPointXYZ g2oPoint;
namespace svo {
namespace vio {
class Feature;
@@ -103,6 +103,6 @@ public:
}
};
} // namespace svo
} // namespace vio
#endif // SVO_POINT_H_
GPU_version/vio/include/gpu_svo/pose_optimizer.h → GPU_version/vio/include/vio/pose_optimizer.h
+3 -3
View File
@@ -17,9 +17,9 @@
#ifndef SVO_POSE_OPTIMIZER_H_
#define SVO_POSE_OPTIMIZER_H_
#include <gpu_svo/global.h>
#include <vio/global.h>
namespace svo {
namespace vio {
using namespace Eigen;
using namespace Sophus;
@@ -45,6 +45,6 @@ void optimizeGaussNewton(
size_t& num_obs);
} // namespace pose_optimizer
} // namespace svo
} // namespace vio
#endif // SVO_POSE_OPTIMIZER_H_
GPU_version/vio/include/gpu_svo/reprojector.h → GPU_version/vio/include/vio/reprojector.h
+4 -4
View File
@@ -17,14 +17,14 @@
#ifndef SVO_REPROJECTION_H_
#define SVO_REPROJECTION_H_
#include <gpu_svo/global.h>
#include <gpu_svo/matcher.h>
#include <vio/global.h>
#include <vio/matcher.h>
namespace vk {
class AbstractCamera;
}
namespace svo {
namespace vio {
class Map;
class Point;
@@ -96,6 +96,6 @@ private:
bool reprojectPoint(FramePtr frame, Point* point);
};
} // namespace svo
} // namespace vio
#endif // SVO_REPROJECTION_H_
GPU_version/vio/include/gpu_svo/robust_cost.h → GPU_version/vio/include/vio/robust_cost.h
View File
GPU_version/vio/include/gpu_svo/sparse_img_align.h → GPU_version/vio/include/vio/sparse_img_align.h
+6 -4
View File
@@ -17,15 +17,15 @@
#ifndef SVO_SPARSE_IMG_ALIGN_H_
#define SVO_SPARSE_IMG_ALIGN_H_
#include <gpu_svo/nlls_solver.h>
#include <gpu_svo/global.h>
#include <vio/nlls_solver.h>
#include <vio/global.h>
namespace vk {
class AbstractCamera;
}
namespace svo {
namespace vio {
class Feature;
@@ -72,12 +72,14 @@ protected:
void precomputeReferencePatches();
virtual double computeResiduals(const SE2& model, bool linearize_system, bool compute_weight_scale = false);
virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false){};
virtual int solve();
virtual void update (const ModelType& old_model, ModelType& new_model);
virtual void update(){};
virtual void startIteration();
virtual void finishIteration();
};
} // namespace svo
} // namespace vio
#endif // SVO_SPARSE_IMG_ALIGN_H_
GPU_version/vio/include/vio/sparse_img_align_gpu.h
+83
View File
@@ -0,0 +1,83 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef VIO_SPARSE_IMG_ALIGN_GPU_H_
#define VIO_SPARSE_IMG_ALIGN_GPU_H_
#include <vio/nlls_solver.h>
#include <vio/global.h>
#include <vio/cl_class.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/config.h>
#include <vio/point.h>
#include <vio/abstract_camera.h>
#include <vio/vision.h>
#include <vio/math_utils.h>
#include <algorithm>
namespace vio {
class Feature;
/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
class SparseImgAlignGpu : 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:
SparseImgAlignGpu(
int n_levels,
int min_level,
int n_iter,
Method method,
bool verbose,
opencl* residual);
size_t run(
FramePtr ref_frame,
FramePtr cur_frame);
// FILE* data= nullptr;
protected:
int level_; //!< current pyramid level on which the optimization runs.
int max_level_; //!< coarsest pyramid level for the alignment.
int min_level_; //!< finest pyramid level for the alignment.
size_t feature_counter_=0;
opencl* residual_= nullptr;
// cache:
std::vector<bool> visible_fts_;
virtual double computeResiduals(const ModelType& model,
bool linearize_system,
bool compute_weight_scale) {
return 0.0;
};
virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false);
virtual int solve();
virtual void update();
virtual void update(const ModelType& old_model, ModelType& new_model) { };
virtual void startIteration();
virtual void finishIteration();
};
} // namespace vio
#endif // VIO_SPARSE_IMG_ALIGN_GPU_H_
GPU_version/vio/include/gpu_svo/timer.h → GPU_version/vio/include/vio/timer.h
View File
GPU_version/vio/include/gpu_svo/ukf.h → GPU_version/vio/include/vio/ukf.h
+20 -20
View File
@@ -1,12 +1,12 @@
//
// Created by root on 6/8/21.
//
#ifndef GPU_SVO_UKF_H
#define GPU_SVO_UKF_H
#ifndef VIO_UKF_H
#define VIO_UKF_H
#include <iostream>
#include <fstream>
#include "gpu_svo/config.h"
#include "vio/config.h"
#include <Eigen/Dense>
class Base{
public:
@@ -28,9 +28,9 @@ public:
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();
R<<vio::Config::Cmd_Cov(),0,0,
0,vio::Config::Cmd_Cov(),0,
0,0,vio::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)),
@@ -48,9 +48,9 @@ public:
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();
Q<<vio::Config::ACC_Noise(),0,0,
0,vio::Config::ACC_Noise(),0,
0,0,vio::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);
@@ -61,9 +61,9 @@ public:
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();
R<<vio::Config::ACC_Noise(),0,0,
0,vio::Config::ACC_Noise(),0,
0,0,vio::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))),
@@ -82,9 +82,9 @@ public:
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();
Q<<vio::Config::Svo_Ekf(),0,0,
0,vio::Config::Svo_Ekf(),0,
0,0,vio::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);
@@ -124,7 +124,7 @@ public:
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) {
std::pair<Eigen::Matrix<double,3,3>,vio::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)
@@ -138,18 +138,18 @@ public:
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));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
};
std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5> get_location(){
std::pair<Eigen::Matrix<double,3,3>,vio::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));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
}
private:
Base* filter_= nullptr;
bool lock=false;
};
#endif //GPU_SVO_UKF_H
#endif //VIO_UKF_H
GPU_version/vio/include/gpu_svo/vision.h → GPU_version/vio/include/vio/vision.h
+1 -1
View File
@@ -8,7 +8,7 @@
#ifndef VIKIT_VISION_H_
#define VIKIT_VISION_H_
#include <gpu_svo/aligned_mem.h>
#include <vio/aligned_mem.h>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
GPU_version/vio/include/gpu_svo/visualizer.h → GPU_version/vio/include/vio/visualizer.h
+4 -4
View File
@@ -19,16 +19,16 @@
#include <queue>
#include <ros/ros.h>
#include <gpu_svo/global.h>
#include <vio/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>
#include <vio/ukf.h>
namespace svo {
namespace vio {
class Frame;
class Point;
@@ -57,6 +57,6 @@ public:
};
} // end namespace svo
} // end namespace vio
#endif /* SVO_VISUALIZER_H_ */
GPU_version/vio/kernel/compute-residual.cl
+153
View File
@@ -0,0 +1,153 @@
// Copyright (C) 2021 Majid Geravand
// Copyright (C) 2021 Gfuse
// Enable OpenCL 32-bit integer atomic functions.
#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
double2 world2cam(double3 feature)
{
double r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2));
double factor = 1.0;
if(S != 0 || r > 0.001)
factor = (atan(r * 2.0 * tan(0.5 * S)) / (r * S));
return (double2)(C_X + F_X * factor * feature.x/feature.z, C_Y + F_Y * factor * feature.y/feature.z);
}
double3 xyz_cur(double3 cur, double3 ref, double3 ref_feature)
{
if(ref.z < 0.0)
ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
else
ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
double cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
double sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
return (double3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
ref_feature.y,
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
}
void jacobian_xyz2uv(double3 xyz_in_f, double* J)
{
*(J + 0) = -(1. / xyz_in_f[2]); // -1/z
*(J + 1) = (xyz_in_f[0]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // x/z^2
*(J + 2) = -(1.0 + pow((xyz_in_f[0]),2) / ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2]))); // -(1.0 + x^2/z^2)
*(J + 3) = 1e-19; // 0
*(J + 4) = (xyz_in_f[1]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // y/z^2
*(J + 5) = -(xyz_in_f[0]) * (xyz_in_f[1]) / ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // -x*y/z^2
}
void compute_hessain(double3 j, __global double* H, double w)
{
*(H + 0) += j[0] * j[0] * w;
*(H + 1) += j[0] * j[1] * w;
*(H + 2) += j[0] * j[2] * w;
*(H + 3) += j[1] * j[0] * w;
*(H + 4) += j[1] * j[1] * w;
*(H + 5) += j[1] * j[2] * w;
*(H + 6) += j[2] * j[0] * w;
*(H + 7) += j[2] * j[1] * w;
*(H + 8) += j[2] * j[2] * w;
}
__kernel void compute_residual(
__read_only image2d_t image_cur, // current frame
__read_only image2d_t image_ref, // reference frame
__global double3 * cur_pose,//[reference frame pose{x,z,pitch}]
__global double3 * ref_pose,//[current frame pose{x,z,pitch}]
__global double3 * ref_feature, // feature on the reference frame, when we applied the distance calculation: xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm());
__global double2 * featue_px,
int level,
__global float * errors,
__global double * Hessian,
__global double3 * Jacobian,
__global double * chi,
double scale_
)
{
double scale = pow(2.0, -level);
// Prepare a suitable OpenCL image sampler.
sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
// Use global work feature.
int f = get_global_id(0);
// check if reference with patch size is within image
double2 uv_ref = featue_px[f] * scale;
double2 uv_ref_i = floor(uv_ref);
if(uv_ref_i.x - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.y - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.x + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[0] || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[1])return;
// evaluate projection jacobian
double frame_jac[6]={0.0}; // 2X3
jacobian_xyz2uv(sqrt(pow(ref_feature[f] - ref_pose[0], 2.0)), &frame_jac);
// compute bilateral interpolation weights for reference image
double2 subpix= uv_ref - uv_ref_i;
double w_ref_tl = (1.0 - subpix.x) * (1.0 - subpix.y);
double w_ref_tr = subpix.x * (1.0 - subpix.y);
double w_ref_bl = (1.0 - subpix.x) * subpix.y;
double w_ref_br = subpix.x * subpix.y;
// compute bilateral interpolation weights for the current image
double2 uv_cur_pyr = world2cam(xyz_cur(cur_pose[0], ref_pose[0] ,ref_feature[f])) * scale;
double2 uv_cur_i = floor(uv_cur_pyr);
double2 subpix_uv_cur = uv_cur_pyr - uv_cur_i;
double w_cur_tl = (1.0 - subpix_uv_cur.x) * (1.0 - subpix_uv_cur.y);
double w_cur_tr = subpix_uv_cur.x * (1.0 - subpix_uv_cur.y);
double w_cur_bl = (1.0 - subpix_uv_cur.x) * subpix_uv_cur.y;
double w_cur_br = subpix_uv_cur.x * subpix_uv_cur.y;
float e = 0.0;
double chi_=0.0;
for(int y = 0; y < PATCH_SIZE; ++y)
{
int ref_element_addr = (int)(uv_ref_i.y + y - PATCH_HALFSIZE) * get_image_dim(image_ref)[0] + (uv_ref_i.x - PATCH_HALFSIZE);
int cur_element_addr = (int)(uv_cur_i.y + y - PATCH_HALFSIZE) * get_image_dim(image_cur)[0] + (uv_cur_i.x - PATCH_HALFSIZE);
for(int x = 0; x < PATCH_SIZE; ++x, ++ref_element_addr, ++cur_element_addr)
{
// precompute interpolated reference patch color
int2 px_reftl = (int2)( ref_element_addr % get_image_dim(image_ref)[0], ref_element_addr / get_image_dim(image_ref)[0]);
int2 px_reftr = (int2)((ref_element_addr + 1) % get_image_dim(image_ref)[0], (ref_element_addr + 1) / get_image_dim(image_ref)[0]);
int2 px_refbl = (int2)((ref_element_addr + get_image_dim(image_ref)[0]) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0]) / get_image_dim(image_ref)[0]);
int2 px_refbr = (int2)((ref_element_addr + get_image_dim(image_ref)[0] + 1) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0] + 1) / get_image_dim(image_ref)[0]);
double value = w_ref_tl * read_imageui(image_ref, sampler, px_reftl).x + w_ref_tr * read_imageui(image_ref, sampler, px_reftr).x +
w_ref_bl * read_imageui(image_ref, sampler, px_refbl).x + w_ref_br * read_imageui(image_ref, sampler, px_refbr).x;
double dx = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x +
w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 2) % get_image_dim(image_ref)[0], ((ref_element_addr + 2) / get_image_dim(image_ref)[0]))).x +
w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) / get_image_dim(image_ref)[0]))).x +
w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2) / get_image_dim(image_ref)[0]))).x)
-(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr - 1) % get_image_dim(image_ref)[0], ((ref_element_addr - 1) / get_image_dim(image_ref)[0]))).x +
w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0) % get_image_dim(image_ref)[0], ((ref_element_addr + 0) / get_image_dim(image_ref)[0]))).x +
w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1) / get_image_dim(image_ref)[0]))).x +
w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x));
double dy = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x +
w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) / get_image_dim(image_ref)[0]))).x +
w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2) / get_image_dim(image_ref)[0]))).x +
w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) / get_image_dim(image_ref)[0]))).x)
-(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (-get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (-get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x +
w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (1 - get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (1 - get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x +
w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0) % get_image_dim(image_ref)[0], ((ref_element_addr + 0) / get_image_dim(image_ref)[0]))).x +
w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x));
// compute residual
int2 px_curtl = (int2)( cur_element_addr % get_image_dim(image_cur)[0], cur_element_addr / get_image_dim(image_cur)[0]);
int2 px_curtr = (int2)((cur_element_addr + 1) % get_image_dim(image_cur)[0], (cur_element_addr + 1) / get_image_dim(image_cur)[0]);
int2 px_curbl = (int2)((cur_element_addr + (get_image_dim(image_cur)[0])) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0])) / get_image_dim(image_cur)[0]);
int2 px_curbr = (int2)((cur_element_addr + (get_image_dim(image_cur)[0]) + 1) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0]) + 1) / get_image_dim(image_cur)[0]);
double res = value - w_cur_tl * read_imageui(image_cur, sampler, px_curtl).x + w_cur_tr * read_imageui(image_cur, sampler, px_curtr).x +
w_cur_bl * read_imageui(image_cur, sampler, px_curbl).x + w_cur_br * read_imageui(image_cur, sampler, px_curbr).x;
// used to compute scale for robust cost
e += fabs(res);
// robustification
double weight = res/scale_; //1.48f * vk::getMedian(errors)
chi_ += pow(res,2) * weight;
double3 j_row0 = (double3)((dx * frame_jac[0]), (dx * frame_jac[1]), (dx * frame_jac[2]));
double3 j_row1 = (double3)((dy * frame_jac[3]), (dy * frame_jac[4]), (dy * frame_jac[5]));
double3 J = (j_row0 + j_row1) * (F_X / scale);
compute_hessain(J, &Hessian[f*9], weight);
Jacobian[f] -= J * res * weight;
}
}
errors[f] = e / pow(PATCH_SIZE,2.0);
chi[f]=chi_;
}
GPU_version/vio/kernel/fast-gray.cl
+1 -1
View File
@@ -17,7 +17,7 @@ __kernel void fast_gray(
// Use global work item as 2D image coordinates.
int const x = get_global_id(0);
int const y = get_global_id(1);
if(x>16 && x<get_image_width(image)-16 && y<get_image_height(image)-16 && y>16){
if(x>5 && x<get_image_width(image)-5 && y<get_image_height(image)-5 && y>5){
int2 const xy = (int2)(x, y);
// Read the candidate pixel.
int const p00 = read_imageui(image, sampler, xy).x;
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch>
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/>
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/ -->
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/out.txt
BIN
View File
Binary file not shown.
GPU_version/vio/param/vo_fast.yaml
+1 -1
View File
@@ -3,7 +3,7 @@ svo:
max_n_kfs: 20 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3.
loba_num_iter: 0 #Number of iterations in the local bundle adjustment.
quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad.
quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad.
quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
init_min_tracked: 100
init_min_disparity: 30
structureoptim_max_pts: 50
GPU_version/vio/src/atan_camera.cpp
+6 -3
View File
@@ -7,8 +7,8 @@
#include <math.h>
#include <gpu_svo/atan_camera.h>
#include <gpu_svo/math_utils.h>
#include <vio/atan_camera.h>
#include <vio/math_utils.h>
namespace vk {
@@ -36,11 +36,14 @@ ATANCamera(double width, double height,
tans_ = 0.0;
distortion_ = false;
}
param_ = (double *) malloc(5 * sizeof(double));
}
ATANCamera::
~ATANCamera()
{}
{
delete param_;
}
Vector3d ATANCamera::
cam2world(const double& x, const double& y) const
GPU_version/vio/src/cl_class.cpp
+25 -16
View File
@@ -1,8 +1,8 @@
//
// Created by root on 4/27/21.
//
#include "gpu_svo/cl_class.h"
opencl::opencl() {
#include <vio/cl_class.h>
opencl::opencl(vk::AbstractCamera* cam):cam(cam) {
std::vector<cl::Platform> all_platforms;
cl::Platform::get(&all_platforms);
if (all_platforms.size() == 0){
@@ -20,29 +20,38 @@ opencl::opencl() {
std::cout << " No devices found. Check OpenCL installation!\n";
exit(1);
}
device = all_devices[0];
std::cout << "CL_DEVICE_NAME: " << device.getInfo<CL_DEVICE_NAME>() <<'\n'
<< "CL_DEVICE_OPENCL_C_VERSION: " <<device.getInfo<CL_DEVICE_OPENCL_C_VERSION>()<<'\n'
<< "CL_DEVICE_BUILT_IN_KERNELS: " <<device.getInfo<CL_DEVICE_BUILT_IN_KERNELS>()<<'\n'
<< "CL_DEVICE_COMPILER_AVAILABLE: " <<device.getInfo<CL_DEVICE_COMPILER_AVAILABLE>()<<'\n'
<< "CL_DEVICE_LOCAL_MEM_SIZE: " <<device.getInfo<CL_DEVICE_LOCAL_MEM_SIZE>()<<'\n'
<< "CL_DEVICE_GLOBAL_MEM_SIZE: " <<device.getInfo<CL_DEVICE_GLOBAL_MEM_SIZE>()<<'\n';
context=new cl::Context({ device });
device = new cl::Device(all_devices[0]);
std::cout << "CL_DEVICE_NAME: " << device->getInfo<CL_DEVICE_NAME>() <<'\n'
<< "CL_DEVICE_OPENCL_C_VERSION: " <<device->getInfo<CL_DEVICE_OPENCL_C_VERSION>()<<'\n'
<< "CL_DEVICE_BUILT_IN_KERNELS: " <<device->getInfo<CL_DEVICE_BUILT_IN_KERNELS>()<<'\n'
<< "CL_DEVICE_COMPILER_AVAILABLE: " <<device->getInfo<CL_DEVICE_COMPILER_AVAILABLE>()<<'\n'
<< "CL_DEVICE_LOCAL_MEM_SIZE: " <<device->getInfo<CL_DEVICE_LOCAL_MEM_SIZE>()<<'\n'
<< "CL_DEVICE_GLOBAL_MEM_SIZE: " <<device->getInfo<CL_DEVICE_GLOBAL_MEM_SIZE>()<<'\n';
context=new cl::Context({ *device });
cl::Program::Sources sources;
read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl");
sources.push_back({ fast.src_str, fast.size });
read_cl compute_residual(std::string(KERNEL_DIR)+"/compute-residual.cl");
sources.push_back({ compute_residual.src_str, compute_residual.size });
program=new cl::Program(*context, sources);
if(program->build({ device },"-DFAST_THRESH=40") !=0)
std::cout << " Error building: " << program->getBuildInfo<CL_PROGRAM_BUILD_STATUS>(device)<<'\n'
<<program->getBuildInfo<CL_PROGRAM_BUILD_LOG>(device) << '\n';
queue=new cl::CommandQueue(*context,device,0,NULL);
double* camera=cam->params();
std::string options="-DFAST_THRESH=40 -DPATCH_SIZE=2 -DPATCH_HALFSIZE=1 -DF_X="+ std::to_string(camera[0]) +
" -DF_Y="+std::to_string(camera[1])+
" -DC_X="+std::to_string(camera[2])+
" -DC_Y="+std::to_string(camera[3])+
" -DS="+std::to_string(camera[4]);
if(program->build({ *device },options.c_str()) !=0)
std::cout << " Error building: " << program->getBuildInfo<CL_PROGRAM_BUILD_STATUS>(*device)<<'\n'
<<program->getBuildInfo<CL_PROGRAM_BUILD_LOG>(*device) << '\n';
queue=new cl::CommandQueue(*context,*device,CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,NULL);
}
opencl::~opencl() {
delete context;
delete device;
}
void opencl::clear_buf() {
queue->finish();
queue->flush();
_kernels.clear();
delete program;
}
GPU_version/vio/src/config.cpp
+4 -4
View File
@@ -14,10 +14,10 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <gpu_svo/params_helper.h>
#include <gpu_svo/config.h>
#include <vio/params_helper.h>
#include <vio/config.h>
namespace svo {
namespace vio {
Config::Config() :
trace_name(vk::getParam<string>("svo/trace_name", "svo")),
@@ -69,5 +69,5 @@ Config& Config::getInstance()
return instance;
}
} // namespace svo
} // namespace vio
GPU_version/vio/src/depth_filter.cpp
+13 -13
View File
@@ -15,21 +15,21 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/abstract_camera.h>
#include <gpu_svo/vision.h>
#include <vio/math_utils.h>
#include <vio/abstract_camera.h>
#include <vio/vision.h>
#include <boost/bind.hpp>
#include <boost/math/distributions/normal.hpp>
#include <gpu_svo/global.h>
#include <gpu_svo/depth_filter.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/point.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/matcher.h>
#include <gpu_svo/config.h>
#include <gpu_svo/feature_detection.h>
#include <vio/global.h>
#include <vio/depth_filter.h>
#include <vio/frame.h>
#include <vio/point.h>
#include <vio/feature.h>
#include <vio/matcher.h>
#include <vio/config.h>
#include <vio/feature_detection.h>
namespace svo {
namespace vio {
int Seed::batch_counter = 0;
int Seed::seed_counter = 0;
@@ -354,4 +354,4 @@ double DepthFilter::computeTau(
return (t_norm*sin(beta_plus)/sin(PI-acos(f.dot(t)/t_norm)-beta_plus) - z); // ( triangle angles sum to PI and law of sines ) tau
}
} // namespace svo
} // namespace vio
GPU_version/vio/src/feature_alignment.cpp
+3 -3
View File
@@ -20,9 +20,9 @@
#ifdef __ARM_NEON__
#include <arm_neon.h>
#endif
#include <gpu_svo/feature_alignment.h>
#include <vio/feature_alignment.h>
namespace svo {
namespace vio {
namespace feature_alignment {
#define SUBPIX_VERBOSE 0
@@ -613,4 +613,4 @@ bool align2D_NEON (
}
} // namespace feature_alignment
} // namespace svo
} // namespace vio
GPU_version/vio/src/feature_detection.cpp
+6 -6
View File
@@ -14,11 +14,11 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <gpu_svo/feature_detection.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/vision.h>
#include <vio/feature_detection.h>
#include <vio/feature.h>
#include <vio/vision.h>
namespace svo {
namespace vio {
namespace feature_detection {
AbstractDetector::AbstractDetector(
@@ -79,7 +79,7 @@ void FastDetector::detect(
gpu_fast_->reload_buf(0,0,img);
int icorner[1]={0};
gpu_fast_->reload_buf(0,2,icorner);
gpu_fast_->run(0,img_pyr[L].cols,img_pyr[L].rows);
gpu_fast_->run(0,img.cols,img.rows);
cl_int2 fast_corners[350000];
gpu_fast_->read(0,1,fast_corners);
int count[1]={0};
@@ -110,5 +110,5 @@ void FastDetector::detect(
}
} // namespace feature_detection
} // namespace svo
} // namespace vio
GPU_version/vio/src/frame.cpp
+8 -8
View File
@@ -15,14 +15,14 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <stdexcept>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/config.h>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/vision.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/point.h>
#include <vio/config.h>
#include <vio/math_utils.h>
#include <vio/vision.h>
namespace svo {
namespace vio {
int Frame::frame_counter_ = 0;
@@ -186,4 +186,4 @@ bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
}
} // namespace frame_utils
} // namespace svo
} // namespace vio
GPU_version/vio/src/frame_handler_base.cpp
+9 -9
View File
@@ -14,19 +14,19 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <gpu_svo/abstract_camera.h>
#include <vio/abstract_camera.h>
#include <stdlib.h>
#include <Eigen/StdVector>
#include <boost/bind.hpp>
#include <fstream>
#include <gpu_svo/frame_handler_base.h>
#include <gpu_svo/config.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/matcher.h>
#include <gpu_svo/map.h>
#include <gpu_svo/point.h>
#include <vio/frame_handler_base.h>
#include <vio/config.h>
#include <vio/feature.h>
#include <vio/matcher.h>
#include <vio/map.h>
#include <vio/point.h>
namespace svo
namespace vio
{
// definition of global and static variables which were declared in the header
@@ -137,4 +137,4 @@ void FrameHandlerBase::optimizeStructure(
}
} // namespace svo
} // namespace vio
GPU_version/vio/src/frame_handler_mono.cpp
+40 -16
View File
@@ -14,18 +14,19 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <gpu_svo/config.h>
#include <gpu_svo/frame_handler_mono.h>
#include <gpu_svo/map.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/pose_optimizer.h>
#include <gpu_svo/sparse_img_align.h>
#include <gpu_svo/depth_filter.h>
#include <gpu_svo/for_it.hpp>
#include <vio/config.h>
#include <vio/frame_handler_mono.h>
#include <vio/map.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/point.h>
#include <vio/pose_optimizer.h>
#include <vio/sparse_img_align.h>
#include <vio/depth_filter.h>
#include <vio/for_it.hpp>
#include <assert.h>
namespace svo {
namespace vio {
FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init) :
FrameHandlerBase(),
@@ -35,15 +36,33 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
ukfPtr_(init),
time_(ros::Time::now())
{
gpu_fast_= new opencl();
gpu_fast_= new opencl(cam_);
gpu_fast_->make_kernel("fast_gray");
cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1);
gpu_fast_->write_buf(0,0,img);
cl_int2 corners_[350000];
cl_int2 corners_[350000]={0};
gpu_fast_->write_buf(0,1,350000,corners_);
int icorner[1]={0};
gpu_fast_->write_buf(0,2,1,icorner);
gpu_fast_->write_buf(0,3,(1 << 18));
gpu_fast_->make_kernel("compute_residual");
gpu_fast_->write_buf(1,0,img);
gpu_fast_->write_buf(1,1,img);
cl_double3 pose[1]={0.0};
gpu_fast_->write_buf(1,2,3,pose);
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);
double e[300]={0};
gpu_fast_->write_buf(1,7,300,e);
double H[9*300]={0};
gpu_fast_->write_buf(1,8,9*300,H);
cl_double3 J[300];
gpu_fast_->write_buf(1,9,300,J);
double chi2[1]={0.01};
gpu_fast_->write_buf(1,10,1,chi2);
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
initialize();
}
@@ -140,10 +159,15 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
// sparse image align
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::LevenbergMarquardt, false, false);
//SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false);
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//assert (false);
reprojector_.reprojectMap(new_frame_, overlap_kfs_);
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
@@ -299,4 +323,4 @@ void FrameHandlerMono::UpdateCmd(double* value,const ros::Time& time){
ukfPtr_.UpdateCmd(value[0],value[1],value[2],time);
}
} // namespace svo
} // namespace vio
GPU_version/vio/src/homography.cpp
+1 -1
View File
@@ -12,7 +12,7 @@
* by: cforster
*/
#include <gpu_svo/homography.h>
#include <vio/homography.h>
#include <opencv2/opencv.hpp>
#include <stdio.h>
GPU_version/vio/src/initialization.cpp
+10 -10
View File
@@ -14,16 +14,16 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <gpu_svo/config.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/point.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/initialization.h>
#include <gpu_svo/feature_detection.h>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/homography.h>
#include <vio/config.h>
#include <vio/frame.h>
#include <vio/point.h>
#include <vio/feature.h>
#include <vio/initialization.h>
#include <vio/feature_detection.h>
#include <vio/math_utils.h>
#include <vio/homography.h>
namespace svo {
namespace vio {
namespace initialization {
InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
@@ -197,4 +197,4 @@ void computeHomography(
} // namespace initialization
} // namespace svo
} // namespace vio
GPU_version/vio/src/map.cpp
+6 -6
View File
@@ -15,13 +15,13 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <set>
#include <gpu_svo/map.h>
#include <gpu_svo/point.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <vio/map.h>
#include <vio/point.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <boost/bind.hpp>
namespace svo {
namespace vio {
Map::Map() {}
@@ -376,4 +376,4 @@ void mapStatistics(Map* map)
}
} // namespace map_debug
} // namespace svo
} // namespace vio
GPU_version/vio/src/matcher.cpp
+12 -12
View File
@@ -15,18 +15,18 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <cstdlib>
#include <gpu_svo/abstract_camera.h>
#include <gpu_svo/vision.h>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/patch_score.h>
#include <gpu_svo/matcher.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/config.h>
#include <gpu_svo/feature_alignment.h>
#include <vio/abstract_camera.h>
#include <vio/vision.h>
#include <vio/math_utils.h>
#include <vio/patch_score.h>
#include <vio/matcher.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/point.h>
#include <vio/config.h>
#include <vio/feature_alignment.h>
namespace svo {
namespace vio {
namespace warp {
@@ -324,4 +324,4 @@ bool Matcher::findEpipolarMatchDirect(
return false;
}
} // namespace svo
} // namespace vio
GPU_version/vio/src/math_utils.cpp
+1 -1
View File
@@ -5,7 +5,7 @@
* Author: cforster
*/
#include <gpu_svo/math_utils.h>
#include <vio/math_utils.h>
namespace vk {
GPU_version/vio/src/output_helper.cpp
+1 -1
View File
@@ -5,7 +5,7 @@
* Author: chrigi
*/
#include <gpu_svo/output_helper.h>
#include <vio/output_helper.h>
#include <visualization_msgs/Marker.h>
GPU_version/vio/src/point.cpp
+6 -6
View File
@@ -15,12 +15,12 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <stdexcept>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/point.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <vio/math_utils.h>
#include <vio/point.h>
#include <vio/frame.h>
#include <vio/feature.h>
namespace svo {
namespace vio {
int Point::point_counter_ = 0;
@@ -175,4 +175,4 @@ void Point::optimize(const size_t n_iter)
#endif
}
} // namespace svo
} // namespace vio
GPU_version/vio/src/pose_optimizer.cpp
+8 -8
View File
@@ -15,15 +15,15 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <stdexcept>
#include <gpu_svo/pose_optimizer.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/point.h>
#include <gpu_svo/robust_cost.h>
#include <gpu_svo/math_utils.h>
#include <vio/pose_optimizer.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/point.h>
#include <vio/robust_cost.h>
#include <vio/math_utils.h>
#include <sophus/se2.h>
namespace svo {
namespace vio {
namespace pose_optimizer {
void optimizeGaussNewton(
@@ -175,4 +175,4 @@ void optimizeGaussNewton(
}
} // namespace pose_optimizer
} // namespace svo
} // namespace vio
GPU_version/vio/src/reprojector.cpp
+11 -11
View File
@@ -16,19 +16,19 @@
#include <algorithm>
#include <stdexcept>
#include <gpu_svo/reprojector.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/point.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/map.h>
#include <gpu_svo/config.h>
#include <vio/reprojector.h>
#include <vio/frame.h>
#include <vio/point.h>
#include <vio/feature.h>
#include <vio/map.h>
#include <vio/config.h>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <gpu_svo/abstract_camera.h>
#include <gpu_svo/math_utils.h>
#include <gpu_svo/timer.h>
#include <vio/abstract_camera.h>
#include <vio/math_utils.h>
#include <vio/timer.h>
namespace svo {
namespace vio {
Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) :
map_(map)
@@ -216,4 +216,4 @@ bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
return false;
}
} // namespace svo
} // namespace vio
GPU_version/vio/src/robust_cost.cpp
+2 -2
View File
@@ -20,8 +20,8 @@
#include <numeric>
#include <algorithm>
#include <gpu_svo/robust_cost.h>
#include <gpu_svo/math_utils.h>
#include <vio/robust_cost.h>
#include <vio/math_utils.h>
namespace vk {
namespace robust_cost {
GPU_version/vio/src/sparse_img_align.cpp
+15 -17
View File
@@ -15,16 +15,16 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <gpu_svo/sparse_img_align.h>
#include <gpu_svo/frame.h>
#include <gpu_svo/feature.h>
#include <gpu_svo/config.h>
#include <gpu_svo/point.h>
#include <gpu_svo/abstract_camera.h>
#include <gpu_svo/vision.h>
#include <gpu_svo/math_utils.h>
#include <vio/sparse_img_align.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/config.h>
#include <vio/point.h>
#include <vio/abstract_camera.h>
#include <vio/vision.h>
#include <vio/math_utils.h>
namespace svo {
namespace vio {
SparseImgAlign::SparseImgAlign(
int max_level, int min_level, int n_iter,
@@ -37,7 +37,7 @@ SparseImgAlign::SparseImgAlign(
n_iter_init_ = n_iter_;
method_ = method;
verbose_ = verbose;
eps_ = 0.0001;
eps_ = 1e-5;
}
size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
@@ -99,8 +99,8 @@ void SparseImgAlign::precomputeReferencePatches()
*visiblity_it = true;
// cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!!
const double depth(((*it)->point->pos_ - ref_pos).norm());
const Vector3d xyz_ref((*it)->f*depth);
// const double depth(((*it)->point->pos_ - ref_pos).norm());
const Vector3d xyz_ref((*it)->f*((*it)->point->pos_ - ref_pos).norm());
// evaluate projection jacobian
Matrix<double,2,3> frame_jac;
@@ -171,8 +171,8 @@ double SparseImgAlign::computeResiduals(
continue;
// compute pixel location in cur img
const double depth = ((*it)->point->pos_ - ref_pos).norm();
const Vector3d xyz_ref((*it)->f*depth);
//const double depth = ((*it)->point->pos_ - ref_pos).norm();
const Vector3d xyz_ref((*it)->f*((*it)->point->pos_ - ref_pos).norm());
const Vector3d xyz_cur((T_cur_from_ref.rotation_matrix()(0,0)*xyz_ref.x())+(T_cur_from_ref.rotation_matrix()(0,1)*xyz_ref.z())+T_cur_from_ref.translation()(0),
xyz_ref.y(),
-1.0*(T_cur_from_ref.rotation_matrix()(1,0)*xyz_ref.x())+(T_cur_from_ref.rotation_matrix()(1,1)*xyz_ref.z())+T_cur_from_ref.translation()(1));
@@ -224,8 +224,6 @@ double SparseImgAlign::computeResiduals(
const Vector3d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter));
H_.noalias() += J*J.transpose()*weight;
Jres_.noalias() -= J*res*weight;
if(display_)
resimg_.at<float>((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0;
}
}
}
@@ -265,5 +263,5 @@ void SparseImgAlign::finishIteration()
}
}
} // namespace svo
} // namespace vio
GPU_version/vio/src/sparse_img_align_gpu.cpp
+156
View File
@@ -0,0 +1,156 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <vio/sparse_img_align_gpu.h>
namespace vio {
SparseImgAlignGpu::SparseImgAlignGpu(
int max_level, int min_level, int n_iter,
Method method, bool verbose,opencl* residual) :
max_level_(max_level),
min_level_(min_level),
residual_(residual)
{
n_iter_ = n_iter;
n_iter_init_ = n_iter_;
method_ = method;
verbose_ = verbose;
eps_ = 1e-10;
// data = fopen((std::string(getenv("HOME"))+"/Project/data.txt").c_str(), "w+");
}
size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
{
reset();
if(ref_frame->fts_.empty())
{
return 0;
}
cl_double3 ref_pos[1]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()};
cl_double3 cur_pos[1]={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);
cl_double3 features[ref_frame->fts_.size()];
cl_double2 featue_px[ref_frame->fts_.size()];
// fprintf(data, "ref_frame:\n %f,%f,%f\n",ref_pos[0],ref_pos[1],ref_pos[2]);
// fprintf(data, "cur_frame:\n %f,%f,%f\n",cur_pos[0],cur_pos[1],cur_pos[2]);
// fprintf(data, "camera:\n %f,%f,%f,%f,%f\n features:\n",camera[0],camera[1],camera[2],camera[3],camera[4]);
feature_counter_ = 0; // is used to compute the index of the cached jacobian
for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){
if(*it == nullptr)continue;
if((*it)->point == nullptr)continue;
Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0].x,0.0,ref_pos[0].y)).norm();
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);
// fprintf(data, "%f,%f,%f,%f,%f\n",xyz_ref(0),xyz_ref(1),xyz_ref(2),(*it)->px(0),(*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_)
{
cv::Mat cur_img = cur_frame->img_pyr_.at(level_);
cv::Mat ref_img = ref_frame->img_pyr_.at(level_);
// cv::imwrite(std::string(getenv("HOME"))+"/Project/cur"+to_string(level_)+".png",cur_img);
// cv::imwrite(std::string(getenv("HOME"))+"/Project/fer"+to_string(level_)+".png",ref_img);
residual_->reload_buf(1,0,cur_img);
residual_->reload_buf(1,1,ref_img);
residual_->write_buf(1,6,level_);
mu_ = 1.0;
optimize(T_cur);
}
cl_double3 pos[1]={0};
residual_->read(0,2,pos);
cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
return 1;
}
double SparseImgAlignGpu::computeResiduals(
bool linearize_system,
bool compute_weight_scale)
{
double H[9*300]={0};
cl_double3 J[300]={0.0};
double chi2_[300]={0.0};
residual_->write_buf(1,11,scale_);
residual_->reload_buf(1,10,chi2_);
residual_->reload_buf(1,8,H);
residual_->reload_buf(1,9,J);
/// TODO GPU compute based on the feature
residual_->run(1,feature_counter_);
float error[300]={0.0};
double chi[300]={0.0};
residual_->read(1,10,feature_counter_,chi);
residual_->read(1,7,feature_counter_,error);
for(int i = 1; i < feature_counter_; ++i){
error[0] += error[i];
chi[0] += chi[i];
}
scale_ = error[0]*(1.48f / feature_counter_);
return chi[0]/(feature_counter_*8);
}
int SparseImgAlignGpu::solve()
{
double H[300*9]={0.0};
cl_double3 J[300]={0.0};
residual_->read(1,8,feature_counter_*9,H);
residual_->read(1,9,feature_counter_,J);
for(int i = 1; i < feature_counter_; ++i){
H[0] += H[i*9];
H[1] += H[i*9+1];
H[2] += H[i*9+2];
H[3] += H[i*9+3];
H[4] += H[i*9+4];
H[5] += H[i*9+5];
H[6] += H[i*9+6];
H[7] += H[i*9+7];
H[8] += H[i*9+8];
J[0].x -= J[i].x;
J[0].y -= J[i].y;
J[0].z -= J[i].z;
}
x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Vector3d(J[0].x,J[0].y,J[0].z));
if((bool) std::isnan((double) x_[0]))
return 0;
return 1;
}
void SparseImgAlignGpu::update()
{
/// TODO the update situation may have a smarter solution
cl_double3 pos[1]={0.0,0.0,0.0};
residual_->read(1,2,pos);
Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-0.5*x_);
pos[0].x=update.translation()(0);
pos[0].y=update.translation()(1);
pos[0].z=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());
residual_->reload_buf(1,2,pos);
}
void SparseImgAlignGpu::startIteration()
{}
void SparseImgAlignGpu::finishIteration()
{
}
} // namespace vio
GPU_version/vio/src/vision.cpp
+1 -1
View File
@@ -5,7 +5,7 @@
* Author: cforster
*/
#include <gpu_svo/vision.h>
#include <vio/vision.h>
#if __SSE2__
# include <emmintrin.h>
GPU_version/vio/src/visualizer.cpp
+10 -10
View File
@@ -14,19 +14,19 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <gpu_svo/visualizer.h>
#include <gpu_svo/frame_handler_mono.h>
#include <gpu_svo/point.h>
#include <gpu_svo/map.h>
#include <gpu_svo/feature.h>
#include <vio/visualizer.h>
#include <vio/frame_handler_mono.h>
#include <vio/point.h>
#include <vio/map.h>
#include <vio/feature.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <gpu_svo/timer.h>
#include <gpu_svo/output_helper.h>
#include <vio/timer.h>
#include <vio/output_helper.h>
#include <deque>
#include <gpu_svo/ukf.h>
#include <vio/ukf.h>
namespace svo {
namespace vio {
Visualizer::
Visualizer() :
@@ -77,4 +77,4 @@ void Visualizer::publishMinimal(
}
}
} // end namespace gpu_svo
} // end namespace vio
GPU_version/vio/src/vo_node.cpp
+10 -10
View File
@@ -17,8 +17,8 @@
#include <ros/ros.h>
#include <string>
#include <chrono>
#include <gpu_svo/frame_handler_mono.h>
#include <gpu_svo/params_helper.h>
#include <vio/frame_handler_mono.h>
#include <vio/params_helper.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Twist.h>
@@ -26,21 +26,21 @@
#include <boost/thread.hpp>
#include <cv_bridge/cv_bridge.h>
#include <Eigen/Core>
#include <gpu_svo/abstract_camera.h>
#include <gpu_svo/camera_loader.h>
#include <gpu_svo/frame.h>
#include <vio/abstract_camera.h>
#include <vio/camera_loader.h>
#include <vio/frame.h>
#include <vio/getOdom.h>
#include <vio/start.h>
#include <vio/stop.h>
#include <gpu_svo/depth_filter.h>
#include <vio/depth_filter.h>
#if VIO_DEBUG
#include <gpu_svo/visualizer.h>
#include <vio/visualizer.h>
#endif
class VioNode
{
public:
svo::FrameHandlerMono* vo_;
vio::FrameHandlerMono* vo_;
vk::AbstractCamera* cam_;
VioNode();
~VioNode();
@@ -54,7 +54,7 @@ public:
bool start(vio::start::Request& req, vio::start::Response& res){
if(req.on==1){
start_=true;
if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME)
if(vo_->stage()!=vio::FrameHandlerBase::STAGE_DEFAULT_FRAME)
vo_->start();
else{
vo_->depthFilter()->startThread();
@@ -106,7 +106,7 @@ VioNode::VioNode() :
throw std::runtime_error("Camera model not correctly specified.");
Eigen::Matrix<double,3,1> init;
init<<1e-19,1e-19,1e-19;
vo_ = new svo::FrameHandlerMono(cam_,init);
vo_ = new vio::FrameHandlerMono(cam_,init);
usleep(500);
}
GPU_version/vio/test/test_pipeline.cpp
+2 -2
View File
@@ -30,7 +30,7 @@
#include <iostream>
#include "test_utils.h"
namespace svo {
namespace vio {
class BenchmarkNode
{
@@ -84,7 +84,7 @@ void BenchmarkNode::runFromFolder()
}
}
} // namespace svo
} // namespace vio
int main(int argc, char** argv)
{
GPU_version/vio/test/test_pose_optimizer.cpp
+1 -1
View File
@@ -29,7 +29,7 @@
namespace {
using namespace svo;
using namespace vio;
using namespace Eigen;
/// Pose Optimizer Test-Fixture
GPU_version/vio/test/test_sparse_img_align.cpp
+10 -11
View File
@@ -14,18 +14,17 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <vikit/pinhole_camera.h>
#include <vikit/math_utils.h>
#include <vikit/file_reader.h>
#include <vikit/timer.h>
#include <vikit/blender_utils.h>
#include <vio/pinhole_camera.h>
#include <vio/math_utils.h>
#include <vio/file_reader.h>
#include <vio/timer.h>
#include <iostream>
#include <svo/feature_detection.h>
#include <svo/sparse_img_align.h>
#include <svo/frame.h>
#include <svo/point.h>
#include <svo/feature.h>
#include <svo/config.h>
#include <vio/feature_detection.h>
#include <vio/sparse_img_align.h>
#include <vio/frame.h>
#include <vio/point.h>
#include <vio/feature.h>
#include <vio/config.h>
#include "test_utils.h"
namespace {
GPU_version/vio/test/test_utils.h
+2 -2
View File
@@ -24,7 +24,7 @@
# include <vikit/params_helper.h>
#endif
namespace svo {
namespace vio {
namespace test_utils {
std::string getDatasetDir()
@@ -51,7 +51,7 @@ std::string getTraceDir()
}
} // namespace test_utils
} // namespace svo
} // namespace vio
#endif // TEST_UTILS_H_