From 7dc930d13ecc312cb88f351b97c221ab79c9614b Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 18 Aug 2021 08:11:35 +0200 Subject: [PATCH] edit kernel --- GPU_version/vio/kernel/compute-residual.cl | 180 ++++++++++--------- GPU_version/vio/src/frame_handler_mono.cpp | 18 +- GPU_version/vio/src/reprojector.cpp | 2 +- GPU_version/vio/src/sparse_img_align_gpu.cpp | 8 +- GPU_version/vio/src/vo_node.cpp | 2 +- 5 files changed, 112 insertions(+), 98 deletions(-) diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index c5b899f..67e7794 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -35,6 +35,17 @@ void jacobian_xyz2uv(double3 xyz_in_f, double* J) *(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 jacobian_xyz2uv_2(double3 xyz_in_f, double3 ref_pose, double3 cur_pose, double* J) +{ + double3 txtytetha = cur_pose - ref_pose; + double2 uv = world2cam(xyz_in_f); + *(J + 0) = 1.0; + *(J + 1) = 0.0; + *(J + 2) = ((-1 * sin(txtytetha[2]) * cos(ref_pose[2])) - (cos(txtytetha[2]) * sin(ref_pose[2]))) * uv[0] + ((cos(txtytetha[2]) * cos(ref_pose[2])) - (sin(txtytetha[2]) * sin(ref_pose[2]))) * uv[1]; + *(J + 3) = 0.0; + *(J + 4) = 1.0; + *(J + 5) = ((-1 * cos(txtytetha[2]) * cos(ref_pose[2])) + (sin(txtytetha[2]) * sin(ref_pose[2]))) * uv[0] - ((sin(txtytetha[2]) * cos(ref_pose[2])) + (cos(txtytetha[2]) * sin(ref_pose[2]))) * uv[1]; +} void compute_hessain(double3 j, __global double* H, double w) { *(H + 0) += j[0] * j[0] * w; @@ -52,102 +63,105 @@ __kernel void compute_residual( __read_only image2d_t image_cur, // current frame __read_only image2d_t image_ref, // reference frame __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, - int level, +__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, +int level, __global float * errors, __global double * Hessian, __global double3 * Jacobian, __global double * chi, - double scale_ +double scale_ ) { - 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 - 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]={0.0}; // 2X3 - jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (double3)(ref_pose[0].x,0.0,ref_pose[0].y), 2.0)), &frame_jac); +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 +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 +//if(f==0) +// printf("ref_feature[f]: %f %f %f\n", ref_feature[f].x, ref_feature[f].y, ref_feature[f].z); +double frame_jac[6]={0.0}; // 2X3 +jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (double3)(ref_pose[0].x,0.0,ref_pose[0].y), 2.0)), &frame_jac); +//jacobian_xyz2uv_2(ref_feature[f], ref_pose[0], cur_pose[0], &frame_jac); - // compute bilateral interpolation weights for reference image - 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 reference image +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 - 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 = (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); +// compute bilateral interpolation weights for the current image +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 = (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, ++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; +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; - 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 = (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]); +// 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 = 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; +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; - } - } - errors[f] = e / pow(PATCH_SIZE,2.0); - chi[f]=chi_; +} +} +errors[f] = e / pow(PATCH_SIZE,2.0); +chi[f]=chi_; } diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 236400a..7cc4edf 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -48,20 +48,20 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrixmake_kernel("compute_residual"); gpu_fast_->write_buf(1,0,img); gpu_fast_->write_buf(1,1,img); - cl_double3 pose[1]={0.0}; + cl_float3 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}; + cl_float3 f[300]={0}; gpu_fast_->write_buf(1,4,300,f); - cl_double2 px[300]={0}; + cl_float2 px[300]={0}; gpu_fast_->write_buf(1,5,300,px); - double e[300]={0}; + float e[300]={0}; gpu_fast_->write_buf(1,7,300,e); - double H[9*300]={0}; + float H[9*300]={0}; gpu_fast_->write_buf(1,8,9*300,H); - cl_double3 J[300]; + cl_float3 J[300]; gpu_fast_->write_buf(1,9,300,J); - double chi2[1]={0.01}; + float chi2[1]={0.01}; gpu_fast_->write_buf(1,10,1,chi2); klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); initialize(); @@ -164,12 +164,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() 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/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 15bda16..d2d68bc 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -206,7 +206,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) bool Reprojector::reprojectPoint(FramePtr frame, Point* point) { Vector2d px(frame->w2c(point->pos_)); - if(frame->cam_->isInFrame(px.cast(), 8)) // 8px is the patch size in the matcher + if(frame->cam_->isInFrame(px.cast(), 3)) // 8px is the patch size in the matcher { const int k = static_cast(px[1]/grid_.cell_size)*grid_.grid_n_cols + static_cast(px[0]/grid_.cell_size); diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index 418fcdd..5dd9a7a 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -40,12 +40,12 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame) { return 0; } - 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()}; + cl_float3 ref_pos[1]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()}; + cl_float3 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); - cl_double3 features[ref_frame->fts_.size()]; - cl_double2 featue_px[ref_frame->fts_.size()]; + cl_float3 features[ref_frame->fts_.size()]; + cl_float2 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]); diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 489d7f2..63e9de0 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -13,7 +13,7 @@ // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#define VIO_DEBUG false +#define VIO_DEBUG true #include #include #include