From a2802878f471c9e7c15115bd0235fb68dccff006 Mon Sep 17 00:00:00 2001 From: VahdPourAmin Date: Sat, 24 Jul 2021 22:00:49 +0200 Subject: [PATCH] opencl kernel clean and aproximately ready to test --- GPU_version/vio/kernel/compute-residual.cl | 172 ++++++++++----------- 1 file changed, 80 insertions(+), 92 deletions(-) diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index 501b558..662f14d 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -4,48 +4,44 @@ // 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(double f_x, double f_y, double c_x, double c_y, double s_, double x, double y, double z) { - double2 uv=(x/z,y/z); - double r = sqrt(pow(uv[0],2)+pow(uv[1],2)); - double factor=1.0; - if(s_ !=0 || r > 0.001)factor=(atan(r * 2.0 * tan(0.5 * s_)) / (r*s_)); - return double3(cx_ + fx_ * factor * uv[0],cy_ + fy_ * factor * uv[1]); + double2 uv = (x/z, y/z); + double r = sqrt(pow(uv[0], 2) + pow(uv[1], 2)); + double factor = 1.0; + if(s_ != 0 || r > 0.001) + factor = (atan(r * 2.0 * tan(0.5 * s_)) / (r * s_)); + return double3(cx_ + fx_ * factor * uv[0], cy_ + fy_ * factor * uv[1]); } -double3 xyz_cur(double cur_x,double cur_z,double cur_pitch,double ref_x,double ref_z,double ref_pitch,double3 ref_feature){ - if(ref_pitch<0.0) - ref_pitch=3.141592653589793238462643383279502884197169399375-ref_pitch; + +double3 xyz_cur(double cur_x, double cur_z, double cur_pitch, double ref_x, double ref_z, double ref_pitch, double3 ref_feature) +{ + if(ref_pitch < 0.0) + ref_pitch = 3.141592653589793238462643383279502884197169399375 - ref_pitch; 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_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_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_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) +double3 xyz_ref(double fts_pos_x, double fts_pos_y, double fts_pos_z, double ref_x, double ref_y, double ref_z) { - double x = sqrt(pow((fts_pos_x - ref_x), 2)); - double y = sqrt(pow((fts_pos_y - ref_y), 2)); - double z = sqrt(pow((fts_pos_z - ref_z), 2)); - return double3(x, y, 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) { - double x = xyz_in_f[0]; - double y = xyz_in_f[1]; - double z_inv = 1. / xyz_in_f[2]; - double z_inv_2 = z_inv * z_inv; - - *(J + 0) = -z_inv; // -1/z - *(J + 1) = x * z_inv_2; // x/z^2 - *(J + 2) = -(1.0 + pow(x,2) / z_inv_2); // -(1.0 + x^2/z^2) - - *(J + 3) = 1e-19; // 0 - *(J + 4) = y * z_inv_2; // y/z^2 - *(J + 5) = -x * y / z_inv_2; // -x*y/z^2 + *(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 + 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, double* H, double w) @@ -88,33 +84,30 @@ __kernel void compute-residual( int border = PATCH_HALFSIZE + 1; int stride = get_image_dim(image_cur)[0]; //width, height double scale = pow(2, (-1.0 * level)); - double focal_length = camera_model[0]; - size_t feature_counter = 0; - double chi2 = 0.0; - int n_meas_ = 0; + size_t n_meas_ = 0; // Prepare a suitable OpenCL image sampler. sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; // Use global work feature. int const f = get_global_id(0); - //ref_feature[f].x; - //ref_feature[f].y; - //ref_feature[f].z; + // 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(ref_feature[f]->point == NULL || u_ref_i - border < 0 || v_ref_i - border < 0 || //??????? ref_feature[f]->point - if(u_ref_i - border < 0 || v_ref_i - border < 0 || - u_ref_i + border >= get_image_dim(image_ref)[0] || v_ref_i + border >= get_image_dim(image_ref)[1]) + 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; - xyz_reference = xyz_ref(ref_feature[f][0], ref_feature[f][1], ref_feature[f][2], ref_pose.x, ref_pose.y, ref_pose.z); // ref_pose.y ???? + xyz_reference = xyz_ref(ref_feature[f][0], ref_feature[f][1], ref_feature[f][2], ref_pose.x, ref_pose.y, ref_pose.z); + // evaluate projection jacobian double* frame_jac = (double *) malloc(6 * sizeof(double)); // 2X3 jacobian_xyz2uv(xyz_reference, 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); @@ -122,7 +115,7 @@ __kernel void compute-residual( 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; - /// compute residual ... + double3 xyz_current = xyz_cur(cur_pose.x, cur_pose.z, cur_pose.pitch, ref_pose.x ,ref_pose.z ,ref_pose.pitch, 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; @@ -132,9 +125,10 @@ __kernel void compute-residual( 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 - border < 0 || v_cur_i - border < 0 || - u_cur_i + border >= get_image_dim(image_cur)[0] || v_cur_i + border >= get_image_dim(image_cur)[0]) - return; + 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 double subpix_u_cur = u_cur - u_cur_i; double subpix_v_cur = v_cur - v_cur_i; @@ -142,58 +136,53 @@ __kernel void compute-residual( 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; - /// ... compute residual - - /// TODO - check & implementation - // float* cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; - // float* ref_patch_cache_ptr = reinterpret_cast(ref_patch_cache_.data) + patch_area_*feature_counter; - for(int y = 0; y < PATCH_SIZE; ++y) { - /// TODO - check & implementation - int ref_element_add = image_ref.data + (v_ref_i + y - PATCH_HALFSIZE) * stride + (u_ref_i - PATCH_HALFSIZE); - int cur_element_add = image_cur.data + (v_cur_i + y - PATCH_HALFSIZE) * stride + (u_cur_i - PATCH_HALFSIZE); - // uint8_t* ref_img_ptr = (uint8_t*) ref_img.data + (v_ref_i+y-patch_halfsize_)*stride + (u_ref_i-patch_halfsize_); - // uint8_t* cur_img_ptr = (uint8_t*) image_cur.data + (v_cur_i + y - patch_halfsize_) * stride + (u_cur_i - patch_halfsize_); + 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); - // for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_img_ptr, ++cache_ptr) - // for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++cur_img_ptr, ++ref_patch_cache_ptr) - for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_element, ++cur_element) + for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_element_addr, ++cur_element_addr) { - /// TODO - check & implementation // precompute interpolated reference patch color - // *cache_ptr = w_ref_tl*ref_img_ptr[0] + w_ref_tr*ref_img_ptr[1] + w_ref_bl*ref_img_ptr[stride] + w_ref_br*ref_img_ptr[stride+1]; - int2 px_reftl = (ref_element_add % get_image_dim(image_ref)[0], ref_element_add / get_image_dim(image_ref)[0]); - int2 px_reftr = ((ref_element_add + 1) % get_image_dim(image_ref)[0], (ref_element_add + 1) / get_image_dim(image_ref)[0]); - int2 px_refbl = ((ref_element_add + stride) % get_image_dim(image_ref)[0], (ref_element_add + stride) / get_image_dim(image_ref)[0]); - int2 px_refbr = ((ref_element_add + stride + 1) % get_image_dim(image_ref)[0], (ref_element_add + stride + 1) / get_image_dim(image_ref)[0]); + int2 px_reftl = ( ref_element_addr % get_image_dim(image_ref)[0], ref_element_addr / get_image_dim(image_ref)[0]); + int2 px_reftr = ((ref_element_addr + 1) % get_image_dim(image_ref)[0], (ref_element_addr + 1) / get_image_dim(image_ref)[0]); + int2 px_refbl = ((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 = ((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; - // we use the inverse compositional: thereby we can take the gradient always at the same position - // get gradient of warped image (~gradient at warped position) - double dx = 0.5f * ((w_ref_tl * ref_img_ptr[1] + w_ref_tr * ref_img_ptr[2] + w_ref_bl * ref_img_ptr[stride+1] + w_ref_br * ref_img_ptr[stride+2]) - -(w_ref_tl * ref_img_ptr[-1] + w_ref_tr * ref_img_ptr[0] + w_ref_bl * ref_img_ptr[stride-1] + w_ref_br * ref_img_ptr[stride])); - double dy = 0.5f * ((w_ref_tl * ref_img_ptr[stride] + w_ref_tr * ref_img_ptr[1+stride] + w_ref_bl * ref_img_ptr[stride*2] + w_ref_br * ref_img_ptr[stride*2+1]) - -(w_ref_tl * ref_img_ptr[-stride] + w_ref_tr * ref_img_ptr[1-stride] + w_ref_bl * ref_img_ptr[0] + w_ref_br * ref_img_ptr[1])); + double dx = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((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, ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x)); // compute residual - /// TODO - check & implementation - // double intensity_cur = w_cur_tl * cur_img_ptr[0] + w_cur_tr * cur_img_ptr[1] + w_cur_bl * cur_img_ptr[stride] + w_cur_br * cur_img_ptr[stride+1]; - int2 px_curtl = (cur_element_add % get_image_dim(image_cur)[0], cur_element_add / get_image_dim(image_cur)[0]); - int2 px_curtr = ((cur_element_add + 1) % get_image_dim(image_cur)[0], (cur_element_add + 1) / get_image_dim(image_cur)[0]); - int2 px_curbl = ((cur_element_add + stride) % get_image_dim(image_cur)[0], (cur_element_add + stride) / get_image_dim(image_cur)[0]); - int2 px_curbr = ((cur_element_add + stride + 1) % get_image_dim(image_cur)[0], (cur_element_add + stride + 1) / get_image_dim(image_cur)[0]); + int2 px_curtl = ( cur_element_addr % get_image_dim(image_cur)[0], cur_element_add / get_image_dim(image_cur)[0]); + int2 px_curtr = ((cur_element_addr + 1) % get_image_dim(image_cur)[0], (cur_element_add + 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_add + (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_add + (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; - // const float res = intensity_cur - (*ref_patch_cache_ptr); double res = intensity_cur - value; // used to compute scale for robust cost - if(compute_weight_scale) //????????? compute_weight_scale ???????????? - errors[(y * PATCH_SIZE) + x] = fabs(res) + // if(compute_weight_scale) + errors[f * pow(PATCH_SIZE, 2) + (y * PATCH_SIZE) + x] = fabs(res); // robustification double weight = 2.0; @@ -201,22 +190,21 @@ __kernel void compute-residual( // weight = weight_function_->value(res/scale_); // } - chi2 += res * res * weight; + *chi += 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) * (focal_length / scale); - compute_hessain(j, Hessian, weight); // H_.noalias() += J * J.transpose() * weight; - compute_Jacobian(j, Jacobian, res, weight) // Jres_.noalias() -= J * res * weight; + // 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 = chi2 / n_meas_; - feature_counter++; } } - + *chi = *chi / n_meas_; + free(frame_jac); }