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

Fixed some bugs and got better results

Browse Source
This commit is contained in:
VahdPourAmin
2021-09-12 21:48:50 +02:00
parent 827524a3d0
commit 604ae26297
5 changed files with 108 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/sparse_img_align_gpu.h
+2 -1
View File
@@ -52,7 +52,8 @@ public:
size_t run(
FramePtr ref_frame,
FramePtr cur_frame);
FramePtr cur_frame,
FILE* log);
// FILE* data= nullptr;
GPU_version/vio/kernel/compute-residual.cl
+43 -11
View File
@@ -43,12 +43,43 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
void jacobian_xyz2uv(float3 xyz_in_f, float* J)
{
J[0] = -(1. / xyz_in_f.z); // -1/z
J[0] = -(1. / xyz_in_f.z); // -1/z
J[1] = (xyz_in_f.x) / (xyz_in_f.z * xyz_in_f.z); // x/z^2
J[2] = -(1.0 + pow((xyz_in_f.x),2) / (xyz_in_f.z * xyz_in_f.z)); // -(1.0 + x^2/z^2)
J[3] = 1e-19; // 0
J[3] = 1e-19; // 0
J[4] = (xyz_in_f.y) / (xyz_in_f.z * xyz_in_f.z); // y/z^2
J[5] = -(xyz_in_f.x) * (xyz_in_f.y) / (xyz_in_f.z * xyz_in_f.z); // -x*y/z^2
J[5] = -(xyz_in_f.x) * (xyz_in_f.y) / (xyz_in_f.z * xyz_in_f.z); // -x*y/z^2
}
void jacobian_xyz2uv_(float3 xyz_in_f, float3 cur_p, float* J)
{
double x_n = xyz_in_f.x;
double y_n = xyz_in_f.y;
double z_n = xyz_in_f.z;
double r = sqrt(pow(xyz_in_f.x/xyz_in_f.z, 2) + pow(xyz_in_f.y/xyz_in_f.z, 2));
double x_c = cur_p.x;
double z_c = cur_p.y;
double theta = cur_p.z;
double alpha = (F_X*(theta/r))-(F_X*((x_n*x_n)/(r*r))*theta)+((1+3*S*theta*theta)/((r*r)+1))*((F_X*x_n*x_n)/(r*r));
double beta = -(F_X*((x_n*y_n)/(r*r))*theta)+((1+3*S*theta*theta)/((r*r)+1))*((F_X*x_n*y_n)/(r*r));
double gamma = -(F_Y*((x_n*y_n)/(r*r))*theta)+((1+3*S*theta*theta)/((r*r)+1))*((F_Y*x_n*y_n)/(r*r));
double lamda = (F_Y*(theta/r))-(F_Y*((y_n*y_n)/(r*r))*theta)+((1+3*S*theta*theta)/((r*r)+1))*((F_Y*y_n*y_n)/(r*r));
double Xf_Xc = x_n - x_c;
double Zf_Zc = z_n - z_c;
double n1 = -1*sin(theta)*Xf_Xc + cos(theta)*Zf_Zc;
double n2 = -1*cos(theta)*Xf_Xc - sin(theta)*Zf_Zc;
J[0] = ((-1*cos(theta)/z_n)*alpha)-((sin(theta)/z_n)*beta)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*n1);
J[1] = 0;
J[2] = ((sin(theta)/z_n)*alpha)-((cos(theta)/z_n)*beta)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*n2);
J[3] = ((-1*cos(theta)/z_n)*gamma)-((sin(theta)/z_n)*lamda)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n1);
J[4] = 0;
J[5] = ((sin(theta)/z_n)*gamma)-((cos(theta)/z_n)*lamda)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n2);
}
void compute_hessain(float3 j, __global float* H, float w)
@@ -68,15 +99,15 @@ __kernel void compute_residual(
__read_only image2d_t image_cur, // current frame
__read_only image2d_t image_ref, // reference frame
__global float3 * cur_pose,//[reference 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 float2 * featue_px,
int level,
__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 float2 * featue_px,
int level,
__global float * errors,
__global float * Hessian,
__global float * Hessian,
__global float3 * Jacobian,
__global float * chi,
float scale_
__global float * chi,
float scale_
)
{
float scale = pow(2.0, -level);
@@ -90,7 +121,8 @@ 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).x || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref).y)return;
// evaluate projection jacobian
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);
jacobian_xyz2uv_(ref_feature[f], cur_pose[0], &frame_jac);
// compute bilateral interpolation weights for reference image
float2 subpix= uv_ref - uv_ref_i;
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/src/frame_handler_mono.cpp
+45 -1
View File
@@ -176,7 +176,51 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->Cov_ = init_f.first;
// sparse image align
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
if(!img_align.run(last_frame_, new_frame_))new_frame_->T_f_w_=init_f.second;
/*#if VIO_DEBUG
fprintf(log_,"[%s] Before alignment, distance between two frames x:%f ,z=%f,angle between two frames:%f last_frame feature size : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()), last_frame_->fts_.size());
int nullcount = 0;
int nullpointcount = 0;
for(auto f = last_frame_->fts_.begin(); f!= last_frame_->fts_.end(); f++)
{
if(*f == nullptr) {
nullcount++;
continue;
}
if((*f)->point == nullptr) {
nullpointcount++;
continue;
}
fprintf(log_,"[%s] last frame : %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
(*f)->point->pos_.x(), (*f)->point->pos_.y(), (*f)->point->pos_.z());
}
fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
#endif*/
if(!img_align.run(last_frame_, new_frame_, log_))new_frame_->T_f_w_=init_f.second;
/*#if VIO_DEBUG
fprintf(log_,"[%s] After alignment, distance between two frames x:%f ,z=%f,angle between two frames:%f last_frame feature size : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()), last_frame_->fts_.size());
nullcount = 0;
nullpointcount = 0;
for(auto f = last_frame_->fts_.begin(); f!= last_frame_->fts_.end(); f++)
{
if(*f == nullptr) {
nullcount++;
continue;
}
if((*f)->point == nullptr) {
nullpointcount++;
continue;
}
fprintf(log_,"[%s] last frame : %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
(*f)->point->pos_.x(), (*f)->point->pos_.y(), (*f)->point->pos_.z());
}
fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
#endif*/
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second;
GPU_version/vio/src/sparse_img_align_gpu.cpp
+17 -5
View File
@@ -32,15 +32,23 @@ SparseImgAlignGpu::SparseImgAlignGpu(
eps_ = 1e-10;
}
size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
{
reset();
if(ref_frame->fts_.empty())
/*if(ref_frame->fts_.empty()) // more than 10
{
return 0;
}
}*/
cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()};
/*#if VIO_DEBUG
fprintf(log,"[%s] ref pose : , x:%f, z=%f, pitch:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
ref_pos[0].x, ref_pos[0].y, ref_pos[0].z);
#endif*/
cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()};
/*#if VIO_DEBUG
fprintf(log,"[%s] cur pose : , x:%f, z=%f, pitch:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
cur_pos[0].x, cur_pos[0].y, cur_pos[0].z);
#endif*/
residual_->reload_buf(1,2,cur_pos);
residual_->reload_buf(1,3,ref_pos);
cl_float3 features[ref_frame->fts_.size()];
@@ -57,6 +65,10 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
featue_px[feature_counter_].y=(*it)->px(1);
++feature_counter_;
}
if(!feature_counter_) // more than 10
{
return 0;
}
residual_->reload_buf(1,4,features);
residual_->reload_buf(1,5,featue_px);
SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it
@@ -67,8 +79,8 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
residual_->reload_buf(1,0,cur_img);
residual_->reload_buf(1,1,ref_img);
residual_->write_buf(1,6,level_);
mu_ = 1.0;
optimize(T_cur);
mu_ = 1.0;
optimize(T_cur);
}
cl_float3 pos[1]={0};
residual_->read(1,2,1,pos);