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

add first version of kernel with out error

Browse Source
This commit is contained in:
gfuse
2021-08-11 16:29:45 +02:00
parent e20e2c76e1
commit 819cc047c6
9 changed files with 230 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/cl_class.h
+47 -48
View File
@@ -17,19 +17,17 @@ class kernel{
public: public:
kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());}; kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());};
~kernel(){ ~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(); _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){ 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
buf_size),id)); , sizeof(T) * buf_size,buf),
cl_int err=queue->enqueueWriteBuffer(_buffers.back().first.first,CL_TRUE, 0, sizeof(T) * buf_size, buf); buf_size),id));
if(err!=0){ cl_int err=_kernel->setArg(id,_buffers.back().first.first);
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){ if(err!=0){
std::cerr<<"Error C:Kernel, F:write, L3:setArg\t"<<id<<"\t"<<err<<'\n'; std::cerr<<"Error C:Kernel, F:write, L3:setArg\t"<<id<<"\t"<<err<<'\n';
return err; return err;
@@ -38,12 +36,12 @@ public:
}; };
int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){ 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), 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)),id)); reinterpret_cast<uchar*>(buf.data)),id));
cl_int err=_kernel->setArg(id,_images.back().first); cl_int err=_kernel->setArg(id,_images.back().first);
if(err!=0){ if(err!=0){
@@ -55,26 +53,18 @@ public:
template<typename T> template<typename T>
int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){ int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
try{ for(auto&& i:_buffers)if(i.second==id){
if(id>_buffers.size())return CL_MAP_FAILURE; T* Map_buf=(T*)queue->enqueueMapBuffer(i.first.first,CL_TRUE,CL_MAP_WRITE,0,sizeof(T) * i.first.second);
for(auto&& i:_buffers)if(i.second==id){ memcpy(Map_buf,buf,sizeof(T) * i.first.second);
cl_int err1=queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf); if(queue->enqueueUnmapMemObject(i.first.first,Map_buf)!=CL_SUCCESS)std::cerr<<"Reload buffer on GPU failed"<<'\n';
cl_int err2=_kernel->setArg(id,i.first.first);
if(err1!=0 || err2!=0){
std::cerr<<"Error C:Kernel, F:write, L3:setArg\t"<<id<<"\t"<<err1<<"\t"<<err2<<'\n';
return CL_MAP_FAILURE;
}
}
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
} }
return CL_SUCCESS;
}; };
int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){ int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
try{ try{
for(auto&& i:_images)if(i.second==id){ for(auto&& i:_images)if(i.second==id){
i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, 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), cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
buf.size().width, buf.size().width,
buf.size().height, buf.size().height,
@@ -85,7 +75,7 @@ public:
std::cerr<<"Error C:Kernel, F:write image, L:setArg ID:\t"<<id<<"\t"<<err<<'\n'; std::cerr<<"Error C:Kernel, F:write image, L:setArg ID:\t"<<id<<"\t"<<err<<'\n';
return err; return err;
} }
} }
return CL_SUCCESS; return CL_SUCCESS;
}catch (std::exception& err){ }catch (std::exception& err){
std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n'; std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
@@ -100,6 +90,9 @@ public:
for(auto i:_images)if(i.second==id)return i.first; for(auto i:_images)if(i.second==id)return i.first;
} }
private: 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<std::pair<cl::Buffer,size_t>,size_t>> _buffers;
std::vector<std::pair<cl::Image2D,size_t>> _images; std::vector<std::pair<cl::Image2D,size_t>> _images;
@@ -168,34 +161,40 @@ public:
} }
int32_t run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) { 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; cl_int err=0;
if(z>1 && y>1){ queue->flush();
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/); if(z>1 && y>1){
}else if(z<2 && y>1){ err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/);
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/); }else if(z<2 && y>1){
}else{ err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/);
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/); }else{
}; err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/);
queue->finish(); };
if(err!=0){ if(err!=0){
std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_FUNCTION_NAME>(); 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'; 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) 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'; 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> template<typename T>
void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, T* out){ 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); 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(); void clear_buf();
private: private:
std::vector<kernel> _kernels; std::vector<kernel> _kernels;
cl::Context* context = NULL; cl::Context* context = nullptr;
cl::Device device; cl::Device* device = nullptr;
cl::Program* program = NULL; cl::Program* program = nullptr;
cl::CommandQueue* queue=NULL; cl::CommandQueue* queue= nullptr;
}; };
#endif //VIO_OPENCL_CL_CLASS_H #endif //VIO_OPENCL_CL_CLASS_H
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -148,7 +148,7 @@ public:
J(0,0) = -z_inv; // -1/z J(0,0) = -z_inv; // -1/z
J(0,1) = x*z_inv_2; // x/z^2 J(0,1) = x*z_inv_2; // x/z^2
J(0,2) = -(1.0 + pow(x,2)*z_inv_2); // -(1.0 + x^2/z^2) J(0,2) = -(1.0 + pow(x,2)/z_inv_2); // -(1.0 + x^2/z^2)
J(1,0) = 1e-19; // 0 J(1,0) = 1e-19; // 0
J(1,1) = y*z_inv_2; // y/z^2 J(1,1) = y*z_inv_2; // y/z^2
GPU_version/vio/include/vio/nlls_solver.h
+2 -2
View File
@@ -110,7 +110,7 @@ public:
// robust least squares // robust least squares
bool use_weights_; bool use_weights_;
float scale_; double scale_=2.0;
robust_cost::ScaleEstimatorPtr scale_estimator_; robust_cost::ScaleEstimatorPtr scale_estimator_;
robust_cost::WeightFunctionPtr weight_function_; robust_cost::WeightFunctionPtr weight_function_;
@@ -131,7 +131,7 @@ public:
eps_(0.0000000001), eps_(0.0000000001),
iter_(0), iter_(0),
use_weights_(false), use_weights_(false),
scale_(0.0), scale_(2.0),
scale_estimator_(NULL), scale_estimator_(NULL),
weight_function_(NULL) weight_function_(NULL)
{ } { }
GPU_version/vio/include/vio/sparse_img_align_gpu.h
+1
View File
@@ -54,6 +54,7 @@ public:
FramePtr ref_frame, FramePtr ref_frame,
FramePtr cur_frame); FramePtr cur_frame);
// FILE* data= nullptr;
protected: protected:
int level_; //!< current pyramid level on which the optimization runs. int level_; //!< current pyramid level on which the optimization runs.
GPU_version/vio/kernel/compute-residual.cl
+85 -138
View File
@@ -4,45 +4,37 @@
// Enable OpenCL 32-bit integer atomic functions. // Enable OpenCL 32-bit integer atomic functions.
#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable #pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
double2 world2cam(double f_x, double f_y, double c_x, double c_y, double s_, double x, double y, double z) double2 world2cam(double3 feature)
{ {
double r = sqrt(pow(x/z, 2) + pow(y/z, 2)); double r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2));
double factor = 1.0; double factor = 1.0;
if(s_ != 0 || r > 0.001) if(S != 0 || r > 0.001)
factor = (atan(r * 2.0 * tan(0.5 * s_)) / (r * s_)); factor = (atan(r * 2.0 * tan(0.5 * S)) / (r * S));
return (double2)(c_x + f_x * factor * x/z, c_y + f_y * factor * y/z); return (double2)(C_X + F_X * factor * feature.x/feature.z, C_Y + F_Y * factor * feature.y/feature.z);
} }
double3 xyz_cur(double cur_x, double cur_z, double cur_pitch, double ref_x, double ref_z, double ref_pitch, double3 ref_feature) double3 xyz_cur(double3 cur, double3 ref, double3 ref_feature)
{ {
if(ref_pitch < 0.0) if(ref.z < 0.0)
ref_pitch = 3.141592653589793238462643383279502884197169399375 - ref_pitch; ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
else else
ref_pitch = ref_pitch - 3.1415926535897932384626433832795028841971693993; ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
double cc_ss = cos(cur_pitch) * cos(ref_pitch) - sin(cur_pitch) * sin(ref_pitch); double cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
double sc_cs = cos(cur_pitch) * sin(ref_pitch) + cos(ref_pitch) * sin(cur_pitch); 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_pitch) - ref_z * sin(cur_pitch), 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, ref_feature.y,
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur_z + ref_x * sin(cur_pitch) - ref_z * cos(cur_pitch)); -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
}
double3 xyz_ref(double fts_pos_x, double fts_pos_y, double fts_pos_z, double ref_x, double ref_y, double ref_z)
{
return (double3)(sqrt(pow((fts_pos_x - ref_x), 2)),
sqrt(pow((fts_pos_y - ref_y), 2)),
sqrt(pow((fts_pos_z - ref_z), 2)));
} }
void jacobian_xyz2uv(double3 xyz_in_f, double* J) void jacobian_xyz2uv(double3 xyz_in_f, double* J)
{ {
*(J + 0) = -(1. / xyz_in_f[2]); // -1/z *(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 + 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 + 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 + 3) = 1e-19; // 0
*(J + 4) = (xyz_in_f[1]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // y/z^2 *(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 *(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) void compute_hessain(double3 j, __global double* H, double w)
{ {
*(H + 0) += j[0] * j[0] * w; *(H + 0) += j[0] * j[0] * w;
@@ -56,151 +48,106 @@ void compute_hessain(double3 j, __global double* H, double w)
*(H + 8) += j[2] * j[2] * w; *(H + 8) += j[2] * j[2] * w;
} }
void compute_Jacobian(double3 j, __global double* Jacobian, double r, double w)
{
*(Jacobian + 0) -= j[0] * r * w;
*(Jacobian + 1) -= j[1] * r * w;
*(Jacobian + 2) -= j[2] * r * w;
}
__kernel void compute_residual( __kernel void compute_residual(
__read_only image2d_t image_cur, // current frame __read_only image2d_t image_cur, // current frame
__read_only image2d_t image_ref, // reference frame __read_only image2d_t image_ref, // reference frame
__global double * cur_pose,//[reference frame pose{x,z,pitch}] __global double3 * cur_pose,//[reference frame pose{x,z,pitch}]
__global double * ref_pose,//[current 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 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, __global double2 * featue_px,
__global int * level, int level,
__global double * camera_model, // camera model [f_x,f_y,c_x,c_y,s_]
__global float * errors, __global float * errors,
__global double * Hessian, __global double * Hessian,
__global double * Jacobian, __global double3 * Jacobian,
__global double * chi __global double * chi,
double scale_
) )
{ {
const int PATCH_AREA_ = PATCH_SIZE * PATCH_SIZE; double scale = pow(2.0, -level);
int border = PATCH_HALFSIZE + 1;
int stride = get_image_dim(image_cur)[0]; //width, height
double scale = pow(2.0, -level[0]);
size_t n_meas_ = 0;
// Prepare a suitable OpenCL image sampler. // Prepare a suitable OpenCL image sampler.
sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
// Use global work feature. // Use global work feature.
int f = get_global_id(0); int f = get_global_id(0);
// check if reference with patch size is within image // check if reference with patch size is within image
double u_ref = featue_px[f].x * scale; double2 uv_ref = featue_px[f] * scale;
double v_ref = featue_px[f].y * scale; double2 uv_ref_i = floor(uv_ref);
int u_ref_i = floor(u_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;
int v_ref_i = floor(v_ref);
if(u_ref_i - (PATCH_HALFSIZE + 1) < 0 || v_ref_i - (PATCH_HALFSIZE + 1) < 0 ||
u_ref_i + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[0] || v_ref_i + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[1])
return;
double3 xyz_reference = xyz_ref(ref_feature[f][0], ref_feature[f][1], ref_feature[f][2], ref_pose[0], ref_pose[1], ref_pose[2]);
// evaluate projection jacobian // evaluate projection jacobian
double frame_jac[6]; // 2X3 double frame_jac[6]={0.0}; // 2X3
jacobian_xyz2uv(xyz_reference, &frame_jac); jacobian_xyz2uv(sqrt(pow(ref_feature[f] - ref_pose[0], 2.0)), &frame_jac);
// compute bilateral interpolation weights for reference image // compute bilateral interpolation weights for reference image
double subpix_u_ref = u_ref - u_ref_i; double2 subpix= uv_ref - uv_ref_i;
double subpix_v_ref = v_ref - v_ref_i; double w_ref_tl = (1.0 - subpix.x) * (1.0 - subpix.y);
double w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); double w_ref_tr = subpix.x * (1.0 - subpix.y);
double w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); double w_ref_bl = (1.0 - subpix.x) * subpix.y;
double w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; double w_ref_br = subpix.x * subpix.y;
double w_ref_br = subpix_u_ref * subpix_v_ref;
size_t pixel_counter = 0;
double3 xyz_current = xyz_cur(cur_pose[0], cur_pose[1], cur_pose[2], ref_pose[0] ,ref_pose[1] ,ref_pose[2], ref_feature[f]);
double2 uv_cur_pyr = world2cam(camera_model[0], camera_model[1], camera_model[2], camera_model[3], camera_model[4],
xyz_current[0], xyz_current[1], xyz_current[2]) * scale;
double u_cur = uv_cur_pyr[0];
double v_cur = uv_cur_pyr[1];
int u_cur_i = floor(u_cur);
int v_cur_i = floor(v_cur);
// check if projection is within the image
if(u_cur_i < 0 || v_cur_i < 0 || u_cur_i - (PATCH_HALFSIZE + 1) < 0 || v_cur_i - (PATCH_HALFSIZE + 1) < 0 ||
u_cur_i + (PATCH_HALFSIZE + 1) >= get_image_dim(image_cur)[0] || v_cur_i + (PATCH_HALFSIZE + 1) >= get_image_dim(image_cur)[1])
return;
// compute bilateral interpolation weights for the current image // compute bilateral interpolation weights for the current image
double subpix_u_cur = u_cur - u_cur_i; double2 uv_cur_pyr = world2cam(xyz_cur(cur_pose[0], ref_pose[0] ,ref_feature[f])) * scale;
double subpix_v_cur = v_cur - v_cur_i; double2 uv_cur_i = floor(uv_cur_pyr);
double w_cur_tl = (1.0 - subpix_u_cur) * (1.0 - subpix_v_cur); double2 subpix_uv_cur = uv_cur_pyr - uv_cur_i;
double w_cur_tr = subpix_u_cur * (1.0 - subpix_v_cur); double w_cur_tl = (1.0 - subpix_uv_cur.x) * (1.0 - subpix_uv_cur.y);
double w_cur_bl = (1.0 - subpix_u_cur) * subpix_v_cur; double w_cur_tr = subpix_uv_cur.x * (1.0 - subpix_uv_cur.y);
double w_cur_br = subpix_u_cur * subpix_v_cur; 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) for(int y = 0; y < PATCH_SIZE; ++y)
{ {
int ref_element_addr = (v_ref_i + y - PATCH_HALFSIZE) * get_image_dim(image_ref)[0] + (u_ref_i - PATCH_HALFSIZE); 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 = (v_cur_i + y - PATCH_HALFSIZE) * get_image_dim(image_cur)[0] + (u_cur_i - 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, ++pixel_counter, ++ref_element_addr, ++cur_element_addr) for(int x = 0; x < PATCH_SIZE; ++x, ++ref_element_addr, ++cur_element_addr)
{ {
// precompute interpolated reference patch color // 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_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_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_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]); 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 + 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; 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 + 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_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_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_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_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_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_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)); 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 + 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_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_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_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_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_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_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)); 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 // compute residual
int2 px_curtl = ( cur_element_addr % get_image_dim(image_cur)[0], cur_element_addr / get_image_dim(image_cur)[0]); 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 = ((cur_element_addr + 1) % get_image_dim(image_cur)[0], (cur_element_addr + 1) / 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 = ((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_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 = ((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]); 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 intensity_cur = 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;
double res = intensity_cur - value; 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;
// used to compute scale for robust cost
// if(compute_weight_scale)
errors[(int)(f * pow((double)PATCH_SIZE, 2.0) + (y * PATCH_SIZE) + x)] = fabs(res);
// robustification
double weight = 2.0;
// if(use_weights_) { // use_weights_ dafault value is false
// weight = weight_function_->value(res/scale_);
// }
chi[0] += res * res * weight;
n_meas_++;
// if(linearize_system)
// {
// compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error)
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) * (camera_model[0] / scale));
compute_hessain(J, Hessian, weight); // H_.noalias() += J * J.transpose() * weight;
compute_Jacobian(J, Jacobian, res, weight); // Jres_.noalias() -= J * res * weight;
// }
} }
} }
chi[0] = chi[0] / n_meas_; errors[f] = e / pow(PATCH_SIZE,2.0);
chi[f]=chi_;
} }
GPU_version/vio/src/cl_class.cpp
+16 -14
View File
@@ -1,7 +1,7 @@
// //
// Created by root on 4/27/21. // Created by root on 4/27/21.
// //
#include "vio/cl_class.h" #include <vio/cl_class.h>
opencl::opencl() { opencl::opencl() {
std::vector<cl::Platform> all_platforms; std::vector<cl::Platform> all_platforms;
cl::Platform::get(&all_platforms); cl::Platform::get(&all_platforms);
@@ -20,31 +20,33 @@ opencl::opencl() {
std::cout << " No devices found. Check OpenCL installation!\n"; std::cout << " No devices found. Check OpenCL installation!\n";
exit(1); exit(1);
} }
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_DEVICE_OPENCL_C_VERSION: " <<device.getInfo<CL_DEVICE_OPENCL_C_VERSION>()<<'\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_BUILT_IN_KERNELS: " <<device->getInfo<CL_DEVICE_BUILT_IN_KERNELS>()<<'\n'
<< "CL_DEVICE_COMPILER_AVAILABLE: " <<device.getInfo<CL_DEVICE_COMPILER_AVAILABLE>()<<'\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_LOCAL_MEM_SIZE: " <<device->getInfo<CL_DEVICE_LOCAL_MEM_SIZE>()<<'\n'
<< "CL_DEVICE_GLOBAL_MEM_SIZE: " <<device.getInfo<CL_DEVICE_GLOBAL_MEM_SIZE>()<<'\n'; << "CL_DEVICE_GLOBAL_MEM_SIZE: " <<device->getInfo<CL_DEVICE_GLOBAL_MEM_SIZE>()<<'\n';
context=new cl::Context({ device }); context=new cl::Context({ *device });
cl::Program::Sources sources; cl::Program::Sources sources;
read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl"); read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl");
sources.push_back({ fast.src_str, fast.size }); sources.push_back({ fast.src_str, fast.size });
read_cl compute_residual(std::string(KERNEL_DIR)+"/compute-residual.cl"); read_cl compute_residual(std::string(KERNEL_DIR)+"/compute-residual.cl");
sources.push_back({ compute_residual.src_str, compute_residual.size }); sources.push_back({ compute_residual.src_str, compute_residual.size });
program=new cl::Program(*context, sources); program=new cl::Program(*context, sources);
if(program->build({ device },"-DFAST_THRESH=40 -DPATCH_SIZE=4 -DPATCH_HALFSIZE=2") !=0) if(program->build({ *device },"-DFAST_THRESH=40 -DPATCH_SIZE=2 -DPATCH_HALFSIZE=1 -DF_X=296.724480 -DF_Y=395.357280 -DC_X=317.877600 -DC_Y=238.752640 -DS=1.016870") !=0)
std::cout << " Error building: " << program->getBuildInfo<CL_PROGRAM_BUILD_STATUS>(device)<<'\n' std::cout << " Error building: " << program->getBuildInfo<CL_PROGRAM_BUILD_STATUS>(*device)<<'\n'
<<program->getBuildInfo<CL_PROGRAM_BUILD_LOG>(device) << '\n'; <<program->getBuildInfo<CL_PROGRAM_BUILD_LOG>(*device) << '\n';
queue=new cl::CommandQueue(*context,device,0,NULL); queue=new cl::CommandQueue(*context,*device,CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,NULL);
} }
opencl::~opencl() { opencl::~opencl() {
delete context;
delete device;
} }
void opencl::clear_buf() { void opencl::clear_buf() {
queue->finish(); queue->finish();
queue->flush();
_kernels.clear(); _kernels.clear();
delete program;
} }
GPU_version/vio/src/frame_handler_mono.cpp
+16 -14
View File
@@ -24,6 +24,7 @@
#include <vio/sparse_img_align.h> #include <vio/sparse_img_align.h>
#include <vio/depth_filter.h> #include <vio/depth_filter.h>
#include <vio/for_it.hpp> #include <vio/for_it.hpp>
#include <assert.h>
namespace vio { namespace vio {
@@ -47,25 +48,21 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
gpu_fast_->make_kernel("compute_residual"); gpu_fast_->make_kernel("compute_residual");
gpu_fast_->write_buf(1,0,img); gpu_fast_->write_buf(1,0,img);
gpu_fast_->write_buf(1,1,img); gpu_fast_->write_buf(1,1,img);
double pose[3]={0}; cl_double3 pose[1]={0.0};
gpu_fast_->write_buf(1,2,3,pose); gpu_fast_->write_buf(1,2,3,pose);
gpu_fast_->write_buf(1,3,3,pose); gpu_fast_->write_buf(1,3,3,pose);
cl_double3 f[300]={0}; cl_double3 f[300]={0};
gpu_fast_->write_buf(1,4,300,f); gpu_fast_->write_buf(1,4,300,f);
cl_double2 px[300]={0}; cl_double2 px[300]={0};
gpu_fast_->write_buf(1,5,300,px); gpu_fast_->write_buf(1,5,300,px);
int level[1]={0};
gpu_fast_->write_buf(1,6,1,level);
double c[5]={0};
gpu_fast_->write_buf(1,7,5,c);
double e[300]={0}; double e[300]={0};
gpu_fast_->write_buf(1,8,300,e); gpu_fast_->write_buf(1,7,300,e);
double H[9]={0}; double H[9*300]={0};
gpu_fast_->write_buf(1,9,9,H); gpu_fast_->write_buf(1,8,9*300,H);
double J[3]={0}; cl_double3 J[300];
gpu_fast_->write_buf(1,10,3,J); gpu_fast_->write_buf(1,9,300,J);
double chi2[1]={0.0}; double chi2[1]={0.01};
gpu_fast_->write_buf(1,11,1,chi2); gpu_fast_->write_buf(1,10,1,chi2);
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
initialize(); initialize();
} }
@@ -162,10 +159,15 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_=init_f.second; new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first; new_frame_->Cov_ = init_f.first;
// sparse image align // sparse image align
//SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, true); //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false);
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::GaussNewton, false,gpu_fast_); 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; if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//assert (false);
reprojector_.reprojectMap(new_frame_, overlap_kfs_); 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; size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final; double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton( pose_optimizer::optimizeGaussNewton(
GPU_version/vio/src/sparse_img_align.cpp
+1 -1
View File
@@ -37,7 +37,7 @@ SparseImgAlign::SparseImgAlign(
n_iter_init_ = n_iter_; n_iter_init_ = n_iter_;
method_ = method; method_ = method;
verbose_ = verbose; verbose_ = verbose;
eps_ = 0.0001; eps_ = 1e-5;
} }
size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
GPU_version/vio/src/sparse_img_align_gpu.cpp
+61 -38
View File
@@ -29,7 +29,8 @@ SparseImgAlignGpu::SparseImgAlignGpu(
n_iter_init_ = n_iter_; n_iter_init_ = n_iter_;
method_ = method; method_ = method;
verbose_ = verbose; verbose_ = verbose;
eps_ = 0.0001; 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) size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
@@ -39,24 +40,26 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
{ {
return 0; return 0;
} }
double* camera=ref_frame->cam_->params(); cl_double3 ref_pos[1]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()};
double ref_pos[3]={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()};
double cur_pos[3]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()};
residual_->reload_buf(1,2,cur_pos); residual_->reload_buf(1,2,cur_pos);
residual_->reload_buf(1,3,ref_pos); residual_->reload_buf(1,3,ref_pos);
residual_->reload_buf(1,7,camera);
cl_double3 features[ref_frame->fts_.size()]; cl_double3 features[ref_frame->fts_.size()];
cl_double2 featue_px[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 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=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){
if(*it == nullptr)continue; if(*it == nullptr)continue;
if((*it)->point == nullptr)continue; if((*it)->point == nullptr)continue;
Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm(); 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_].x=xyz_ref(0);
features[feature_counter_].y=xyz_ref(1); features[feature_counter_].y=xyz_ref(1);
features[feature_counter_].z=xyz_ref(2); features[feature_counter_].z=xyz_ref(2);
featue_px[feature_counter_].x=(*it)->px(0); featue_px[feature_counter_].x=(*it)->px(0);
featue_px[feature_counter_].y=(*it)->px(1); 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_; ++feature_counter_;
} }
residual_->reload_buf(1,4,features); residual_->reload_buf(1,4,features);
@@ -66,16 +69,17 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
{ {
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_);
// 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,0,cur_img);
residual_->reload_buf(1,1,ref_img); residual_->reload_buf(1,1,ref_img);
residual_->reload_buf(1,6,&level_); residual_->write_buf(1,6,level_);
mu_ = 1.0; mu_ = 1.0;
optimize(T_cur); optimize(T_cur);
} }
double pos[3]={0}; cl_double3 pos[1]={0};
residual_->read(1,2,pos); residual_->read(0,2,pos);
cur_frame->T_f_w_ = SE2_5(pos[0],pos[1],pos[2]); cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
return 1; return 1;
} }
@@ -83,43 +87,62 @@ double SparseImgAlignGpu::computeResiduals(
bool linearize_system, bool linearize_system,
bool compute_weight_scale) bool compute_weight_scale)
{ {
double chi2[1]={0}; double H[9*300]={0};
residual_->reload_buf(1,11,chi2); cl_double3 J[300]={0.0};
/// TODO GPU compute based on the feature double chi2_[300]={0.0};
residual_->run(1,feature_counter_); residual_->write_buf(1,11,scale_);
if(compute_weight_scale && iter_ == 0) { residual_->reload_buf(1,10,chi2_);
float error[feature_counter_]; residual_->reload_buf(1,8,H);
residual_->read(1,8,error); residual_->reload_buf(1,9,J);
std::vector<float> errors(error,error+feature_counter_); /// TODO GPU compute based on the feature
scale_ = scale_estimator_->compute(errors); residual_->run(1,feature_counter_);
} float error[300]={0.0};
double chi[300]={0.0};
double out[1]={0.0}; residual_->read(1,10,feature_counter_,chi);
residual_->read(1,11,out); residual_->read(1,7,feature_counter_,error);
return out[0]; 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() int SparseImgAlignGpu::solve()
{ {
double H[9]={0}; double H[300*9]={0.0};
double J[3]={0}; cl_double3 J[300]={0.0};
residual_->read(1,9,H); residual_->read(1,8,feature_counter_*9,H);
residual_->read(1,10,J); residual_->read(1,9,feature_counter_,J);
x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Matrix<double,3,1>(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])) if((bool) std::isnan((double) x_[0]))
return 0; return 0;
return 1; return 1;
} }
void SparseImgAlignGpu::update() void SparseImgAlignGpu::update()
{ {
/// TODO the update situation may have a smarter solution /// TODO the update situation may have a smarter solution
double pos[3]={0}; cl_double3 pos[1]={0.0,0.0,0.0};
residual_->read(1,2,pos); residual_->read(1,2,pos);
SE2 update = SE2(pos[2],Eigen::Vector2d(pos[0],pos[1])) * SE2::exp(-0.5*x_); Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-0.5*x_);
pos[0]=update.translation()(0); pos[0].x=update.translation()(0);
pos[1]=update.translation()(1); pos[0].y=update.translation()(1);
pos[2]=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real()); pos[0].z=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());
residual_->reload_buf(1,2,pos); residual_->reload_buf(1,2,pos);
} }
void SparseImgAlignGpu::startIteration() void SparseImgAlignGpu::startIteration()