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

moved proj. jacobian out of loop.

Browse Source
This commit is contained in:
Christian Forster
2014-06-27 14:57:03 +02:00
parent d6da40f6a0
commit ec351f478f
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/src/sparse_img_align.cpp
+4 -4
View File
@@ -107,6 +107,10 @@ void SparseImgAlign::precomputeReferencePatches()
const double depth(((*it)->point->pos_ - ref_pos).norm()); const double depth(((*it)->point->pos_ - ref_pos).norm());
const Vector3d xyz_ref((*it)->f*depth); const Vector3d xyz_ref((*it)->f*depth);
// evaluate projection jacobian
Matrix<double,2,6> frame_jac;
Frame::jacobian_xyz2uv(xyz_ref, frame_jac);
// compute bilateral interpolation weights for reference image // compute bilateral interpolation weights for reference image
const float subpix_u_ref = u_ref-u_ref_i; const float subpix_u_ref = u_ref-u_ref_i;
const float subpix_v_ref = v_ref-v_ref_i; const float subpix_v_ref = v_ref-v_ref_i;
@@ -131,10 +135,6 @@ void SparseImgAlign::precomputeReferencePatches()
float dy = 0.5f * ((w_ref_tl*ref_img_ptr[stride] + w_ref_tr*ref_img_ptr[1+stride] + w_ref_bl*ref_img_ptr[stride*2] + w_ref_br*ref_img_ptr[stride*2+1]) float dy = 0.5f * ((w_ref_tl*ref_img_ptr[stride] + w_ref_tr*ref_img_ptr[1+stride] + w_ref_bl*ref_img_ptr[stride*2] + w_ref_br*ref_img_ptr[stride*2+1])
-(w_ref_tl*ref_img_ptr[-stride] + w_ref_tr*ref_img_ptr[1-stride] + w_ref_bl*ref_img_ptr[0] + w_ref_br*ref_img_ptr[1])); -(w_ref_tl*ref_img_ptr[-stride] + w_ref_tr*ref_img_ptr[1-stride] + w_ref_bl*ref_img_ptr[0] + w_ref_br*ref_img_ptr[1]));
// evaluate jacobian and cache
Matrix<double,2,6> frame_jac;
Frame::jacobian_xyz2uv(xyz_ref, frame_jac);
// cache the jacobian // cache the jacobian
jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) = jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) =
(dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_)); (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));