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

edit kernel

Browse Source
This commit is contained in:
gfuse
2021-08-18 08:11:35 +02:00
parent 941577314c
commit 7dc930d13e
5 changed files with 112 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/kernel/compute-residual.cl
+14
View File
@@ -35,6 +35,17 @@ void jacobian_xyz2uv(double3 xyz_in_f, double* J)
*(J + 4) = (xyz_in_f[1]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // 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 jacobian_xyz2uv_2(double3 xyz_in_f, double3 ref_pose, double3 cur_pose, double* J)
{
double3 txtytetha = cur_pose - ref_pose;
double2 uv = world2cam(xyz_in_f);
*(J + 0) = 1.0;
*(J + 1) = 0.0;
*(J + 2) = ((-1 * sin(txtytetha[2]) * cos(ref_pose[2])) - (cos(txtytetha[2]) * sin(ref_pose[2]))) * uv[0] + ((cos(txtytetha[2]) * cos(ref_pose[2])) - (sin(txtytetha[2]) * sin(ref_pose[2]))) * uv[1];
*(J + 3) = 0.0;
*(J + 4) = 1.0;
*(J + 5) = ((-1 * cos(txtytetha[2]) * cos(ref_pose[2])) + (sin(txtytetha[2]) * sin(ref_pose[2]))) * uv[0] - ((sin(txtytetha[2]) * cos(ref_pose[2])) + (cos(txtytetha[2]) * sin(ref_pose[2]))) * uv[1];
}
void compute_hessain(double3 j, __global double* H, double w)
{
*(H + 0) += j[0] * j[0] * w;
@@ -73,8 +84,11 @@ __kernel void compute_residual(
double2 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;
// evaluate projection jacobian
//if(f==0)
// printf("ref_feature[f]: %f %f %f\n", ref_feature[f].x, ref_feature[f].y, ref_feature[f].z);
double frame_jac[6]={0.0}; // 2X3
jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (double3)(ref_pose[0].x,0.0,ref_pose[0].y), 2.0)), &frame_jac);
//jacobian_xyz2uv_2(ref_feature[f], ref_pose[0], cur_pose[0], &frame_jac);
// compute bilateral interpolation weights for reference image
double2 subpix= uv_ref - uv_ref_i;
GPU_version/vio/src/frame_handler_mono.cpp
+9 -9
View File
@@ -48,20 +48,20 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
gpu_fast_->make_kernel("compute_residual");
gpu_fast_->write_buf(1,0,img);
gpu_fast_->write_buf(1,1,img);
cl_double3 pose[1]={0.0};
cl_float3 pose[1]={0.0};
gpu_fast_->write_buf(1,2,3,pose);
gpu_fast_->write_buf(1,3,3,pose);
cl_double3 f[300]={0};
cl_float3 f[300]={0};
gpu_fast_->write_buf(1,4,300,f);
cl_double2 px[300]={0};
cl_float2 px[300]={0};
gpu_fast_->write_buf(1,5,300,px);
double e[300]={0};
float e[300]={0};
gpu_fast_->write_buf(1,7,300,e);
double H[9*300]={0};
float H[9*300]={0};
gpu_fast_->write_buf(1,8,9*300,H);
cl_double3 J[300];
cl_float3 J[300];
gpu_fast_->write_buf(1,9,300,J);
double chi2[1]={0.01};
float chi2[1]={0.01};
gpu_fast_->write_buf(1,10,1,chi2);
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
initialize();
@@ -164,12 +164,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//assert (false);
reprojector_.reprojectMap(new_frame_, overlap_kfs_);
/*
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
*/
size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
GPU_version/vio/src/reprojector.cpp
+1 -1
View File
@@ -206,7 +206,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
{
Vector2d px(frame->w2c(point->pos_));
if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
if(frame->cam_->isInFrame(px.cast<int>(), 3)) // 8px is the patch size in the matcher
{
const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols
+ static_cast<int>(px[0]/grid_.cell_size);
GPU_version/vio/src/sparse_img_align_gpu.cpp
+4 -4
View File
@@ -40,12 +40,12 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
{
return 0;
}
cl_double3 ref_pos[1]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()};
cl_double3 cur_pos[1]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()};
cl_float3 ref_pos[1]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()};
cl_float3 cur_pos[1]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()};
residual_->reload_buf(1,2,cur_pos);
residual_->reload_buf(1,3,ref_pos);
cl_double3 features[ref_frame->fts_.size()];
cl_double2 featue_px[ref_frame->fts_.size()];
cl_float3 features[ref_frame->fts_.size()];
cl_float2 featue_px[ref_frame->fts_.size()];
// fprintf(data, "ref_frame:\n %f,%f,%f\n",ref_pos[0],ref_pos[1],ref_pos[2]);
// fprintf(data, "cur_frame:\n %f,%f,%f\n",cur_pos[0],cur_pos[1],cur_pos[2]);
// fprintf(data, "camera:\n %f,%f,%f,%f,%f\n features:\n",camera[0],camera[1],camera[2],camera[3],camera[4]);
GPU_version/vio/src/vo_node.cpp
+1 -1
View File
@@ -13,7 +13,7 @@
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#define VIO_DEBUG false
#define VIO_DEBUG true
#include <ros/ros.h>
#include <string>
#include <chrono>