From 47e2993ed8051cbb88c74ef8985d181cb7fbb35a Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 4 Aug 2021 18:18:21 +0200 Subject: [PATCH] debug kernel --- GPU_version/vio/include/vio/atan_camera.h | 1 + GPU_version/vio/kernel/compute-residual.cl | 157 +++++++++---------- GPU_version/vio/launch/Px30.launch | 2 +- GPU_version/vio/out.txt | Bin 31796 -> 5883 bytes GPU_version/vio/src/atan_camera.cpp | 5 +- GPU_version/vio/src/frame_handler_mono.cpp | 21 +-- GPU_version/vio/src/sparse_img_align_gpu.cpp | 29 ++-- 7 files changed, 108 insertions(+), 107 deletions(-) diff --git a/GPU_version/vio/include/vio/atan_camera.h b/GPU_version/vio/include/vio/atan_camera.h index 97c2f75..943b668 100644 --- a/GPU_version/vio/include/vio/atan_camera.h +++ b/GPU_version/vio/include/vio/atan_camera.h @@ -14,6 +14,7 @@ #define ATAN_CAMERA_H_ #include +#include #include #include #include diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index 2817e39..d02fd2b 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -22,15 +22,15 @@ double3 xyz_cur(double cur_x, double cur_z, double cur_pitch, double ref_x, doub 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)); + 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))); + 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) @@ -43,7 +43,7 @@ void jacobian_xyz2uv(double3 xyz_in_f, double* J) *(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) +void compute_hessain(double3 j, __global double* H, double w) { *(H + 0) += j[0] * j[0] * w; *(H + 1) += j[0] * j[1] * w; @@ -56,7 +56,7 @@ void compute_hessain(double3 j, double* H, double w) *(H + 8) += j[2] * j[2] * w; } -void compute_Jacobian(double3 j, double* Jacobian, double r, double 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; @@ -64,30 +64,30 @@ void compute_Jacobian(double3 j, double* Jacobian, double r, double 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 * 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 double * camera_model, // camera model [f_x,f_y,c_x,c_y,s_] - __global float * errors, - __global double * Hessian, - __global double * Jacobian, - double chi + __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 * 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_] + __global float * errors, + __global double * Hessian, + __global double * Jacobian, + __global double * chi ) { 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 = pown(2.0, -level); + double scale = pow(2.0, -level[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); + int f = get_global_id(0); // check if reference with patch size is within image double u_ref = featue_px[f].x * scale; @@ -96,14 +96,14 @@ __kernel void compute_residual( 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; + u_ref_i + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[0] || v_ref_i + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[1]) + return; double3 xyz_reference = xyz_ref(ref_feature[f][0], ref_feature[f][1], ref_feature[f][2], ref_pose[0], ref_pose[1], ref_pose[2]); // evaluate projection jacobian - double* frame_jac = (double *) malloc(6 * sizeof(double)); // 2X3 - jacobian_xyz2uv(xyz_reference, frame_jac); + double frame_jac[6]; // 2X3 + jacobian_xyz2uv(xyz_reference, &frame_jac); // compute bilateral interpolation weights for reference image double subpix_u_ref = u_ref - u_ref_i; @@ -114,7 +114,7 @@ __kernel void compute_residual( double w_ref_br = subpix_u_ref * subpix_v_ref; size_t pixel_counter = 0; - double3 xyz_current = xyz_cur(cur_pose.x, cur_pose.z, cur_pose.pitch, ref_pose[0] ,ref_pose[1] ,ref_pose[2], ref_feature[f]); + 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]; @@ -124,8 +124,8 @@ __kernel void compute_residual( // 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; + 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; @@ -142,67 +142,66 @@ __kernel void compute_residual( for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_element_addr, ++cur_element_addr) { - // precompute interpolated reference patch color - 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; + // 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, ((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 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, ((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)); + 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_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; + // 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; - double res = intensity_cur - value; + double res = intensity_cur - value; - // used to compute scale for robust cost - // if(compute_weight_scale) - errors[f * pow(PATCH_SIZE, 2) + (y * PATCH_SIZE) + x] = fabs(res); + // 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_); - // } + // robustification + double weight = 2.0; + // if(use_weights_) { // use_weights_ dafault value is false + // weight = weight_function_->value(res/scale_); + // } - *chi += res * res * weight; - n_meas_++; + 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; - // } + // 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 = *chi / n_meas_; - free(frame_jac); + chi[0] = chi[0] / n_meas_; } diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index 36114e8..b66a344 100644 --- a/GPU_version/vio/launch/Px30.launch +++ b/GPU_version/vio/launch/Px30.launch @@ -1,6 +1,6 @@ - + diff --git a/GPU_version/vio/out.txt b/GPU_version/vio/out.txt index 8447d6114a8abe41392eac2b1920d1bc060d267e..ab3b540fa51ae81430eba7a1f2fcb09d85adb652 100644 GIT binary patch literal 5883 zcmb_gTXW(#7T&k5;xF`z9=o>|Bgv9%GdmA7O=qYfkbYDbV~gO8ZFy{y%l`U3 zvOz!^aHcA$A_cm9=OUeRzN1gKIK4&V>G#SpQ=bE|3ptgvX*@`xE@SD0bED_JB~>_!d%BWhxr^w)yiNBhFc&h z=bppyImnfaA6AvYWNsU#O{ho7Sv8k01`k_I*uthQirNlmE@kHwUB2#ty3`hew16_( zb20JGfrbZ~dJYr31(ajrIL_IV5BD`?p39is1}ZqQ-LvLL*qBc&OoZ#SVLI4xs6MB~ zj~R-yxQc@${-2D_?&BCU#ysvG^IG|&dJjb$`6b{Ob?oyu1{_8~U3T;wG@@3`Pvgvw z;u7L0u6FKghxTj;%cG7vfUb%le3X&Vncz01ITsM-C*6>%QmVX)Q@>gjvfKqFYjLlG zIi})hNNSDCs#u5By4ZlH!Hb_)rO}(Ws10^8j%|AC^b&O+_}M!3W5`towH1`RggGZc zAD5*}WLO^}oZ&8+V7RE~-9c;u3fo7+NxMVapZOrb=t|p^Wx^GM9t?+mrs8rJbl5tds5o(5&KN%F z*^57BKSAQwL-n)+tgqAeSeM)+sFtco z5nShcD7@pWlF8@!7c{)OLGyJ6u_+NOK(kzC!^!9C%P(k<=Sds}Rjjh|Z=Lfn%{KJg zH*`@1`6>=!$8h3*8~qp$NB(qhIqD-L^JZ@u5)`9b{K7q`lKXpRSEDH)4E-OY`C>eq z_BX1bA+jxu_LqLW8BebL@zno)G@p(pi$1zfRQL#_LhJ6!608d<>BrjWUz)ifdJ3{o zMn5eIR}!|qYw3L)#RY^i2niB@f8)M(!`bE4cru#%gCB$OWbkzYt{{8PnaqZRiGMk| z^cUm*z?|UR7JKs-liAmU1)eJ$3haluC-{+TF-l$H+Tv(nYtB_xL9t6cJu`GB0cliQ zFoNBq7qEd)xw(BCP?fh}GKgk;T2mFN^bk{zoX3Q^j9r4ovBbV*oR!Msa0fh8g`5Hj zg_12_{WyI%DFH~IM)HL2Yi{Wv>1%+B* zF!C1^Jjo(>kjPLN6qkL7YL|d|=-;b2fgZrd2xty?Dn*6-a}9wOSqS|;xkej$g8In8 z;%J@f0Cqw|972Z3fdzS5qkF}k6j>jptHF;`I@1*AmwWveg zkF=dSLV!tB-{3as9E`U=^xSRe5Gb%{{toHFf|c|mZHJZsoGh6KWnN$E-#wvRr9q*lNE29qKko>k(wsk+gxDMtFv5ZadUY&nE%oFb@SH& zmM|L3(bZrM1nU}jQV0D5)j~U+H-jKWcN>N3qf{m;io<#>3~zJ=QeqKiIrXr%!_eqH zf?H*VHzZRp$7ay*W4#uLv6(OpuukY^6!Q9;CieLBCewo~yxaV?fvQFBBpx(vFlU6C z?x>IGZuD(y+DB~9u48=)Ftpuvvj94B=wY;`M&R-8w2%1SlBNaF&TKrqWhdc_HjCA7 zEH$8+cx@*1JITRBgzF>GZyg8Y%Lb`_S2~=ik(#%mgNf#m3moA#if%>MZ*DU5KqrSxK0|6LA#>Exn-?mCta957L7mx# z^K)FSP##C{28vyAx8Wxwg*4$`4i*rl{>^;+>n6ydPYIHhDyu$$?tYWmKOn_ylgJzV zP3DnRrA_J;>&ypDAtk;iJsVTDwnOqz>#D13{4wi2r?GxJ7Xrnub6gxsbgPp+?7lnj zCOE#UUrx4L_MAyDZ{Ti?So9bti8e%KNWR2XXL}kyO_Zy3V{)XPGqllP?@Se>B2_@` Y>gf8_who0V%@cUKuCJL_^N1jr&mpaJB|Jj`$3 zuNwqNmOz$7ZiA~5wYrNqI52wo@r>B69oo8M|7n>8&&i_1b=0N$4Q{>mjc=P;06Gaf zVu8gdvf5SEF6HFWVxbf%Yuea$HPa5Tj_fcMUm@wPq{<4n)NbZThrVunvCM>Lo-1Hl z<#gc# zvP#T!{;q}r5?+NOW)&DPz+@H0Kc-j(;R7?cVMeli5nYv+IZ;{3X+jqqo4z;Evc%;) z;@uVpkmdw_wY44e`lBy_({7N~z`Tgt>NiJ7il&FaPPvusE158Y_ zz6=v(@r7J=l#bfrR@!TSY-)Y)HYLc+>9tL_Tt^RwoMxciREcdxV0oSh$@fs<_*RlqBZs~Zr46RGW7_Ubk2HlE-veg$M7c*S+SnuZnCj6EiD?-Q zTrbKm@rGnLbs6tuZ>&2I*dDP1Y<=*-Y!jccylVtMrZ*ip);<|h$4&dJ8{`G4N0ns1 zL89mDGTA3|v-5^=Tch!uwA)1ZugIuSN;yBDV%CLDkKF0L%O+&#K}bT6>}lG>w{M}w zm>IIa2X?$M3FzpS4~|3Y2A|=>#3Fn6wpTLilwX14dXkukdTi0vMSN>$`4-HJ4D`T? zInuNnx}i7;|H+J);9V~yI!Sgk0<}(EXeu)ak=#H0sTorrv_ohh{#HkgMz`1&{IjI| zXQn){Lj#*G*kb*zxNGt(`NC(v2-i`!p{3J(^=f$Cc)o`2{)&G-{T0ik10BZZ_ji>^ z*3uuzNA6bg$qFb_PGv<))Sd*jr(lgGm|)#c;6(-$#_&0HLadZ&V3~7<3IGtuZkm2(%$3D|k{c`3uzlQE@VLkIs05k~ETU+sJe4GA=g}-&ZZq-B zK3B4yY4}8Dy_bAa35(p8685Qt>l9Pw#PJ*98yP;96$>Q15zVb6yhKE(TE=>&u2{nS zb`l;i8(t=j90QGHzk!4cX_g@2<#MOBOwPp;7Pge|N*m6R24NPyTM0)rzp8|pl#3-S z_jplxDiFtRqInO9z>C-rPGY@&M{!vLFz*AFbu&WPsh#5MGZN_VTj_iEX1Ee9&LYh$ zTTq3N6_L(YP@~bhC%?_@NEx`;u(*7QtVL`ch`ukr%K67CGfbiLZ_!8=o8RM*)cld zgd4q<+=T(J3Wej83>t`_0dtl9f>mPhwf{RB1WU0tyvAm4;89 zP4H=U73N{5_nmTGoD@X9DL#FrlczlIDOniB@YLA`o+>3LlFlzw=u{dyZFC!RskXL0 zl*mP=>PA5pv6upi(MjG0ohl_SmB8c53Y|(rr(NI1EbUNYpOki^AR%4+ZucGdP86lq zM5nTDV^&JeVpN4rrJ>W#Z)28pCV@Mi!g_yWwCpcg^4+q8bTK-8ANQRYKoz@ArCr9X zbey77g-xYl)2{AfZhaL7zgTPLT@_D`oi;jkT90%wHhmAC(`;Q7gVU}rV^&I@$v*p8 zp;KAt)Y;W-%pF{Xsq>_n8%rSqsEyERJ<>l0I>B#Zi=z;opxoWw#;lZ_MWtD}LZ{Nu zY1g+gw>Xek^UCBV=(Hy3?*^S_hM)Ucj80|U#;lZ_RZN9WrJ>W#Z)0wLBC%$WlTFZR zP142a^tJ9gWvzc@xfq^GyN+3DIjfZlpGw0g+;`&BxS z0@17;<&>90Q%rQ{moRe}Nt?1({};j{rY&Esv>;2Lg=Um+MQ#?eXwfG8y*T77OlO3E zs0q_hJ_}Tuu^$$E?Un{N)olF2;{k8>=LhfmZ?(4v$FFH%+y;IXvg|N2)(W1fPvgfQ zJvx7Fmc?yFfKnwJXZA@kB{AmDX0))Wxu5l{s(4exB_%$Mb^6sHC5q*@XtC){q`c`Z z_jzJFLpZtxs_PN`4k2ak<}j3>EYCdQ?7D41m_InE$XkNS&t8!QG;7 z04g{pph0dIEtc*)IU7?W?+`i$RJvt1iY<~ou>P>~TX7Xh>5Oc+T}<%xHf z6HA$zAA2IWqQSD!xgNa=)7Ri<^tLiQjt8`)wY2K~8t%zZ9|X7w7~~&Pe1fyzYWLs2 zMd)qINA%&2fZ)S4jLFL2!|@+HX__8}zCD;CosL$od$*R40KKM;>@aY2D64Ttq#}t+ z5Ba>Li)}8}u)_0*>C5ME#A0vGq9ALU;UXq7Wgx1@_)%fi@C%~}&=i+5+DLbXO9o@r z7>j+b7>b=YbfbfkSxO!RcaAd$A%OEAQ+x@)JkCXMw1%kAKB7O9`d%bKm`aqCXZ{xE zzA?f{$qPImz!swa?^u4qh2sv`EOmest|v-~ElypWcp!HXj%HZG92bdwNIamphiGY$&mD*j`j6|yu+m4 zwU8Xo4QRFfC3Z3s2YjG;0|p zj4wEdr`Y284=ke_k_>(!(*WV%Tjc!+DZa+~0}1sn7Sc`Kjcmti)y`oR0qKXn)9h2$ z7!LW|Da*tp!s`fT|1gl`}|(ST-faXfyA%;I0$v0;!`qd5?DSSB%?5#^xADE z0_<7{o!@4e?rN7GPEHOkey_cVe`-jhc6e}sjq(B+=-wldTn%fM4O*)$RALmGj?wpe z_yvaJnYO|DqB`akS=(B?)atToI*%mJV2|dtNUGy3e6%~4R3-@oXcm)F`tV|gNj79U;#^isv?Z)0=_7kYNNX2!clL;snb)#Jj!0pbKAv|Eb;Dq1EUlr&z)53scDMJ(q9cK?4DBoI6@`yd2CT*9N8+bk=jq0R}XL@4{OWC8vq@FFT zxPg^3E#4$)@Fum|>DjA8EE>{gn?ER#IwgmHjap5ghF+5qpy1$IcYu}v9t>N(6W=p{ zDpyez5{%B4^PL@z$OOMDypoV4^g2#?%8vmP+R4EsR;l*k;^;-Z$dQ*&cSc?i;_Y1I z;{>TYfS4JGEYOdU#|lF_rM5qHHE;?@TnC<$sm$gM@bIQ#lcp+OHv29g&3@Kqz&;qp z*~lDtMZq3VU(ZPsf^X7Z`odwdQS@(8WyGq!3+!Sl+QA04A oMxrpiJGj@8PASo`heX7lk3Eg;YeoH-)8!!NH=B)#U4;qj|7aX}aR2}S diff --git a/GPU_version/vio/src/atan_camera.cpp b/GPU_version/vio/src/atan_camera.cpp index b2ed822..7be69e0 100644 --- a/GPU_version/vio/src/atan_camera.cpp +++ b/GPU_version/vio/src/atan_camera.cpp @@ -36,11 +36,14 @@ ATANCamera(double width, double height, tans_ = 0.0; distortion_ = false; } + param_ = (double *) malloc(5 * sizeof(double)); } ATANCamera:: ~ATANCamera() -{} +{ + delete param_; +} Vector3d ATANCamera:: cam2world(const double& x, const double& y) const diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 9cb1471..eddcafc 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -54,20 +54,20 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrixwrite_buf(1,4,300,f); cl_double2 px[300]={0}; gpu_fast_->write_buf(1,5,300,px); - int level=0; - gpu_fast_->write_buf(1,6,level); + int level[1]={0}; + gpu_fast_->write_buf(1,6,1,level); double c[5]={0}; gpu_fast_->write_buf(1,7,5,c); - float e[300]={0}; + double e[300]={0}; gpu_fast_->write_buf(1,8,300,e); - float H[6]={0}; - gpu_fast_->write_buf(1,9,6,H); - float J[3]={0}; + 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=0.0; - gpu_fast_->write_buf(1,11,chi2); + double chi2[1]={0.0}; + gpu_fast_->write_buf(1,11,1,chi2); imageAlign_=new SparseImgAlignGpu(Config::kltMaxLevel(), Config::kltMinLevel(), - 30, SparseImgAlign::GaussNewton, false,gpu_fast_); + 30, SparseImgAlign::GaussNewton, true,gpu_fast_); klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); initialize(); } @@ -164,9 +164,10 @@ 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, false); - if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; + if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;*/ if(imageAlign_->run(last_frame_, new_frame_)==0)return RESULT_FAILURE; reprojector_.reprojectMap(new_frame_, overlap_kfs_); size_t sfba_n_edges_final=0; diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index a7891d2..b2eaf4f 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -35,7 +35,6 @@ SparseImgAlignGpu::SparseImgAlignGpu( size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame) { reset(); - if(ref_frame->fts_.empty()) { return 0; @@ -46,15 +45,11 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame) residual_->reload_buf(1,2,cur_pos); residual_->reload_buf(1,3,ref_pos); residual_->reload_buf(1,7,camera); - cl_double3* features; - cl_double2* featue_px; + cl_double3 features[ref_frame->fts_.size()]; + cl_double2 featue_px[ref_frame->fts_.size()]; feature_counter_ = 0; // is used to compute the index of the cached jacobian - std::vector::iterator visiblity_it = visible_fts_.begin(); - for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end(); - ++it, ++visiblity_it){ - if(!*visiblity_it) - continue; - const Vector3d xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm()); + for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){ + Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm(); features[feature_counter_].x=xyz_ref(0); features[feature_counter_].y=xyz_ref(1); features[feature_counter_].z=xyz_ref(2); @@ -84,8 +79,8 @@ double SparseImgAlignGpu::computeResiduals( bool linearize_system, bool compute_weight_scale) { - double chi2=0.0; - residual_->reload_buf(1,11,&chi2); + 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) { @@ -94,14 +89,16 @@ double SparseImgAlignGpu::computeResiduals( std::vector errors(error,error+feature_counter_); scale_ = scale_estimator_->compute(errors); } - residual_->read(1,11,&chi2); - return chi2; + + double out[1]={0.0}; + residual_->read(1,11,out); + return out[0]; } int SparseImgAlignGpu::solve() { - double* H; - double* J; + 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)); @@ -112,7 +109,7 @@ int SparseImgAlignGpu::solve() void SparseImgAlignGpu::update() { /// TODO the update situation may have a smarter solution - double* pos; + double pos[3]={0}; residual_->read(1,2,pos); SE2 update = SE2(pos[2],Eigen::Vector2d(pos[0],pos[1])) * SE2::exp(-0.25*x_); pos[0]=update.translation()(0);