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

debugging opencl

Browse Source
This commit is contained in:
gfuse
2021-10-04 21:14:59 +02:00
parent b39cb43382
commit 060d58536b
9 changed files with 344 additions and 2020 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-136
View File
@@ -1,136 +0,0 @@
[11:32:26.215] New key frame
[11:32:26.215] BA loop run
[11:32:26.217] init error: 8.472095
[11:32:26.222] end error: 0.002451
[11:32:26.332] New key frame
[11:32:26.332] BA loop run
[11:32:26.334] init error: 12.629072
[11:32:26.340] end error: 2.173331
[11:32:26.468] New key frame
[11:32:26.468] BA loop run
[11:32:26.470] init error: 65.010863
[11:32:26.477] end error: 1.928134
[11:32:26.617] New key frame
[11:32:26.617] BA loop run
[11:32:26.619] init error: 111.982387
[11:32:26.627] end error: 8.215693
[11:32:26.780] New key frame
[11:32:26.780] BA loop run
[11:32:26.782] init error: 133637.531428
[11:32:26.789] end error: 11.632299
[11:32:27.443] New key frame
[11:32:27.443] BA loop run
[11:32:27.447] init error: 1319.422136
[11:32:27.455] end error: 12.848739
[11:32:27.621] New key frame
[11:32:27.621] BA loop run
[11:32:27.625] init error: 182.371153
[11:32:27.641] end error: 13.471113
[11:32:27.930] New key frame
[11:32:27.930] BA loop run
[11:32:27.935] init error: 281.210624
[11:32:27.952] end error: 70.606181
[11:32:28.077] New key frame
[11:32:28.077] BA loop run
[11:32:28.082] init error: 309.023063
[11:32:28.422] end error: 23.878624
[11:32:28.430] New key frame
[11:32:28.430] BA loop run
[11:32:28.440] init error: 399.805659
[11:32:29.963] end error: 21.797671
[11:32:29.976] New key frame
[11:32:29.976] BA loop run
[11:32:29.984] init error: 10406.282878
[11:32:30.266] end error: 27.018722
[11:32:30.497] New key frame
[11:32:30.497] BA loop run
[11:32:30.505] init error: 306.484531
[11:32:30.770] end error: 29.021864
[11:32:30.843] New key frame
[11:32:30.843] BA loop run
[11:32:30.868] init error: 218.967075
[11:32:31.623] end error: 38.436167
[11:32:31.642] New key frame
[11:32:31.642] BA loop run
[11:32:31.668] init error: 242.205017
[11:32:31.915] end error: 39.613393
[11:32:32.126] New key frame
[11:32:32.126] BA loop run
[11:32:32.137] init error: 1068.056513
[11:32:32.471] end error: 32.717972
[11:32:32.628] New key frame
[11:32:32.628] BA loop run
[11:32:32.640] init error: 205.632654
[11:32:33.290] end error: 46.394407
[11:32:33.307] New key frame
[11:32:33.307] BA loop run
[11:32:33.327] init error: 257.984168
[11:32:34.235] end error: 45.118705
[11:32:34.253] New key frame
[11:32:34.253] BA loop run
[11:32:34.269] init error: 378.196667
[11:32:35.135] end error: 72.485312
[11:32:35.152] New key frame
[11:32:35.152] BA loop run
[11:32:35.166] init error: 2940.286773
[11:32:36.051] end error: 70.470916
[11:32:36.064] New key frame
[11:32:36.064] BA loop run
[11:32:36.098] init error: 175.651875
[11:32:36.564] end error: 51.774350
[11:32:36.576] New key frame
[11:32:36.576] BA loop run
[11:32:36.594] init error: 212.690168
[11:32:37.025] end error: 60.040802
[11:32:37.035] New key frame
[11:32:37.035] BA loop run
[11:32:37.065] init error: 224.779653
[11:32:37.748] end error: 50.944995
[11:32:37.762] New key frame
[11:32:37.762] BA loop run
[11:32:37.799] init error: 144.458647
[11:32:38.281] end error: 55.943794
[11:32:38.292] New key frame
[11:32:38.292] BA loop run
[11:32:38.308] init error: 417.348226
[11:32:38.776] end error: 65.937311
[11:32:38.793] New key frame
[11:32:38.793] BA loop run
[11:32:38.826] init error: 321.374078
[11:32:39.508] end error: 117.666568
[11:32:39.522] New key frame
[11:32:39.522] BA loop run
[11:32:39.560] init error: 582.482623
[11:32:40.106] end error: 56.439056
[11:32:40.120] New key frame
[11:32:40.120] BA loop run
[11:32:40.145] init error: 406.636760
[11:32:40.672] end error: 56.901814
[11:32:40.685] New key frame
[11:32:40.685] BA loop run
[11:32:40.728] init error: 855.372149
[11:32:41.341] end error: 65.671640
[11:32:41.357] New key frame
[11:32:41.357] BA loop run
[11:32:41.397] init error: 587.088866
[11:32:41.858] end error: 114.055639
[11:32:41.936] New key frame
[11:32:41.936] BA loop run
[11:32:41.963] init error: 439.136273
[11:32:42.631] end error: 63.348724
[11:32:42.646] New key frame
[11:32:42.646] BA loop run
[11:32:42.681] init error: 661.332414
[11:32:43.163] end error: 88.189274
[11:32:43.327] New key frame
[11:32:43.327] BA loop run
[11:32:43.341] init error: 526.133967
[11:32:44.023] end error: 79.628375
[11:32:44.038] New key frame
[11:32:44.038] BA loop run
[11:32:44.060] init error: 490.175656
[11:32:44.972] end error: 50.982413
[11:32:44.982] New key frame
[11:32:44.982] BA loop run
[11:32:45.005] init error: 610.616898
[11:32:45.434] end error: 57.851118
GPU_version/vio/frame_handler_log.txt
+211 -1704
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/cl_class.h
+56 -87
View File
@@ -13,6 +13,7 @@
#include <CL/opencl.h> #include <CL/opencl.h>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <vio/abstract_camera.h> #include <vio/abstract_camera.h>
#include <vio/for_it.hpp>
class kernel{ class kernel{
public: public:
@@ -22,90 +23,71 @@ public:
assert(error== CL_SUCCESS); assert(error== CL_SUCCESS);
}; };
~kernel(){ ~kernel(){
for(auto&& i:_images)i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); 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); for(auto&& i:_buffers)i.first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
_buffers.clear(); _buffers.clear();
_images.clear(); _images.clear();
}; };
template<typename T> template<typename T>
int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){ void load(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){
assert(buf); assert(buf);
cl_int error; cl_int error;
_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 _buffers.push_back(std::pair<std::shared_ptr<cl::Buffer>,size_t>(std::make_shared<cl::Buffer>(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, sizeof(T) * buf_size,buf,&error),id));
, sizeof(T) * buf_size,buf,&error),
buf_size),id));
cl_int err=_kernel->setArg(id,_buffers.back().first.first);
assert(err == CL_SUCCESS);
assert(error == CL_SUCCESS); assert(error == CL_SUCCESS);
return error; /* cl::Event event;
T* Map_buf=(T*)queue->enqueueMapBuffer(*_buffers.back().first,CL_NON_BLOCKING,CL_MAP_WRITE,0,sizeof(T) * buf_size,NULL,&event,&error);
event.wait();
memcpy(Map_buf,buf,sizeof(T) * buf_size);
assert(queue->enqueueUnmapMemObject(*_buffers.back().first,Map_buf,NULL,&event)==CL_SUCCESS);
event.wait();*/
cl_int err=_kernel->setArg(id,*_buffers.back().first);
assert(err == CL_SUCCESS);
}; };
int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){ void load(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){
assert(!buf.empty()); assert(!buf.empty());
cl_int error; cl_int error;
_images.push_back(std::pair<cl::Image2D,size_t>(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR, _images.push_back(std::pair<std::shared_ptr<cl::Image2D>,size_t>(std::make_shared<cl::Image2D>(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR,
cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
buf.size().width, buf.size().width,
buf.size().height, buf.size().height,
0, 0,
reinterpret_cast<uchar*>(buf.data),&error),id)); reinterpret_cast<uchar*>(buf.data),&error),id));
cl_int err=_kernel->setArg(id,_images.back().first); assert(error == CL_SUCCESS);
cl_int err=_kernel->setArg(id,*_images.back().first);
assert(err == CL_SUCCESS); assert(err == CL_SUCCESS);
assert(error == CL_SUCCESS);
return error;
}; };
void release(size_t id/*buffer ID*/){
template<typename T> for(auto it=_buffers.begin();it!=_buffers.end();++it){
int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){ if((*it).second==id){
assert(buf); (*it).first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
cl_int error; (*it).first.reset();
for(auto&& i:_buffers)if(i.second==id){ _buffers.erase(it);
cl::Event event; return;
T* Map_buf=(T*)queue->enqueueMapBuffer(i.first.first,CL_NON_BLOCKING,CL_MAP_WRITE,0,sizeof(T) * i.first.second,NULL,&event,&error); }
event.wait();
memcpy(Map_buf,buf,sizeof(T) * i.first.second);
assert(queue->enqueueUnmapMemObject(i.first.first,Map_buf,NULL,&event)==CL_SUCCESS);
event.wait();
} }
assert(error == CL_SUCCESS); for(auto it=_images.begin();it!=_images.end();++it){
return error; if((*it).second==id){
}; (*it).first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){ (*it).first.reset();
assert(!buf.empty()); _images.erase(it);
cl_int error; return;
try{ }
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),&error);
cl_int err=_kernel->setArg(id,i.first);
assert(err == CL_SUCCESS);
}
assert(error == CL_SUCCESS);
return error;
}catch (std::exception& err){
std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
} }
}; };
cl::Kernel* _kernel = NULL; void release(){
std::pair<cl::Buffer,size_t> read(size_t id){ _buffers.clear();
for(auto i:_buffers)if(i.second==id)return i.first; _images.clear();
} }
cl::Image2D read_img(size_t id){ cl::Kernel* _kernel = NULL;
for(auto i:_images)if(i.second==id)return i.first; std::shared_ptr<cl::Buffer> read(size_t id){
for(auto&& i:_buffers)if(i.second==id)return i.first;
} }
private: private:
static void notify(cl_mem *, void * user_data) { 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::list<std::pair<std::shared_ptr<cl::Buffer>,size_t>> _buffers;
std::vector<std::pair<cl::Image2D,size_t>> _images; std::list<std::pair<std::shared_ptr<cl::Image2D>,size_t>> _images;
}; };
class read_cl{ class read_cl{
@@ -129,35 +111,20 @@ public:
~opencl(); ~opencl();
int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));}; int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));};
template<typename T> template<typename T>
bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) { void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) {
if(_kernels.at(id1).write(id2,buf,queue,context,size)!= CL_SUCCESS)return false; _kernels.at(id1).load(id2,buf,queue,context,size);
return true;
} }
bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) { void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
if(_kernels.at(id1).write(id2,buf,context)!= CL_SUCCESS)return false; _kernels.at(id1).load(id2,buf,context);
return true;
} }
template<typename T> template<typename T>
bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){ void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
if(_kernels.at(id1)._kernel->setArg(id2,value)!= CL_SUCCESS)return false; _kernels.at(id1)._kernel->setArg(id2,value);
return true;
}; };
template<typename T>
bool reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T* buf) {
if(_kernels.at(id1).reload(id2,buf,queue)!= CL_SUCCESS)return false;
return true;
}
bool reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
if(_kernels.at(id1).reload(id2,buf,context)!= CL_SUCCESS)return false;
return true;
}
cl_int run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) { cl_int 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; cl_int err=0;
cl::Event event; cl::Event event;
if(queue->flush()==CL_OUT_OF_HOST_MEMORY){ assert(queue->flush()==CL_SUCCESS);
std::cerr<<"GPU out of memory goodbye:)\n";
exit(0);
}
if(z>1 && y>1){ if(z>1 && y>1){
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/,NULL,&event); err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/,NULL,&event);
}else if(z<2 && y>1){ }else if(z<2 && y>1){
@@ -167,10 +134,7 @@ public:
}; };
assert(err==CL_SUCCESS); assert(err==CL_SUCCESS);
event.wait(); event.wait();
if(queue->finish()==CL_OUT_OF_HOST_MEMORY){ assert(queue->finish()==CL_SUCCESS);
std::cerr<<"GPU out of memory goodbye:)\n";
exit(0);
}
return 1; return 1;
} }
@@ -179,14 +143,19 @@ public:
assert(out); assert(out);
cl_int error; cl_int error;
cl::Event event; cl::Event event;
T* Map_buf=(T*)queue->enqueueMapBuffer(_kernels.at(id1).read(id2).first,CL_NON_BLOCKING,CL_MAP_READ,0,sizeof(T) * size,NULL,&event,&error); T* Map_buf=(T*)queue->enqueueMapBuffer(*_kernels.at(id1).read(id2),CL_NON_BLOCKING,CL_MAP_READ,0,sizeof(T) * size,NULL,&event,&error);
assert(error == CL_SUCCESS); assert(error == CL_SUCCESS);
event.wait(); event.wait();
memcpy(out,Map_buf,sizeof(T) * size); memcpy(out,Map_buf,sizeof(T) * size);
if(queue->enqueueUnmapMemObject(_kernels.at(id1).read(id2).first,Map_buf,NULL,&event)!=CL_SUCCESS)std::cerr<<"Read buffer on GPU failed"<<'\n'; assert(queue->enqueueUnmapMemObject(*_kernels.at(id1).read(id2),Map_buf,NULL,&event)==CL_SUCCESS);
event.wait(); event.wait();
} }
void clear_buf(); void clear_buf(){
for(auto k:_kernels)k.release();
};
void release(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/){
_kernels.at(id1).release(id2);
}
private: private:
std::vector<kernel> _kernels; std::vector<kernel> _kernels;
cl::Context* context = nullptr; cl::Context* context = nullptr;
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch> <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 --> <!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" /> <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/param/vo_fast.yaml
+2 -2
View File
@@ -1,6 +1,6 @@
svo: svo:
grid_size: 8 #Feature grid size of a cell in [px]. grid_size: 8 #Feature grid size of a cell in [px].
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. max_n_kfs: 15 #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: 10 #Number of iterations in the local bundle adjustment. loba_num_iter: 10 #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_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad.
quality_max_drop_fts: 90 #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.
@@ -12,5 +12,5 @@ svo:
init_min_inliers: 50 init_min_inliers: 50
ACC_noise: 0.4 #acc white noise in continuous in EKF ACC_noise: 0.4 #acc white noise in continuous in EKF
GYO_noise: 0.4 #gyro white noise in continuous in EKF GYO_noise: 0.4 #gyro white noise in continuous in EKF
svo_ekf: 0.1 #Svo noise that will be devided by number of Reprojected points after opmization in EKF svo_ekf: 0.2 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.1 #control vector noise in EKF Cmd_Cov: 0.1 #control vector noise in EKF
GPU_version/vio/src/cl_class.cpp
+4 -5
View File
@@ -20,6 +20,7 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) {
std::cout << " No devices found. Check OpenCL installation!\n"; std::cout << " No devices found. Check OpenCL installation!\n";
exit(1); exit(1);
} }
device = new cl::Device(all_devices[0]); device = new cl::Device(all_devices[0]);
std::cout << "CL_DEVICE_NAME: " << device->getInfo<CL_DEVICE_NAME>() <<'\n' std::cout << "CL_DEVICE_NAME: " << device->getInfo<CL_DEVICE_NAME>() <<'\n'
<< "CL_DRIVER_VERSION: " <<device->getInfo<CL_DRIVER_VERSION>()<<'\n' << "CL_DRIVER_VERSION: " <<device->getInfo<CL_DRIVER_VERSION>()<<'\n'
@@ -51,12 +52,10 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) {
queue=new cl::CommandQueue(*context,*device,CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,NULL); queue=new cl::CommandQueue(*context,*device,CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,NULL);
} }
opencl::~opencl() { opencl::~opencl() {
clear_buf();
queue->finish();
delete program;
delete context; delete context;
delete device; delete device;
} }
void opencl::clear_buf() {
queue->finish();
_kernels.clear();
delete program;
}
GPU_version/vio/src/feature_detection.cpp
+20 -17
View File
@@ -70,51 +70,54 @@ void FastDetector::detect(
std::shared_ptr<Frame> frame, std::shared_ptr<Frame> frame,
const ImgPyr& img_pyr, const ImgPyr& img_pyr,
const double detection_threshold, const double detection_threshold,
list<shared_ptr<Feature>>& fts){ list<shared_ptr<Feature>>& fts)
{
Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,0.0,0,0.0f)); Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,0.0,0,0.0f));
std::vector<cv::KeyPoint> keypoints; std::vector<cv::KeyPoint> keypoints;
fts.clear();
for(int L=0; L<n_pyr_levels_; ++L) for(int L=0; L<n_pyr_levels_; ++L)
{ {
if(L>img_pyr.size())return; if(L>img_pyr.size())return;
int scale = (1<<L); int scale = (1<<L);
cv::Mat img=img_pyr.at(L); cv::Mat img=img_pyr.at(L);
if(!gpu_fast_->reload_buf(0,0,img)){ gpu_fast_->load(0,0,img);
ROS_ERROR("Failed to write into GPU goodbye :)"); cl_int2 fast_corner[1000]={0};
exit(0); gpu_fast_->load(0,1,1000,fast_corner);
};
cl_int icorner[1]={0}; cl_int icorner[1]={0};
if(!gpu_fast_->reload_buf(0,2,icorner)){ gpu_fast_->load(0,2,1,icorner);
ROS_ERROR("Failed to write into GPU goodbye :)");
exit(0);
};
gpu_fast_->run(0,img.cols,img.rows); gpu_fast_->run(0,img.cols,img.rows);
cl_int count[1]={0}; cl_int count[1];
gpu_fast_->read(0,2,1,count); gpu_fast_->read(0,2,1,count);
cl_int2 fast_corners[350000]={0};
gpu_fast_->read(0,1,350000,fast_corners);
if(count[0]<1 ){ if(count[0]<1 ){
ROS_ERROR("Can not communicate with GPU"); ROS_ERROR("Can not communicate with GPU");
exit(0); exit(0);
} }
int size=count[0]; int size=count[0];
cl_int2 fast_corners[size];
gpu_fast_->read(0,1,size,fast_corners);
for(uint i=0;i<size;++i) for(uint i=0;i<size;++i)
{ {
if(fast_corners[i].x<1 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){ if(fast_corners[i].x<5 || fast_corners[i].x>img_pyr[L].cols-5 || fast_corners[i].y>img_pyr[L].rows-5 || fast_corners[i].y<5){
continue; continue;
} }
float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y); float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7.f,-1,score)); keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7.f,-1,score));
} }
gpu_fast_->release(0,0);
gpu_fast_->release(0,1);
gpu_fast_->release(0,2);
} }
if(keypoints.size()<1){ if(keypoints.size()<1){
ROS_ERROR("GPU Driver crash try again!"); ROS_ERROR("GPU Driver crash try again!");
assert(0); assert(0);
} }
std::cerr<<keypoints.size()<<'\n';
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4); cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
cv::Mat descriptor; cv::Mat descriptor;
extractor->compute(frame->img(), keypoints, descriptor); extractor->compute(frame->img(), keypoints, descriptor);
for(auto&& p:_for(keypoints))fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64))); fts.clear();
for(auto&& p:_for(keypoints)){
fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64)));
}
resetGrid(); resetGrid();
} }
GPU_version/vio/src/frame_handler_mono.cpp
-33
View File
@@ -40,37 +40,7 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
{ {
gpu_fast_= new opencl(cam_); gpu_fast_= new opencl(cam_);
gpu_fast_->make_kernel("fast_gray"); gpu_fast_->make_kernel("fast_gray");
cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1);
std::vector<bool> errors;
errors.push_back(gpu_fast_->write_buf(0,0,img));
cl_int2 corners_[350000]={0};
errors.push_back(gpu_fast_->write_buf(0,1,350000,corners_));
cl_int icorner[1]={0};
errors.push_back(gpu_fast_->write_buf(0,2,1,icorner));
gpu_fast_->make_kernel("compute_residual"); gpu_fast_->make_kernel("compute_residual");
errors.push_back(gpu_fast_->write_buf(1,0,img));
errors.push_back(gpu_fast_->write_buf(1,1,img));
cl_float3 pose[1]={0.0};
errors.push_back(gpu_fast_->write_buf(1,2,3,pose));
errors.push_back(gpu_fast_->write_buf(1,3,3,pose));
cl_float3 f[300]={0};
errors.push_back(gpu_fast_->write_buf(1,4,300,f));
cl_float2 px[300]={0};
errors.push_back(gpu_fast_->write_buf(1,5,300,px));
cl_float e[300]={0};
errors.push_back(gpu_fast_->write_buf(1,7,300,e));
cl_float H[9*300]={0};
errors.push_back(gpu_fast_->write_buf(1,8,9*300,H));
cl_float3 J[300];
errors.push_back(gpu_fast_->write_buf(1,9,300,J));
cl_float chi2[300]={0.01};
errors.push_back(gpu_fast_->write_buf(1,10,300,chi2));
for(auto e:errors){
if(e!=true){
std::cerr<<"Failed to write into GPU goodbye :) "<<'\n';
exit(0);
}
}
initialize(); initialize();
#if VIO_DEBUG #if VIO_DEBUG
log_ =fopen((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(),"w+"); log_ =fopen((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(),"w+");
@@ -84,9 +54,6 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
void FrameHandlerMono::initialize() void FrameHandlerMono::initialize()
{ {
feature_detection::DetectorPtr feature_detector(
new feature_detection::FastDetector(
cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels()));
ba_glob_ = new BA_Glob(map_); ba_glob_ = new BA_Glob(map_);
ba_glob_->startThread(); ba_glob_->startThread();
} }
GPU_version/vio/src/sparse_img_align_gpu.cpp
+50 -35
View File
@@ -15,6 +15,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <vio/sparse_img_align_gpu.h> #include <vio/sparse_img_align_gpu.h>
#include <vio/for_it.hpp>
namespace vio { namespace vio {
@@ -35,49 +36,58 @@ SparseImgAlignGpu::SparseImgAlignGpu(
size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log) size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
{ {
reset(); reset();
errors.clear();
cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()}; cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()};
cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()}; cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()};
errors.push_back(residual_->reload_buf(1,2,cur_pos)); residual_->load(1,2,1,cur_pos);
errors.push_back(residual_->reload_buf(1,3,ref_pos)); residual_->load(1,3,1,ref_pos);
cl_float3 features[ref_frame->fts_.size()]; cl_float3 features[ref_frame->fts_.size()];
cl_float2 featue_px[ref_frame->fts_.size()]; cl_float2 featue_px[ref_frame->fts_.size()];
feature_counter_ = 0; // is used to compute the index of the cached jacobian feature_counter_ = 0; // is used to compute the index of the cached jacobian
for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){ for(auto it:_for(ref_frame->fts_)){
if(*it == nullptr)continue; ++feature_counter_;
if((*it)->point == nullptr)continue; if(it.item->point == nullptr){
Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0].x,0.0,ref_pos[0].y)).norm(); features[it.index].x=0.f;
features[feature_counter_].x=xyz_ref(0); features[it.index].y=0.f;
features[feature_counter_].y=xyz_ref(1); features[it.index].z=0.f;
features[feature_counter_].z=xyz_ref(2); featue_px[it.index].x=0.f;
featue_px[feature_counter_].x=(*it)->px(0); featue_px[it.index].y=0.f;
featue_px[feature_counter_].y=(*it)->px(1); continue;
if(feature_counter_>299)break; }
++feature_counter_; Vector3d xyz_ref=it.item->f*(ref_frame->se3().inverse()*it.item->point->pos_).norm();
features[it.index].x=xyz_ref(0);
features[it.index].y=xyz_ref(1);
features[it.index].z=xyz_ref(2);
featue_px[it.index].x=it.item->px(0);
featue_px[it.index].y=it.item->px(1);
} }
if(!feature_counter_) // more than 10 if(!feature_counter_) // more than 10
{ {
return 0; return 0;
} }
errors.push_back(residual_->reload_buf(1,4,features)); feature_counter_=ref_frame->fts_.size();
errors.push_back(residual_->reload_buf(1,5,featue_px)); cl_float error[feature_counter_];
cl_float chi[feature_counter_];
residual_->load(1,10,feature_counter_,chi);
residual_->load(1,7,feature_counter_,error);
residual_->load(1,4,feature_counter_,features);
residual_->load(1,5,feature_counter_,featue_px);
SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it
for(level_=max_level_; level_>=min_level_; --level_) for(level_=max_level_; level_>=min_level_; --level_)
{ {
cv::Mat cur_img = cur_frame->img_pyr_.at(level_); cv::Mat cur_img = cur_frame->img_pyr_.at(level_);
cv::Mat ref_img = ref_frame->img_pyr_.at(level_); cv::Mat ref_img = ref_frame->img_pyr_.at(level_);
errors.push_back(residual_->reload_buf(1,0,cur_img)); residual_->load(1,0,cur_img);
errors.push_back(residual_->reload_buf(1,1,ref_img)); residual_->load(1,1,ref_img);
errors.push_back(residual_->write_buf(1,6,level_)); residual_->load(1,6,level_);
mu_ = 1.0; mu_ = 1.0;
for(auto&& e:errors)if(!e)return -1;
errors.clear();
optimize(T_cur); optimize(T_cur);
residual_->release(1,0);
residual_->release(1,1);
} }
for(auto&& e:errors)if(!e)return -1;
cl_float3 pos[1]={0}; cl_float3 pos[1]={0};
residual_->read(1,2,1,pos); residual_->read(1,2,1,pos);
if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 0; if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 1;
cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z); cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
return 1; return 1;
} }
@@ -86,17 +96,17 @@ double SparseImgAlignGpu::computeResiduals(
bool linearize_system, bool linearize_system,
bool compute_weight_scale) bool compute_weight_scale)
{ {
cl_float H[9*300]={0}; cl_float H[9*feature_counter_];
cl_float3 J[300]={0.0}; cl_float3 J[feature_counter_];
cl_float chi2_[300]={0.0}; cl_float chi2_[feature_counter_];
cl_float scale=(float)scale_; cl_float scale=(float)scale_;
errors.push_back(residual_->write_buf(1,11,scale)); residual_->load(1,11,scale);
errors.push_back(residual_->reload_buf(1,10,chi2_)); residual_->load(1,10,feature_counter_,chi2_);
errors.push_back(residual_->reload_buf(1,8,H)); residual_->load(1,8,9*feature_counter_,H);
errors.push_back(residual_->reload_buf(1,9,J)); residual_->load(1,9,feature_counter_,J);
residual_->run(1,feature_counter_); residual_->run(1,feature_counter_);
cl_float error[300]={0.0}; cl_float error[feature_counter_];
cl_float chi[300]={0.0}; cl_float chi[feature_counter_];
residual_->read(1,10,feature_counter_,chi); residual_->read(1,10,feature_counter_,chi);
residual_->read(1,7,feature_counter_,error); residual_->read(1,7,feature_counter_,error);
for(int i = 1; i < feature_counter_; ++i){ for(int i = 1; i < feature_counter_; ++i){
@@ -109,8 +119,8 @@ double SparseImgAlignGpu::computeResiduals(
bool SparseImgAlignGpu::solve() bool SparseImgAlignGpu::solve()
{ {
cl_float H[300*9]={0.0}; cl_float H[feature_counter_*9];
cl_float3 J[300]={0.0}; cl_float3 J[feature_counter_];
residual_->read(1,8,feature_counter_*9,H); residual_->read(1,8,feature_counter_*9,H);
residual_->read(1,9,feature_counter_,J); residual_->read(1,9,feature_counter_,J);
for(int i = 1; i < feature_counter_; ++i){ for(int i = 1; i < feature_counter_; ++i){
@@ -133,6 +143,10 @@ bool SparseImgAlignGpu::solve()
x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d((double)J[0].x,(double)J[0].y,(double)J[0].z)); x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d((double)J[0].x,(double)J[0].y,(double)J[0].z));
double norm=x_.norm(); double norm=x_.norm();
if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1); if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1);
residual_->release(1,10);
residual_->release(1,11);
residual_->release(1,8);
residual_->release(1,9);
return true; return true;
} }
void SparseImgAlignGpu::update() void SparseImgAlignGpu::update()
@@ -143,7 +157,8 @@ void SparseImgAlignGpu::update()
pos[0].x=(float)update.translation()(0); pos[0].x=(float)update.translation()(0);
pos[0].y=(float)update.translation()(1); pos[0].y=(float)update.translation()(1);
pos[0].z=(float)atan2(update.so2().unit_complex().imag(),update.so2().unit_complex().real()); pos[0].z=(float)atan2(update.so2().unit_complex().imag(),update.so2().unit_complex().real());
errors.push_back(residual_->reload_buf(1,2,pos)); residual_->release(1,2);
residual_->load(1,2,1,pos);
} }
void SparseImgAlignGpu::startIteration() void SparseImgAlignGpu::startIteration()