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

debug kernel

Browse Source
This commit is contained in:
gfuse
2021-08-04 18:18:21 +02:00
parent bf9fed0c3e
commit 47e2993ed8
7 changed files with 108 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/atan_camera.h
+1
View File
@@ -14,6 +14,7 @@
#define ATAN_CAMERA_H_
#include <stdlib.h>
#include <iostream>
#include <string>
#include <Eigen/Eigen>
#include <vio/abstract_camera.h>
GPU_version/vio/kernel/compute-residual.cl
+78 -79
View File
@@ -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_;
}
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<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 -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/out.txt
BIN
View File
Binary file not shown.
GPU_version/vio/src/atan_camera.cpp
+4 -1
View File
@@ -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
GPU_version/vio/src/frame_handler_mono.cpp
+11 -10
View File
@@ -54,20 +54,20 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
gpu_fast_->write_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;
GPU_version/vio/src/sparse_img_align_gpu.cpp
+13 -16
View File
@@ -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<bool>::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<float> 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<double,3,3>(H).ldlt().solve(Eigen::Matrix<double,3,1>(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);