diff --git a/GPU_version/vio/include/vio/cl_class.h b/GPU_version/vio/include/vio/cl_class.h index 2b30b3b..99b6168 100644 --- a/GPU_version/vio/include/vio/cl_class.h +++ b/GPU_version/vio/include/vio/cl_class.h @@ -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 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,size_t>(std::pair(cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(T) * buf_size), - 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"<setArg(id,_buffers.back().first.first); + _buffers.push_back(std::pair,size_t>(std::pair(cl::Buffer(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR + , sizeof(T) * buf_size,buf), + buf_size),id)); + cl_int err=_kernel->setArg(id,_buffers.back().first.first); if(err!=0){ std::cerr<<"Error C:Kernel, F:write, L3:setArg\t"<(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, - cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), - buf.size().width, - buf.size().height, - 0, - reinterpret_cast(buf.data)),id)); + _images.push_back(std::pair(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(buf.data)),id)); cl_int err=_kernel->setArg(id,_images.back().first); if(err!=0){ @@ -55,26 +53,18 @@ public: template 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"<enqueueMapBuffer(i.first.first,CL_TRUE,CL_MAP_WRITE,0,sizeof(T) * i.first.second); + memcpy(Map_buf,buf,sizeof(T) * i.first.second); + if(queue->enqueueUnmapMemObject(i.first.first,Map_buf)!=CL_SUCCESS)std::cerr<<"Reload buffer on GPU failed"<<'\n'; } + return CL_SUCCESS; }; int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){ try{ for(auto&& i:_images)if(i.second==id){ - i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, + 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, @@ -85,7 +75,7 @@ public: std::cerr<<"Error C:Kernel, F:write image, L:setArg ID:\t"<,size_t>> _buffers; std::vector> _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) { cl_int err=0; - 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){ - err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*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){ - std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo(); - std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo()<<" ,Run out: "<getInfo();++i) + 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){ + err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/); + }else{ + err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/); + }; + if(err!=0){ + std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo(); + std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo()<<" ,Run out: "<getInfo();++i) std::cerr<<"argument type "<getArgInfo(i)<<'\n'; - } - return err; + } + return queue->finish(); } template 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 + 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 _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 diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index b8c10d7..7408805 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -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 diff --git a/GPU_version/vio/include/vio/nlls_solver.h b/GPU_version/vio/include/vio/nlls_solver.h index 886556d..58ec2f1 100644 --- a/GPU_version/vio/include/vio/nlls_solver.h +++ b/GPU_version/vio/include/vio/nlls_solver.h @@ -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) { } diff --git a/GPU_version/vio/include/vio/sparse_img_align_gpu.h b/GPU_version/vio/include/vio/sparse_img_align_gpu.h index 2655b22..27d7b53 100644 --- a/GPU_version/vio/include/vio/sparse_img_align_gpu.h +++ b/GPU_version/vio/include/vio/sparse_img_align_gpu.h @@ -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. diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index 6d11d83..baf62f5 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -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,151 +48,106 @@ 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]); - int2 px_reftr = (int2)((ref_element_addr + 1) % get_image_dim(image_ref)[0], (ref_element_addr + 1) / get_image_dim(image_ref)[0]); - int2 px_refbl = (int2)((ref_element_addr + get_image_dim(image_ref)[0]) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0]) / get_image_dim(image_ref)[0]); - int2 px_refbr = (int2)((ref_element_addr + get_image_dim(image_ref)[0] + 1) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0] + 1) / get_image_dim(image_ref)[0]); - double value = w_ref_tl * read_imageui(image_ref, sampler, px_reftl).x + w_ref_tr * read_imageui(image_ref, sampler, px_reftr).x + - w_ref_bl * read_imageui(image_ref, sampler, px_refbl).x + w_ref_br * read_imageui(image_ref, sampler, px_refbr).x; + // precompute interpolated reference patch color + int2 px_reftl = (int2)( ref_element_addr % get_image_dim(image_ref)[0], ref_element_addr / get_image_dim(image_ref)[0]); + int2 px_reftr = (int2)((ref_element_addr + 1) % get_image_dim(image_ref)[0], (ref_element_addr + 1) / get_image_dim(image_ref)[0]); + int2 px_refbl = (int2)((ref_element_addr + get_image_dim(image_ref)[0]) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0]) / get_image_dim(image_ref)[0]); + int2 px_refbr = (int2)((ref_element_addr + get_image_dim(image_ref)[0] + 1) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0] + 1) / get_image_dim(image_ref)[0]); + double value = w_ref_tl * read_imageui(image_ref, sampler, px_reftl).x + w_ref_tr * read_imageui(image_ref, sampler, px_reftr).x + + w_ref_bl * read_imageui(image_ref, sampler, px_refbl).x + w_ref_br * read_imageui(image_ref, sampler, px_refbr).x; - double dx = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x + - w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 2) % get_image_dim(image_ref)[0], ((ref_element_addr + 2) / get_image_dim(image_ref)[0]))).x + - w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) / get_image_dim(image_ref)[0]))).x + - w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2) / get_image_dim(image_ref)[0]))).x) - -(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr - 1) % get_image_dim(image_ref)[0], ((ref_element_addr - 1) / get_image_dim(image_ref)[0]))).x + - w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0) % get_image_dim(image_ref)[0], ((ref_element_addr + 0) / get_image_dim(image_ref)[0]))).x + - w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1) / get_image_dim(image_ref)[0]))).x + - w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x)); + double dx = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x + + w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 2) % get_image_dim(image_ref)[0], ((ref_element_addr + 2) / get_image_dim(image_ref)[0]))).x + + w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) / get_image_dim(image_ref)[0]))).x + + w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2) / get_image_dim(image_ref)[0]))).x) + -(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr - 1) % get_image_dim(image_ref)[0], ((ref_element_addr - 1) / get_image_dim(image_ref)[0]))).x + + w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0) % get_image_dim(image_ref)[0], ((ref_element_addr + 0) / get_image_dim(image_ref)[0]))).x + + w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1) / get_image_dim(image_ref)[0]))).x + + w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x)); - double dy = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + - w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) / get_image_dim(image_ref)[0]))).x + - w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2) / get_image_dim(image_ref)[0]))).x + - w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) / get_image_dim(image_ref)[0]))).x) - -(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (-get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (-get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + - w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (1 - get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (1 - get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + - w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0) % get_image_dim(image_ref)[0], ((ref_element_addr + 0) / get_image_dim(image_ref)[0]))).x + - w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x)); + double dy = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + + w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) / get_image_dim(image_ref)[0]))).x + + w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2) / get_image_dim(image_ref)[0]))).x + + w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) / get_image_dim(image_ref)[0]))).x) + -(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (-get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (-get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + + w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (1 - get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (1 - get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + + w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0) % get_image_dim(image_ref)[0], ((ref_element_addr + 0) / get_image_dim(image_ref)[0]))).x + + w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x)); - // compute residual - int2 px_curtl = ( 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 + - w_cur_bl * read_imageui(image_cur, sampler, px_curbl).x + w_cur_br * read_imageui(image_cur, sampler, px_curbr).x; + // compute residual + int2 px_curtl = (int2)( cur_element_addr % get_image_dim(image_cur)[0], cur_element_addr / get_image_dim(image_cur)[0]); + int2 px_curtr = (int2)((cur_element_addr + 1) % get_image_dim(image_cur)[0], (cur_element_addr + 1) / get_image_dim(image_cur)[0]); + int2 px_curbl = (int2)((cur_element_addr + (get_image_dim(image_cur)[0])) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0])) / get_image_dim(image_cur)[0]); + int2 px_curbr = (int2)((cur_element_addr + (get_image_dim(image_cur)[0]) + 1) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0]) + 1) / get_image_dim(image_cur)[0]); - double res = 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_; } diff --git a/GPU_version/vio/src/cl_class.cpp b/GPU_version/vio/src/cl_class.cpp index 033ac11..c47c8be 100644 --- a/GPU_version/vio/src/cl_class.cpp +++ b/GPU_version/vio/src/cl_class.cpp @@ -1,7 +1,7 @@ // // Created by root on 4/27/21. // -#include "vio/cl_class.h" +#include opencl::opencl() { std::vector 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() <<'\n' - << "CL_DEVICE_OPENCL_C_VERSION: " <()<<'\n' - << "CL_DEVICE_BUILT_IN_KERNELS: " <()<<'\n' - << "CL_DEVICE_COMPILER_AVAILABLE: " <()<<'\n' - << "CL_DEVICE_LOCAL_MEM_SIZE: " <()<<'\n' - << "CL_DEVICE_GLOBAL_MEM_SIZE: " <()<<'\n'; + device = new cl::Device(all_devices[0]); + std::cout << "CL_DEVICE_NAME: " << device->getInfo() <<'\n' + << "CL_DEVICE_OPENCL_C_VERSION: " <getInfo()<<'\n' + << "CL_DEVICE_BUILT_IN_KERNELS: " <getInfo()<<'\n' + << "CL_DEVICE_COMPILER_AVAILABLE: " <getInfo()<<'\n' + << "CL_DEVICE_LOCAL_MEM_SIZE: " <getInfo()<<'\n' + << "CL_DEVICE_GLOBAL_MEM_SIZE: " <getInfo()<<'\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(device)<<'\n' - <getBuildInfo(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(*device)<<'\n' + <getBuildInfo(*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; } diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index f59dd72..0b4950a 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace vio { @@ -47,25 +48,21 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrixmake_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: "<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( diff --git a/GPU_version/vio/src/sparse_img_align.cpp b/GPU_version/vio/src/sparse_img_align.cpp index e4aa55b..128be16 100644 --- a/GPU_version/vio/src/sparse_img_align.cpp +++ b/GPU_version/vio/src/sparse_img_align.cpp @@ -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) diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index 617c690..431e041 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -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,43 +87,62 @@ double SparseImgAlignGpu::computeResiduals( bool linearize_system, bool compute_weight_scale) { - double chi2[1]={0}; - residual_->reload_buf(1,11,chi2); - /// 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 errors(error,error+feature_counter_); - scale_ = scale_estimator_->compute(errors); - } - - double out[1]={0.0}; - residual_->read(1,11,out); - return out[0]; + double H[9*300]={0}; + cl_double3 J[300]={0.0}; + double chi2_[300]={0.0}; + residual_->write_buf(1,11,scale_); + residual_->reload_buf(1,10,chi2_); + residual_->reload_buf(1,8,H); + residual_->reload_buf(1,9,J); + /// TODO GPU compute based on the feature + residual_->run(1,feature_counter_); + float error[300]={0.0}; + double chi[300]={0.0}; + residual_->read(1,10,feature_counter_,chi); + residual_->read(1,7,feature_counter_,error); + for(int i = 1; i < feature_counter_; ++i){ + error[0] += error[i]; + chi[0] += chi[i]; + } + scale_ = error[0]*(1.48f / feature_counter_); + return chi[0]/(feature_counter_*8); } int SparseImgAlignGpu::solve() { - double H[9]={0}; - double J[3]={0}; - residual_->read(1,9,H); - residual_->read(1,10,J); - x_ = Eigen::Matrix(H).ldlt().solve(Eigen::Matrix(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(H).ldlt().solve(Eigen::Vector3d(J[0].x,J[0].y,J[0].z)); if((bool) std::isnan((double) x_[0])) - return 0; + return 0; return 1; } void SparseImgAlignGpu::update() { /// TODO the update situation may have a smarter solution - double pos[3]={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()); - residual_->reload_buf(1,2,pos); + cl_double3 pos[1]={0.0,0.0,0.0}; + residual_->read(1,2,pos); + Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-0.5*x_); + pos[0].x=update.translation()(0); + pos[0].y=update.translation()(1); + pos[0].z=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real()); + residual_->reload_buf(1,2,pos); } void SparseImgAlignGpu::startIteration()