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_ #define ATAN_CAMERA_H_
#include <stdlib.h> #include <stdlib.h>
#include <iostream>
#include <string> #include <string>
#include <Eigen/Eigen> #include <Eigen/Eigen>
#include <vio/abstract_camera.h> #include <vio/abstract_camera.h>
GPU_version/vio/kernel/compute-residual.cl
+40 -41
View File
@@ -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 *(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 + 0) += j[0] * j[0] * w;
*(H + 1) += j[0] * j[1] * 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; *(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 + 0) -= j[0] * r * w;
*(Jacobian + 1) -= j[1] * r * w; *(Jacobian + 1) -= j[1] * r * w;
@@ -70,24 +70,24 @@ __kernel void compute_residual(
__global double * ref_pose,//[current 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 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 double2 * featue_px,
int level, __global int * level,
__global double * camera_model, // camera model [f_x,f_y,c_x,c_y,s_] __global double * camera_model, // camera model [f_x,f_y,c_x,c_y,s_]
__global float * errors, __global float * errors,
__global double * Hessian, __global double * Hessian,
__global double * Jacobian, __global double * Jacobian,
double chi __global double * chi
) )
{ {
const int PATCH_AREA_ = PATCH_SIZE * PATCH_SIZE; const int PATCH_AREA_ = PATCH_SIZE * PATCH_SIZE;
int border = PATCH_HALFSIZE + 1; int border = PATCH_HALFSIZE + 1;
int stride = get_image_dim(image_cur)[0]; //width, height 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; size_t n_meas_ = 0;
// 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 const 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
double u_ref = featue_px[f].x * scale; double u_ref = featue_px[f].x * scale;
@@ -102,8 +102,8 @@ __kernel void compute_residual(
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]); 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 // evaluate projection jacobian
double* frame_jac = (double *) malloc(6 * sizeof(double)); // 2X3 double frame_jac[6]; // 2X3
jacobian_xyz2uv(xyz_reference, frame_jac); jacobian_xyz2uv(xyz_reference, &frame_jac);
// compute bilateral interpolation weights for reference image // compute bilateral interpolation weights for reference image
double subpix_u_ref = u_ref - u_ref_i; 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; double w_ref_br = subpix_u_ref * subpix_v_ref;
size_t pixel_counter = 0; 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], 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; xyz_current[0], xyz_current[1], xyz_current[2]) * scale;
double u_cur = uv_cur_pyr[0]; double u_cur = uv_cur_pyr[0];
@@ -143,36 +143,36 @@ __kernel void compute_residual(
for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_element_addr, ++cur_element_addr) for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_element_addr, ++cur_element_addr)
{ {
// precompute interpolated reference patch color // 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_reftl = (int2)( 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_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 = ((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 = ((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]);
double value = w_ref_tl * read_imageui(image_ref, sampler, px_reftl).x + w_ref_tr * read_imageui(image_ref, sampler, px_reftr).x + 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; 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 + 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, ((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, ((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, ((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, ((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, ((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, ((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, ((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));
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 + 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, ((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, ((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, ((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, ((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, ((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, ((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, ((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 = ( cur_element_addr % get_image_dim(image_cur)[0], cur_element_add / get_image_dim(image_cur)[0]); 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_add + 1) / 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_add + (get_image_dim(image_cur)[0])) / 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_add + (get_image_dim(image_cur)[0]) + 1) / 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 + 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; w_cur_bl * read_imageui(image_cur, sampler, px_curbl).x + w_cur_br * read_imageui(image_cur, sampler, px_curbr).x;
@@ -180,7 +180,7 @@ __kernel void compute_residual(
// used to compute scale for robust cost // used to compute scale for robust cost
// if(compute_weight_scale) // if(compute_weight_scale)
errors[f * pow(PATCH_SIZE, 2) + (y * PATCH_SIZE) + x] = fabs(res); errors[(int)(f * pow((double)PATCH_SIZE, 2.0) + (y * PATCH_SIZE) + x)] = fabs(res);
// robustification // robustification
double weight = 2.0; double weight = 2.0;
@@ -188,21 +188,20 @@ __kernel void compute_residual(
// weight = weight_function_->value(res/scale_); // weight = weight_function_->value(res/scale_);
// } // }
*chi += res * res * weight; chi[0] += res * res * weight;
n_meas_++; n_meas_++;
// if(linearize_system) // if(linearize_system)
// { // {
// compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error) // 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_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_row1 = (double3)((dy * frame_jac[3]), (dy * frame_jac[4]), (dy * frame_jac[5]));
double3 J((j_row0 + j_row1) * (camera_model[0] / scale)); double3 J = ((j_row0 + j_row1) * (camera_model[0] / scale));
compute_hessain(J, Hessian, weight); // H_.noalias() += J * J.transpose() * weight; compute_hessain(J, Hessian, weight); // H_.noalias() += J * J.transpose() * weight;
compute_Jacobian(J, Jacobian, res, weight) // Jres_.noalias() -= J * res * weight; compute_Jacobian(J, Jacobian, res, weight); // Jres_.noalias() -= J * res * weight;
// } // }
} }
} }
*chi = *chi / n_meas_; chi[0] = chi[0] / n_meas_;
free(frame_jac);
} }
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/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; tans_ = 0.0;
distortion_ = false; distortion_ = false;
} }
param_ = (double *) malloc(5 * sizeof(double));
} }
ATANCamera:: ATANCamera::
~ATANCamera() ~ATANCamera()
{} {
delete param_;
}
Vector3d ATANCamera:: Vector3d ATANCamera::
cam2world(const double& x, const double& y) const 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); gpu_fast_->write_buf(1,4,300,f);
cl_double2 px[300]={0}; cl_double2 px[300]={0};
gpu_fast_->write_buf(1,5,300,px); gpu_fast_->write_buf(1,5,300,px);
int level=0; int level[1]={0};
gpu_fast_->write_buf(1,6,level); gpu_fast_->write_buf(1,6,1,level);
double c[5]={0}; double c[5]={0};
gpu_fast_->write_buf(1,7,5,c); gpu_fast_->write_buf(1,7,5,c);
float e[300]={0}; double e[300]={0};
gpu_fast_->write_buf(1,8,300,e); gpu_fast_->write_buf(1,8,300,e);
float H[6]={0}; double H[9]={0};
gpu_fast_->write_buf(1,9,6,H); gpu_fast_->write_buf(1,9,9,H);
float J[3]={0}; double J[3]={0};
gpu_fast_->write_buf(1,10,3,J); gpu_fast_->write_buf(1,10,3,J);
double chi2=0.0; double chi2[1]={0.0};
gpu_fast_->write_buf(1,11,chi2); gpu_fast_->write_buf(1,11,1,chi2);
imageAlign_=new SparseImgAlignGpu(Config::kltMaxLevel(), Config::kltMinLevel(), 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_); klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
initialize(); initialize();
} }
@@ -164,9 +164,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_=init_f.second; new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first; new_frame_->Cov_ = init_f.first;
// sparse image align // sparse image align
/*
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::LevenbergMarquardt, false, false); 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; if(imageAlign_->run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
reprojector_.reprojectMap(new_frame_, overlap_kfs_); reprojector_.reprojectMap(new_frame_, overlap_kfs_);
size_t sfba_n_edges_final=0; 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) size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
{ {
reset(); reset();
if(ref_frame->fts_.empty()) if(ref_frame->fts_.empty())
{ {
return 0; 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,2,cur_pos);
residual_->reload_buf(1,3,ref_pos); residual_->reload_buf(1,3,ref_pos);
residual_->reload_buf(1,7,camera); residual_->reload_buf(1,7,camera);
cl_double3* features; cl_double3 features[ref_frame->fts_.size()];
cl_double2* featue_px; cl_double2 featue_px[ref_frame->fts_.size()];
feature_counter_ = 0; // is used to compute the index of the cached jacobian 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){
for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end(); Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm();
++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());
features[feature_counter_].x=xyz_ref(0); features[feature_counter_].x=xyz_ref(0);
features[feature_counter_].y=xyz_ref(1); features[feature_counter_].y=xyz_ref(1);
features[feature_counter_].z=xyz_ref(2); features[feature_counter_].z=xyz_ref(2);
@@ -84,8 +79,8 @@ double SparseImgAlignGpu::computeResiduals(
bool linearize_system, bool linearize_system,
bool compute_weight_scale) bool compute_weight_scale)
{ {
double chi2=0.0; double chi2[1]={0};
residual_->reload_buf(1,11,&chi2); residual_->reload_buf(1,11,chi2);
/// TODO GPU compute based on the feature /// TODO GPU compute based on the feature
residual_->run(1,feature_counter_); residual_->run(1,feature_counter_);
if(compute_weight_scale && iter_ == 0) { if(compute_weight_scale && iter_ == 0) {
@@ -94,14 +89,16 @@ double SparseImgAlignGpu::computeResiduals(
std::vector<float> errors(error,error+feature_counter_); std::vector<float> errors(error,error+feature_counter_);
scale_ = scale_estimator_->compute(errors); 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() int SparseImgAlignGpu::solve()
{ {
double* H; double H[9]={0};
double* J; double J[3]={0};
residual_->read(1,9,H); residual_->read(1,9,H);
residual_->read(1,10,J); residual_->read(1,10,J);
x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Matrix<double,3,1>(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() void SparseImgAlignGpu::update()
{ {
/// TODO the update situation may have a smarter solution /// TODO the update situation may have a smarter solution
double* pos; double pos[3]={0};
residual_->read(1,2,pos); residual_->read(1,2,pos);
SE2 update = SE2(pos[2],Eigen::Vector2d(pos[0],pos[1])) * SE2::exp(-0.25*x_); SE2 update = SE2(pos[2],Eigen::Vector2d(pos[0],pos[1])) * SE2::exp(-0.25*x_);
pos[0]=update.translation()(0); pos[0]=update.translation()(0);