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

change the open_cl

Browse Source
This commit is contained in:
gfuse
2021-10-04 19:01:54 +02:00
parent b39cb43382
commit 354e112898
9 changed files with 410 additions and 2019 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
+273 -1704
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/cl_class.h
+53 -86
View File
@@ -13,6 +13,7 @@
#include <CL/opencl.h>
#include <opencv2/opencv.hpp>
#include <vio/abstract_camera.h>
#include <vio/for_it.hpp>
class kernel{
public:
@@ -22,90 +23,69 @@ public:
assert(error== CL_SUCCESS);
};
~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);
for(auto&& i:_images)i.first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
for(auto&& i:_buffers)i.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){
void load(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){
assert(buf);
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
, sizeof(T) * buf_size,buf,&error),
buf_size),id));
cl_int err=_kernel->setArg(id,_buffers.back().first.first);
assert(err == CL_SUCCESS);
_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));
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());
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),
buf.size().width,
buf.size().height,
0,
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(error == CL_SUCCESS);
return error;
};
template<typename T>
int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
assert(buf);
cl_int error;
for(auto&& i:_buffers)if(i.second==id){
cl::Event event;
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();
void release(size_t id/*buffer ID*/){
for(auto it=_buffers.begin();it!=_buffers.end();++it){
if((*it).second==id){
(*it).first.reset();
_buffers.erase(it);
return;
}
assert(error == CL_SUCCESS);
return error;
};
int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
assert(!buf.empty());
cl_int error;
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;
for(auto it=_images.begin();it!=_images.end();++it){
if((*it).second==id){
(*it).first.reset();
_images.erase(it);
return;
}
}
};
void release(){
_buffers.clear();
_images.clear();
}
cl::Kernel* _kernel = NULL;
std::pair<cl::Buffer,size_t> read(size_t id){
for(auto i:_buffers)if(i.second==id)return i.first;
}
cl::Image2D read_img(size_t id){
for(auto i:_images)if(i.second==id)return i.first;
std::shared_ptr<cl::Buffer> read(size_t id){
for(auto&& i:_buffers)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;
std::list<std::pair<std::shared_ptr<cl::Buffer>,size_t>> _buffers;
std::list<std::pair<std::shared_ptr<cl::Image2D>,size_t>> _images;
};
class read_cl{
@@ -129,35 +109,20 @@ public:
~opencl();
int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));};
template<typename T>
bool write_buf(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;
return true;
void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) {
_kernels.at(id1).load(id2,buf,queue,context,size);
}
bool write_buf(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;
return true;
void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
_kernels.at(id1).load(id2,buf,context);
}
template<typename T>
bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
if(_kernels.at(id1)._kernel->setArg(id2,value)!= CL_SUCCESS)return false;
return true;
void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
_kernels.at(id1)._kernel->setArg(id2,value);
};
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 err=0;
cl::Event event;
if(queue->flush()==CL_OUT_OF_HOST_MEMORY){
std::cerr<<"GPU out of memory goodbye:)\n";
exit(0);
}
assert(queue->flush()==CL_SUCCESS);
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);
}else if(z<2 && y>1){
@@ -167,10 +132,7 @@ public:
};
assert(err==CL_SUCCESS);
event.wait();
if(queue->finish()==CL_OUT_OF_HOST_MEMORY){
std::cerr<<"GPU out of memory goodbye:)\n";
exit(0);
}
assert(queue->finish()==CL_SUCCESS);
return 1;
}
@@ -179,14 +141,19 @@ public:
assert(out);
cl_int error;
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);
event.wait();
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();
}
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:
std::vector<kernel> _kernels;
cl::Context* context = nullptr;
GPU_version/vio/param/vo_fast.yaml
+2 -2
View File
@@ -1,6 +1,6 @@
svo:
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.
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.
@@ -12,5 +12,5 @@ svo:
init_min_inliers: 50
ACC_noise: 0.4 #acc 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
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";
exit(1);
}
device = new cl::Device(all_devices[0]);
std::cout << "CL_DEVICE_NAME: " << device->getInfo<CL_DEVICE_NAME>() <<'\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);
}
opencl::~opencl() {
clear_buf();
queue->finish();
delete program;
delete context;
delete device;
}
void opencl::clear_buf() {
queue->finish();
_kernels.clear();
delete program;
}
GPU_version/vio/src/feature_detection.cpp
+25 -15
View File
@@ -70,51 +70,61 @@ void FastDetector::detect(
std::shared_ptr<Frame> frame,
const ImgPyr& img_pyr,
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));
std::vector<cv::KeyPoint> keypoints;
fts.clear();
for(int L=0; L<n_pyr_levels_; ++L)
{
if(L>img_pyr.size())return;
int scale = (1<<L);
cv::Mat img=img_pyr.at(L);
if(!gpu_fast_->reload_buf(0,0,img)){
ROS_ERROR("Failed to write into GPU goodbye :)");
exit(0);
};
cl_int icorner[1]={0};
if(!gpu_fast_->reload_buf(0,2,icorner)){
ROS_ERROR("Failed to write into GPU goodbye :)");
exit(0);
};
gpu_fast_->load(0,0,img);
cl_int2 fast_corner[1000];
gpu_fast_->load(0,1,1000,fast_corner);
cl_int icorner[1];
gpu_fast_->load(0,2,1,icorner);
gpu_fast_->run(0,img.cols,img.rows);
cl_int count[1]={0};
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 ){
ROS_ERROR("Can not communicate with GPU");
exit(0);
}
int size=count[0];
cl_int2 fast_corners[size];
std::cerr<<size<<"\n";
gpu_fast_->read(0,1,size,fast_corners);
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 || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<5){
continue;
}
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));
}
gpu_fast_->release(0,0);
gpu_fast_->release(0,1);
gpu_fast_->release(0,2);
}
if(keypoints.size()<1){
ROS_ERROR("GPU Driver crash try again!");
assert(0);
}
std::cerr<<keypoints.size()<<'\n';
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
cv::Mat 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();
cv::Mat debug1,debug;
debug1=frame->img().clone();
cv::cvtColor(debug1,debug,cv::COLOR_GRAY2RGB);
for(auto&& p:_for(keypoints)){
cv::circle(debug, cv::Point(p.item.pt.x, p.item.pt.y ), 2, cv::Scalar(0, 255,0),2);
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)));
}
cv::imwrite(std::string(PROJECT_DIR)+"/test.png",debug);
//cv::waitKey();
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_->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");
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();
#if VIO_DEBUG
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()
{
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_->startThread();
}
GPU_version/vio/src/sparse_img_align_gpu.cpp
+49 -34
View File
@@ -15,6 +15,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <vio/sparse_img_align_gpu.h>
#include <vio/for_it.hpp>
namespace vio {
@@ -35,49 +36,58 @@ SparseImgAlignGpu::SparseImgAlignGpu(
size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
{
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 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));
errors.push_back(residual_->reload_buf(1,3,ref_pos));
residual_->load(1,2,1,cur_pos);
residual_->load(1,3,1,ref_pos);
cl_float3 features[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
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);
if(feature_counter_>299)break;
for(auto it:_for(ref_frame->fts_)){
++feature_counter_;
if(it.item->point == nullptr){
features[it.index].x=0.f;
features[it.index].y=0.f;
features[it.index].z=0.f;
featue_px[it.index].x=0.f;
featue_px[it.index].y=0.f;
continue;
}
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
{
return 0;
}
errors.push_back(residual_->reload_buf(1,4,features));
errors.push_back(residual_->reload_buf(1,5,featue_px));
feature_counter_=ref_frame->fts_.size();
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
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_);
errors.push_back(residual_->reload_buf(1,0,cur_img));
errors.push_back(residual_->reload_buf(1,1,ref_img));
errors.push_back(residual_->write_buf(1,6,level_));
residual_->load(1,0,cur_img);
residual_->load(1,1,ref_img);
residual_->load(1,6,level_);
mu_ = 1.0;
for(auto&& e:errors)if(!e)return -1;
errors.clear();
optimize(T_cur);
residual_->release(1,0);
residual_->release(1,1);
}
for(auto&& e:errors)if(!e)return -1;
cl_float3 pos[1]={0};
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);
return 1;
}
@@ -86,17 +96,17 @@ double SparseImgAlignGpu::computeResiduals(
bool linearize_system,
bool compute_weight_scale)
{
cl_float H[9*300]={0};
cl_float3 J[300]={0.0};
cl_float chi2_[300]={0.0};
cl_float H[9*feature_counter_];
cl_float3 J[feature_counter_];
cl_float chi2_[feature_counter_];
cl_float scale=(float)scale_;
errors.push_back(residual_->write_buf(1,11,scale));
errors.push_back(residual_->reload_buf(1,10,chi2_));
errors.push_back(residual_->reload_buf(1,8,H));
errors.push_back(residual_->reload_buf(1,9,J));
residual_->load(1,11,scale);
residual_->load(1,10,feature_counter_,chi2_);
residual_->load(1,8,9*feature_counter_,H);
residual_->load(1,9,feature_counter_,J);
residual_->run(1,feature_counter_);
cl_float error[300]={0.0};
cl_float chi[300]={0.0};
cl_float error[feature_counter_];
cl_float chi[feature_counter_];
residual_->read(1,10,feature_counter_,chi);
residual_->read(1,7,feature_counter_,error);
for(int i = 1; i < feature_counter_; ++i){
@@ -109,8 +119,8 @@ double SparseImgAlignGpu::computeResiduals(
bool SparseImgAlignGpu::solve()
{
cl_float H[300*9]={0.0};
cl_float3 J[300]={0.0};
cl_float H[feature_counter_*9];
cl_float3 J[feature_counter_];
residual_->read(1,8,feature_counter_*9,H);
residual_->read(1,9,feature_counter_,J);
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));
double norm=x_.norm();
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;
}
void SparseImgAlignGpu::update()
@@ -143,7 +157,8 @@ void SparseImgAlignGpu::update()
pos[0].x=(float)update.translation()(0);
pos[0].y=(float)update.translation()(1);
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()
GPU_version/vio/test.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 170 KiB