public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

debug files

Browse Source
This commit is contained in:
gfuse
2021-08-18 12:05:44 +02:00
parent dc29b94830
commit 8b63732711
6 changed files with 95 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/nlls_solver_impl.hpp
+1 -1
View File
@@ -37,7 +37,7 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
if(!solve()) if(!solve())
{ {
// matrix was singular and could not be computed // matrix was singular and could not be computed
std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl; if(verbose_)std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
stop_ = true; stop_ = true;
} }
// check if error increased since last optimization // check if error increased since last optimization
GPU_version/vio/kernel/compute-residual.cl
+84 -85
View File
@@ -8,9 +8,9 @@ float2 world2cam(float3 feature)
{ {
float r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2)); float r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2));
float factor = 1.0; float factor = 1.0;
if(S != 0 || r > 0.001) if((float)S != 0 || r > 0.001)
factor = (atan(r * 2.0 * tan(0.5 * S)) / (r * S)); factor = (atan(r * 2.0 * tan(0.5 * (float)S)) / (r * (float)S));
return (float2)(C_X + F_X * factor * feature.x/feature.z, C_Y + F_Y * factor * feature.y/feature.z); return (float2)((float)C_X + (float)F_X * factor * feature.x/feature.z, (float)C_Y + (float)F_Y * factor * feature.y/feature.z);
} }
float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature) float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
@@ -22,8 +22,8 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z); float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z); float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z), return (float3)(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, ref_feature.y,
-1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z)); -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(float3 xyz_in_f, float* J) void jacobian_xyz2uv(float3 xyz_in_f, float* J)
@@ -53,102 +53,101 @@ __kernel void compute_residual(
__read_only image2d_t image_cur, // current frame __read_only image2d_t image_cur, // current frame
__read_only image2d_t image_ref, // reference frame __read_only image2d_t image_ref, // reference frame
__global float3 * cur_pose,//[reference frame pose{x,z,pitch}] __global float3 * cur_pose,//[reference frame pose{x,z,pitch}]
__global float3 * ref_pose,//[current frame pose{x,z,pitch}] __global float3 * ref_pose,//[current frame pose{x,z,pitch}]
__global float3 * 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 float3 * 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 float2 * featue_px, __global float2 * featue_px,
int level, int level,
__global float * errors, __global float * errors,
__global float * Hessian, __global float * Hessian,
__global float3 * Jacobian, __global float3 * Jacobian,
__global float * chi, __global float * chi,
float scale_ float scale_
) )
{ {
float scale = pow(2.0, -level); float scale = pow(2.0, -level);
// Prepare a suitable OpenCL image sampler. // Prepare a suitable OpenCL image sampler.
sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
// Use global work feature. // Use global work feature.
int f = get_global_id(0); int f = get_global_id(0);
// check if reference with patch size is within image // check if reference with patch size is within image
float2 uv_ref = featue_px[f] * scale; float2 uv_ref = featue_px[f] * scale;
float2 uv_ref_i = floor(uv_ref); float2 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; 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 // evaluate projection jacobian
float frame_jac[6]={0.0}; // 2X3 float frame_jac[6]={0.0}; // 2X3
jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (float3)(ref_pose[0].x,0.0,ref_pose[0].y), 2.0)), &frame_jac); jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (float3)(ref_pose[0].x,0.0,ref_pose[0].y), 2.0)), &frame_jac);
// compute bilateral interpolation weights for reference image // compute bilateral interpolation weights for reference image
float2 subpix= uv_ref - uv_ref_i; float2 subpix= uv_ref - uv_ref_i;
float w_ref_tl = (1.0 - subpix.x) * (1.0 - subpix.y); float w_ref_tl = (1.0 - subpix.x) * (1.0 - subpix.y);
float w_ref_tr = subpix.x * (1.0 - subpix.y); float w_ref_tr = subpix.x * (1.0 - subpix.y);
float w_ref_bl = (1.0 - subpix.x) * subpix.y; float w_ref_bl = (1.0 - subpix.x) * subpix.y;
float w_ref_br = subpix.x * subpix.y; float w_ref_br = subpix.x * subpix.y;
// compute bilateral interpolation weights for the current image // compute bilateral interpolation weights for the current image
float2 uv_cur_pyr = world2cam(xyz_cur(cur_pose[0], ref_pose[0] ,ref_feature[f])) * scale; float2 uv_cur_pyr = world2cam(xyz_cur(cur_pose[0], ref_pose[0] ,ref_feature[f])) * scale;
float2 uv_cur_i = floor(uv_cur_pyr); float2 uv_cur_i = floor(uv_cur_pyr);
float2 subpix_uv_cur = uv_cur_pyr - uv_cur_i; float2 subpix_uv_cur = uv_cur_pyr - uv_cur_i;
float w_cur_tl = (1.0 - subpix_uv_cur.x) * (1.0 - subpix_uv_cur.y); float w_cur_tl = (1.0 - subpix_uv_cur.x) * (1.0 - subpix_uv_cur.y);
float w_cur_tr = subpix_uv_cur.x * (1.0 - subpix_uv_cur.y); float w_cur_tr = subpix_uv_cur.x * (1.0 - subpix_uv_cur.y);
float w_cur_bl = (1.0 - subpix_uv_cur.x) * subpix_uv_cur.y; float w_cur_bl = (1.0 - subpix_uv_cur.x) * subpix_uv_cur.y;
float w_cur_br = subpix_uv_cur.x * subpix_uv_cur.y; float w_cur_br = subpix_uv_cur.x * subpix_uv_cur.y;
float e = 0.0; float e = 0.0;
float chi_=0.0; float chi_=0.0;
for(int y = 0; y < PATCH_SIZE; ++y) 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 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); 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) for(int x = 0; x < PATCH_SIZE; ++x, ++ref_element_addr, ++cur_element_addr)
{ {
// precompute interpolated reference patch color // 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_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_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_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]); 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]);
float value = w_ref_tl * read_imageui(image_ref, sampler, px_reftl).x + w_ref_tr * read_imageui(image_ref, sampler, px_reftr).x + float 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; w_ref_bl * read_imageui(image_ref, sampler, px_refbl).x + w_ref_br * read_imageui(image_ref, sampler, px_refbr).x;
float 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 + float 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_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_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_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_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_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_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)); 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));
float 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 + float 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_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_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_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_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_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_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)); 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 // 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_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_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_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]); 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]);
float res = value - w_cur_tl * read_imageui(image_cur, sampler, px_curtl).x + w_cur_tr * read_imageui(image_cur, sampler, px_curtr).x + float 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; 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 // used to compute scale for robust cost
e += fabs(res); e += fabs(res);
// robustification // robustification
float weight = res/scale_; //1.48f * vk::getMedian(errors) float weight = res/scale_; //1.48f * vk::getMedian(errors)
chi_ += pow(res,2) * weight; chi_ += pow(res,2) * weight;
float3 j_row0 = (float3)((dx * frame_jac[0]), (dx * frame_jac[1]), (dx * frame_jac[2])); float3 j_row0 = (float3)((dx * frame_jac[0]), (dx * frame_jac[1]), (dx * frame_jac[2]));
float3 j_row1 = (float3)((dy * frame_jac[3]), (dy * frame_jac[4]), (dy * frame_jac[5])); float3 j_row1 = (float3)((dy * frame_jac[3]), (dy * frame_jac[4]), (dy * frame_jac[5]));
float3 J = (j_row0 + j_row1) * (F_X / scale); float3 J = (j_row0 + j_row1) * ((float)F_X / scale);
compute_hessain(J, &Hessian[f*9], weight); compute_hessain(J, &Hessian[f*9], weight);
Jacobian[f] -= J * res * weight; Jacobian[f] -= J * res * weight;
} }
} }
errors[f] = e / pow(PATCH_SIZE,2.0); errors[f] = e / pow(PATCH_SIZE,2.0);
chi[f]=chi_; chi[f]=chi_;
} }
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch> <launch>
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/ --> <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/>
<!-- Camera topic to subscribe to --> <!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" /> <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/param/vo_fast.yaml
+1 -1
View File
@@ -5,7 +5,7 @@ svo:
quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad.
quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad. quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
init_min_tracked: 100 init_min_tracked: 100
init_min_disparity: 60 init_min_disparity: 40
structureoptim_max_pts: 50 structureoptim_max_pts: 50
kfselect_mindist: 0.01 kfselect_mindist: 0.01
poseoptim_thresh: 4.0 poseoptim_thresh: 4.0
GPU_version/vio/src/frame_handler_mono.cpp
+3 -3
View File
@@ -148,7 +148,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
map_.addKeyframe(new_frame_); map_.addKeyframe(new_frame_);
stage_ = STAGE_DEFAULT_FRAME; stage_ = STAGE_DEFAULT_FRAME;
klt_homography_init_->reset(); klt_homography_init_->reset();
SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map."); SVO_INFO_STREAM("Init: triangulated initial map. Vio is starting ...");
return RESULT_IS_KEYFRAME; return RESULT_IS_KEYFRAME;
} }
@@ -164,12 +164,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//assert (false); //assert (false);
reprojector_.reprojectMap(new_frame_, overlap_kfs_); reprojector_.reprojectMap(new_frame_, overlap_kfs_);
/*
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_ <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\t distance between two frames: "<< <<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
*/
size_t sfba_n_edges_final=0; size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final; double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton( pose_optimizer::optimizeGaussNewton(
GPU_version/vio/src/sparse_img_align_gpu.cpp
+5 -3
View File
@@ -128,8 +128,10 @@ int SparseImgAlignGpu::solve()
J[0].y -= J[i].y; J[0].y -= J[i].y;
J[0].z -= J[i].z; J[0].z -= J[i].z;
} }
double Hd[9]={}; double Hd[9]={(double)H[0],(double)H[1],(double)H[2],
double Jd[3]={}; (double)H[3],(double)H[4],(double)H[5],
(double)H[6],(double)H[7],(double)H[8]};
double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z};
x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2])); x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
if((bool) std::isnan((double) x_[0])) if((bool) std::isnan((double) x_[0]))
return 0; return 0;
@@ -140,7 +142,7 @@ void SparseImgAlignGpu::update()
/// TODO the update situation may have a smarter solution /// TODO the update situation may have a smarter solution
cl_float3 pos[1]={0.0,0.0,0.0}; cl_float3 pos[1]={0.0,0.0,0.0};
residual_->read(1,2,pos); 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_); Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-1.0*x_);
pos[0].x=(float)update.translation()(0); pos[0].x=(float)update.translation()(0);
pos[0].y=(float)update.translation()(1); pos[0].y=(float)update.translation()(1);
pos[0].z=(float)atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real()); pos[0].z=(float)atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());