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
+26 -27
View File
@@ -17,19 +17,17 @@ class kernel{
public:
kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());};
~kernel(){
for(auto&& i:_images)i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
for(auto&& i:_buffers)i.first.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
_buffers.clear();
_images.clear();
};
template<typename T>
int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){
_buffers.push_back(std::pair<std::pair<cl::Buffer,size_t>,size_t>(std::pair<cl::Buffer,size_t>(cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(T) * buf_size),
_buffers.push_back(std::pair<std::pair<cl::Buffer,size_t>,size_t>(std::pair<cl::Buffer,size_t>(cl::Buffer(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR
, sizeof(T) * buf_size,buf),
buf_size),id));
cl_int err=queue->enqueueWriteBuffer(_buffers.back().first.first,CL_TRUE, 0, sizeof(T) * buf_size, buf);
if(err!=0){
std::cerr<<"Error C:Kernel, F:write, L:enqueueWriteBuffer\n"<<err<<'\n';
return err;
}
err=_kernel->setArg(id,_buffers.back().first.first);
cl_int err=_kernel->setArg(id,_buffers.back().first.first);
if(err!=0){
std::cerr<<"Error C:Kernel, F:write, L3:setArg\t"<<id<<"\t"<<err<<'\n';
return err;
@@ -38,7 +36,7 @@ public:
};
int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){
_images.push_back(std::pair<cl::Image2D,size_t>(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
_images.push_back(std::pair<cl::Image2D,size_t>(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR,
cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
buf.size().width,
buf.size().height,
@@ -55,26 +53,18 @@ public:
template<typename T>
int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
try{
if(id>_buffers.size())return CL_MAP_FAILURE;
for(auto&& i:_buffers)if(i.second==id){
cl_int err1=queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf);
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;
}
T* Map_buf=(T*)queue->enqueueMapBuffer(i.first.first,CL_TRUE,CL_MAP_WRITE,0,sizeof(T) * i.first.second);
memcpy(Map_buf,buf,sizeof(T) * i.first.second);
if(queue->enqueueUnmapMemObject(i.first.first,Map_buf)!=CL_SUCCESS)std::cerr<<"Reload buffer on GPU failed"<<'\n';
}
return CL_SUCCESS;
}catch (std::exception& err){
std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
return CL_MAP_FAILURE;
}
};
int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
try{
for(auto&& i:_images)if(i.second==id){
i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
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,
@@ -100,6 +90,9 @@ public:
for(auto i:_images)if(i.second==id)return i.first;
}
private:
static void notify(cl_mem *, void * user_data) {
//std::cerr << "Memory object was deleted." << std::endl;
}
std::vector<std::pair<std::pair<cl::Buffer,size_t>,size_t>> _buffers;
std::vector<std::pair<cl::Image2D,size_t>> _images;
@@ -168,6 +161,7 @@ public:
}
int32_t run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) {
cl_int err=0;
queue->flush();
if(z>1 && y>1){
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/);
}else if(z<2 && y>1){
@@ -175,27 +169,32 @@ public:
}else{
err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/);
};
queue->finish();
if(err!=0){
std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_FUNCTION_NAME>();
std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_NUM_ARGS>()<<" ,Run out: "<<err<<'\n';
for(int i=0;i<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_NUM_ARGS>();++i)
std::cerr<<"argument type "<<i<<":"<<_kernels.at(id1)._kernel->getArgInfo<CL_KERNEL_ARG_TYPE_NAME>(i)<<'\n';
}
return err;
return queue->finish();
}
template<typename T>
void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, T* out){
queue->enqueueReadBuffer(_kernels.at(id1).read(id2).first, CL_TRUE, 0, sizeof(T) * _kernels.at(id1).read(id2).second, out);
}
template<typename T>
void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,size_t size/*size*/, T* out){
T* Map_buf=(T*)queue->enqueueMapBuffer(_kernels.at(id1).read(id2).first,CL_TRUE,CL_MAP_READ,0,sizeof(T) * size);
memcpy(out,Map_buf,sizeof(T) * size);
if(queue->enqueueUnmapMemObject(_kernels.at(id1).read(id2).first,Map_buf)!=CL_SUCCESS)std::cerr<<"Read buffer on GPU failed"<<'\n';
}
void clear_buf();
private:
std::vector<kernel> _kernels;
cl::Context* context = NULL;
cl::Device device;
cl::Program* program = NULL;
cl::CommandQueue* queue=NULL;
cl::Context* context = nullptr;
cl::Device* device = nullptr;
cl::Program* program = nullptr;
cl::CommandQueue* queue= nullptr;
};
#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,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,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
bool use_weights_;
float scale_;
double scale_=2.0;
robust_cost::ScaleEstimatorPtr scale_estimator_;
robust_cost::WeightFunctionPtr weight_function_;
@@ -131,7 +131,7 @@ public:
eps_(0.0000000001),
iter_(0),
use_weights_(false),
scale_(0.0),
scale_(2.0),
scale_estimator_(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 cur_frame);
// FILE* data= nullptr;
protected:
int level_; //!< current pyramid level on which the optimization runs.
GPU_version/vio/kernel/compute-residual.cl
+58 -111
View File
@@ -4,45 +4,37 @@
// Enable OpenCL 32-bit integer atomic functions.
#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;
if(s_ != 0 || r > 0.001)
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);
if(S != 0 || r > 0.001)
factor = (atan(r * 2.0 * tan(0.5 * S)) / (r * S));
return (double2)(C_X + F_X * factor * feature.x/feature.z, C_Y + F_Y * factor * feature.y/feature.z);
}
double3 xyz_cur(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)
ref_pitch = 3.141592653589793238462643383279502884197169399375 - ref_pitch;
if(ref.z < 0.0)
ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
else
ref_pitch = ref_pitch - 3.1415926535897932384626433832795028841971693993;
double cc_ss = cos(cur_pitch) * cos(ref_pitch) - sin(cur_pitch) * sin(ref_pitch);
double sc_cs = cos(cur_pitch) * sin(ref_pitch) + cos(ref_pitch) * sin(cur_pitch);
return (double3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur_x - ref_x * cos(cur_pitch) - ref_z * sin(cur_pitch),
ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
double cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
double sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
return (double3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
ref_feature.y,
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur_z + ref_x * sin(cur_pitch) - ref_z * cos(cur_pitch));
}
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)));
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
}
void jacobian_xyz2uv(double3 xyz_in_f, double* J)
{
*(J + 0) = -(1. / xyz_in_f[2]); // -1/z
*(J + 1) = (xyz_in_f[0]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // x/z^2
*(J + 2) = -(1.0 + pow((xyz_in_f[0]),2) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2]))); // -(1.0 + x^2/z^2)
*(J + 2) = -(1.0 + pow((xyz_in_f[0]),2) / ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2]))); // -(1.0 + x^2/z^2)
*(J + 3) = 1e-19; // 0
*(J + 4) = (xyz_in_f[1]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // y/z^2
*(J + 5) = -(xyz_in_f[0]) * (xyz_in_f[1]) / ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // -x*y/z^2
}
void compute_hessain(double3 j, __global double* H, double w)
{
*(H + 0) += j[0] * j[0] * w;
@@ -56,90 +48,57 @@ void compute_hessain(double3 j, __global double* H, double 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(
__read_only image2d_t image_cur, // current frame
__read_only image2d_t image_ref, // reference frame
__global double * cur_pose,//[reference frame pose{x,z,pitch}]
__global double * ref_pose,//[current frame pose{x,z,pitch}]
__global double3 * cur_pose,//[reference frame pose{x,z,pitch}]
__global double3 * ref_pose,//[current frame pose{x,z,pitch}]
__global double3 * ref_feature, // feature on the reference frame, when we applied the distance calculation: xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm());
__global double2 * featue_px,
__global int * level,
__global double * camera_model, // camera model [f_x,f_y,c_x,c_y,s_]
int level,
__global float * errors,
__global double * Hessian,
__global double * Jacobian,
__global double * chi
__global double3 * Jacobian,
__global double * chi,
double scale_
)
{
const int PATCH_AREA_ = PATCH_SIZE * PATCH_SIZE;
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;
double scale = pow(2.0, -level);
// Prepare a suitable OpenCL image sampler.
sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
// Use global work feature.
int f = get_global_id(0);
// check if reference with patch size is within image
double u_ref = featue_px[f].x * scale;
double v_ref = featue_px[f].y * scale;
int u_ref_i = floor(u_ref);
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]);
double2 uv_ref = featue_px[f] * scale;
double2 uv_ref_i = floor(uv_ref);
if(uv_ref_i.x - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.y - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.x + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[0] || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[1])return;
// evaluate projection jacobian
double frame_jac[6]; // 2X3
jacobian_xyz2uv(xyz_reference, &frame_jac);
double frame_jac[6]={0.0}; // 2X3
jacobian_xyz2uv(sqrt(pow(ref_feature[f] - ref_pose[0], 2.0)), &frame_jac);
// compute bilateral interpolation weights for reference image
double subpix_u_ref = u_ref - u_ref_i;
double subpix_v_ref = v_ref - v_ref_i;
double w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref);
double w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref);
double w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref;
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;
double2 subpix= uv_ref - uv_ref_i;
double w_ref_tl = (1.0 - subpix.x) * (1.0 - subpix.y);
double w_ref_tr = subpix.x * (1.0 - subpix.y);
double w_ref_bl = (1.0 - subpix.x) * subpix.y;
double w_ref_br = subpix.x * subpix.y;
// compute bilateral interpolation weights for the current image
double subpix_u_cur = u_cur - u_cur_i;
double subpix_v_cur = v_cur - v_cur_i;
double w_cur_tl = (1.0 - subpix_u_cur) * (1.0 - subpix_v_cur);
double w_cur_tr = subpix_u_cur * (1.0 - subpix_v_cur);
double w_cur_bl = (1.0 - subpix_u_cur) * subpix_v_cur;
double w_cur_br = subpix_u_cur * subpix_v_cur;
double2 uv_cur_pyr = world2cam(xyz_cur(cur_pose[0], ref_pose[0] ,ref_feature[f])) * scale;
double2 uv_cur_i = floor(uv_cur_pyr);
double2 subpix_uv_cur = uv_cur_pyr - uv_cur_i;
double w_cur_tl = (1.0 - subpix_uv_cur.x) * (1.0 - subpix_uv_cur.y);
double w_cur_tr = subpix_uv_cur.x * (1.0 - subpix_uv_cur.y);
double w_cur_bl = (1.0 - subpix_uv_cur.x) * subpix_uv_cur.y;
double w_cur_br = subpix_uv_cur.x * subpix_uv_cur.y;
float e = 0.0;
double chi_=0.0;
for(int y = 0; y < PATCH_SIZE; ++y)
{
int ref_element_addr = (v_ref_i + y - PATCH_HALFSIZE) * get_image_dim(image_ref)[0] + (u_ref_i - PATCH_HALFSIZE);
int cur_element_addr = (v_cur_i + y - PATCH_HALFSIZE) * get_image_dim(image_cur)[0] + (u_cur_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 = (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
int2 px_reftl = (int2)( ref_element_addr % get_image_dim(image_ref)[0], ref_element_addr / get_image_dim(image_ref)[0]);
@@ -168,39 +127,27 @@ __kernel void compute_residual(
w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x));
// compute residual
int2 px_curtl = ( 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_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_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]);
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 +
int2 px_curtl = (int2)( cur_element_addr % get_image_dim(image_cur)[0], cur_element_addr / get_image_dim(image_cur)[0]);
int2 px_curtr = (int2)((cur_element_addr + 1) % get_image_dim(image_cur)[0], (cur_element_addr + 1) / get_image_dim(image_cur)[0]);
int2 px_curbl = (int2)((cur_element_addr + (get_image_dim(image_cur)[0])) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0])) / get_image_dim(image_cur)[0]);
int2 px_curbr = (int2)((cur_element_addr + (get_image_dim(image_cur)[0]) + 1) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0]) + 1) / get_image_dim(image_cur)[0]);
double res = value - w_cur_tl * read_imageui(image_cur, sampler, px_curtl).x + w_cur_tr * read_imageui(image_cur, sampler, px_curtr).x +
w_cur_bl * read_imageui(image_cur, sampler, px_curbl).x + w_cur_br * read_imageui(image_cur, sampler, px_curbr).x;
double res = intensity_cur - value;
// 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);
e += 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)
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) * (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;
// }
double3 J = (j_row0 + j_row1) * (F_X / scale);
compute_hessain(J, &Hessian[f*9], weight);
Jacobian[f] -= 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.
//
#include "vio/cl_class.h"
#include <vio/cl_class.h>
opencl::opencl() {
std::vector<cl::Platform> all_platforms;
cl::Platform::get(&all_platforms);
@@ -20,31 +20,33 @@ opencl::opencl() {
std::cout << " No devices found. Check OpenCL installation!\n";
exit(1);
}
device = all_devices[0];
std::cout << "CL_DEVICE_NAME: " << device.getInfo<CL_DEVICE_NAME>() <<'\n'
<< "CL_DEVICE_OPENCL_C_VERSION: " <<device.getInfo<CL_DEVICE_OPENCL_C_VERSION>()<<'\n'
<< "CL_DEVICE_BUILT_IN_KERNELS: " <<device.getInfo<CL_DEVICE_BUILT_IN_KERNELS>()<<'\n'
<< "CL_DEVICE_COMPILER_AVAILABLE: " <<device.getInfo<CL_DEVICE_COMPILER_AVAILABLE>()<<'\n'
<< "CL_DEVICE_LOCAL_MEM_SIZE: " <<device.getInfo<CL_DEVICE_LOCAL_MEM_SIZE>()<<'\n'
<< "CL_DEVICE_GLOBAL_MEM_SIZE: " <<device.getInfo<CL_DEVICE_GLOBAL_MEM_SIZE>()<<'\n';
device = new cl::Device(all_devices[0]);
std::cout << "CL_DEVICE_NAME: " << device->getInfo<CL_DEVICE_NAME>() <<'\n'
<< "CL_DEVICE_OPENCL_C_VERSION: " <<device->getInfo<CL_DEVICE_OPENCL_C_VERSION>()<<'\n'
<< "CL_DEVICE_BUILT_IN_KERNELS: " <<device->getInfo<CL_DEVICE_BUILT_IN_KERNELS>()<<'\n'
<< "CL_DEVICE_COMPILER_AVAILABLE: " <<device->getInfo<CL_DEVICE_COMPILER_AVAILABLE>()<<'\n'
<< "CL_DEVICE_LOCAL_MEM_SIZE: " <<device->getInfo<CL_DEVICE_LOCAL_MEM_SIZE>()<<'\n'
<< "CL_DEVICE_GLOBAL_MEM_SIZE: " <<device->getInfo<CL_DEVICE_GLOBAL_MEM_SIZE>()<<'\n';
context=new cl::Context({ device });
context=new cl::Context({ *device });
cl::Program::Sources sources;
read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl");
sources.push_back({ fast.src_str, fast.size });
read_cl compute_residual(std::string(KERNEL_DIR)+"/compute-residual.cl");
sources.push_back({ compute_residual.src_str, compute_residual.size });
program=new cl::Program(*context, sources);
if(program->build({ device },"-DFAST_THRESH=40 -DPATCH_SIZE=4 -DPATCH_HALFSIZE=2") !=0)
std::cout << " Error building: " << program->getBuildInfo<CL_PROGRAM_BUILD_STATUS>(device)<<'\n'
<<program->getBuildInfo<CL_PROGRAM_BUILD_LOG>(device) << '\n';
queue=new cl::CommandQueue(*context,device,0,NULL);
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'
<<program->getBuildInfo<CL_PROGRAM_BUILD_LOG>(*device) << '\n';
queue=new cl::CommandQueue(*context,*device,CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,NULL);
}
opencl::~opencl() {
delete context;
delete device;
}
void opencl::clear_buf() {
queue->finish();
queue->flush();
_kernels.clear();
delete program;
}
GPU_version/vio/src/frame_handler_mono.cpp
+16 -14
View File
@@ -24,6 +24,7 @@
#include <vio/sparse_img_align.h>
#include <vio/depth_filter.h>
#include <vio/for_it.hpp>
#include <assert.h>
namespace vio {
@@ -47,25 +48,21 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
gpu_fast_->make_kernel("compute_residual");
gpu_fast_->write_buf(1,0,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,3,3,pose);
cl_double3 f[300]={0};
gpu_fast_->write_buf(1,4,300,f);
cl_double2 px[300]={0};
gpu_fast_->write_buf(1,5,300,px);
int level[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};
gpu_fast_->write_buf(1,8,300,e);
double H[9]={0};
gpu_fast_->write_buf(1,9,9,H);
double J[3]={0};
gpu_fast_->write_buf(1,10,3,J);
double chi2[1]={0.0};
gpu_fast_->write_buf(1,11,1,chi2);
gpu_fast_->write_buf(1,7,300,e);
double H[9*300]={0};
gpu_fast_->write_buf(1,8,9*300,H);
cl_double3 J[300];
gpu_fast_->write_buf(1,9,300,J);
double chi2[1]={0.01};
gpu_fast_->write_buf(1,10,1,chi2);
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
initialize();
}
@@ -162,10 +159,15 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
// sparse image align
//SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, true);
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::GaussNewton, false,gpu_fast_);
//SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false);
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//assert (false);
reprojector_.reprojectMap(new_frame_, overlap_kfs_);
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
GPU_version/vio/src/sparse_img_align.cpp
+1 -1
View File
@@ -37,7 +37,7 @@ SparseImgAlign::SparseImgAlign(
n_iter_init_ = n_iter_;
method_ = method;
verbose_ = verbose;
eps_ = 0.0001;
eps_ = 1e-5;
}
size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
GPU_version/vio/src/sparse_img_align_gpu.cpp
+55 -32
View File
@@ -29,7 +29,8 @@ SparseImgAlignGpu::SparseImgAlignGpu(
n_iter_init_ = n_iter_;
method_ = method;
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)
@@ -39,24 +40,26 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
{
return 0;
}
double* camera=ref_frame->cam_->params();
double ref_pos[3]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()};
double cur_pos[3]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()};
cl_double3 ref_pos[1]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()};
cl_double3 cur_pos[1]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()};
residual_->reload_buf(1,2,cur_pos);
residual_->reload_buf(1,3,ref_pos);
residual_->reload_buf(1,7,camera);
cl_double3 features[ref_frame->fts_.size()];
cl_double2 featue_px[ref_frame->fts_.size()];
// fprintf(data, "ref_frame:\n %f,%f,%f\n",ref_pos[0],ref_pos[1],ref_pos[2]);
// fprintf(data, "cur_frame:\n %f,%f,%f\n",cur_pos[0],cur_pos[1],cur_pos[2]);
// fprintf(data, "camera:\n %f,%f,%f,%f,%f\n features:\n",camera[0],camera[1],camera[2],camera[3],camera[4]);
feature_counter_ = 0; // is used to compute the index of the cached jacobian
for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){
if(*it == nullptr)continue;
if((*it)->point == nullptr)continue;
Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],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_].y=xyz_ref(1);
features[feature_counter_].z=xyz_ref(2);
featue_px[feature_counter_].x=(*it)->px(0);
featue_px[feature_counter_].y=(*it)->px(1);
// fprintf(data, "%f,%f,%f,%f,%f\n",xyz_ref(0),xyz_ref(1),xyz_ref(2),(*it)->px(0),(*it)->px(1));
++feature_counter_;
}
residual_->reload_buf(1,4,features);
@@ -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 ref_img = ref_frame->img_pyr_.at(level_);
// cv::imwrite(std::string(getenv("HOME"))+"/Project/cur"+to_string(level_)+".png",cur_img);
// cv::imwrite(std::string(getenv("HOME"))+"/Project/fer"+to_string(level_)+".png",ref_img);
residual_->reload_buf(1,0,cur_img);
residual_->reload_buf(1,1,ref_img);
residual_->reload_buf(1,6,&level_);
residual_->write_buf(1,6,level_);
mu_ = 1.0;
optimize(T_cur);
}
double pos[3]={0};
residual_->read(1,2,pos);
cur_frame->T_f_w_ = SE2_5(pos[0],pos[1],pos[2]);
cl_double3 pos[1]={0};
residual_->read(0,2,pos);
cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
return 1;
}
@@ -83,29 +87,48 @@ double SparseImgAlignGpu::computeResiduals(
bool linearize_system,
bool compute_weight_scale)
{
double chi2[1]={0};
residual_->reload_buf(1,11,chi2);
double H[9*300]={0};
cl_double3 J[300]={0.0};
double chi2_[300]={0.0};
residual_->write_buf(1,11,scale_);
residual_->reload_buf(1,10,chi2_);
residual_->reload_buf(1,8,H);
residual_->reload_buf(1,9,J);
/// TODO GPU compute based on the feature
residual_->run(1,feature_counter_);
if(compute_weight_scale && iter_ == 0) {
float error[feature_counter_];
residual_->read(1,8,error);
std::vector<float> errors(error,error+feature_counter_);
scale_ = scale_estimator_->compute(errors);
float error[300]={0.0};
double chi[300]={0.0};
residual_->read(1,10,feature_counter_,chi);
residual_->read(1,7,feature_counter_,error);
for(int i = 1; i < feature_counter_; ++i){
error[0] += error[i];
chi[0] += chi[i];
}
double out[1]={0.0};
residual_->read(1,11,out);
return out[0];
scale_ = error[0]*(1.48f / feature_counter_);
return chi[0]/(feature_counter_*8);
}
int SparseImgAlignGpu::solve()
{
double H[9]={0};
double J[3]={0};
residual_->read(1,9,H);
residual_->read(1,10,J);
x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Matrix<double,3,1>(J));
double H[300*9]={0.0};
cl_double3 J[300]={0.0};
residual_->read(1,8,feature_counter_*9,H);
residual_->read(1,9,feature_counter_,J);
for(int i = 1; i < feature_counter_; ++i){
H[0] += H[i*9];
H[1] += H[i*9+1];
H[2] += H[i*9+2];
H[3] += H[i*9+3];
H[4] += H[i*9+4];
H[5] += H[i*9+5];
H[6] += H[i*9+6];
H[7] += H[i*9+7];
H[8] += H[i*9+8];
J[0].x -= J[i].x;
J[0].y -= J[i].y;
J[0].z -= J[i].z;
}
x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Vector3d(J[0].x,J[0].y,J[0].z));
if((bool) std::isnan((double) x_[0]))
return 0;
return 1;
@@ -113,12 +136,12 @@ int SparseImgAlignGpu::solve()
void SparseImgAlignGpu::update()
{
/// 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);
SE2 update = SE2(pos[2],Eigen::Vector2d(pos[0],pos[1])) * SE2::exp(-0.5*x_);
pos[0]=update.translation()(0);
pos[1]=update.translation()(1);
pos[2]=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());
Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-0.5*x_);
pos[0].x=update.translation()(0);
pos[0].y=update.translation()(1);
pos[0].z=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());
residual_->reload_buf(1,2,pos);
}