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

add mssim error and apply image aliginment on all close frames

Browse Source
This commit is contained in:
gfuse
2021-10-06 13:50:48 +02:00
parent 17ad651f02
commit 3a9bdaf77b
12 changed files with 980 additions and 933 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+122 -123
View File
@@ -1,123 +1,122 @@
[11:06:58.541] New key frame [13:47:32.811] New key frame
[11:06:58.541] BA loop run [13:47:32.811] BA loop run
[11:06:58.542] init error: 111.169209 [13:47:32.814] init error: 9.234706
[11:06:58.543] end error: 0.522741 [13:47:32.820] end error: 0.008980
[11:06:58.674] New key frame [13:47:32.946] New key frame
[11:06:58.674] BA loop run [13:47:32.946] BA loop run
[11:06:58.674] init error: 1535.405047 [13:47:32.948] init error: 27.389863
[11:06:58.675] end error: 0.000000 [13:47:32.954] end error: 2.236662
[11:06:58.782] New key frame [13:47:33.781] New key frame
[11:06:58.782] BA loop run [13:47:33.781] BA loop run
[11:06:58.783] init error: 2166.366615 [13:47:33.783] init error: 94.643184
[11:06:58.783] end error: 0.000000 [13:47:33.789] end error: 3.372762
[11:06:58.895] New key frame [13:47:33.984] New key frame
[11:06:58.895] BA loop run [13:47:33.984] BA loop run
[11:06:58.895] init error: 1969008.499946 [13:47:33.987] init error: 625.435993
[11:06:58.896] end error: 13.098446 [13:47:33.994] end error: 5.147586
[11:06:59.011] New key frame [13:47:34.284] New key frame
[11:06:59.011] BA loop run [13:47:34.284] BA loop run
[11:06:59.011] init error: 48.044026 [13:47:34.287] init error: 107.819050
[11:06:59.012] end error: 0.000311 [13:47:34.295] end error: 10.211351
[11:06:59.127] New key frame [13:47:34.480] New key frame
[11:06:59.127] BA loop run [13:47:34.480] BA loop run
[11:06:59.128] init error: 104.907537 [13:47:34.483] init error: 748.273313
[11:06:59.129] end error: 0.029171 [13:47:34.496] end error: 191.560372
[11:06:59.244] New key frame [13:47:34.712] New key frame
[11:06:59.244] BA loop run [13:47:34.712] BA loop run
[11:06:59.244] init error: 29.231466 [13:47:34.716] init error: 23103.095029
[11:06:59.246] end error: 0.185819 [13:47:34.728] end error: 30.581954
[11:06:59.359] New key frame [13:47:34.998] New key frame
[11:06:59.359] BA loop run [13:47:34.998] BA loop run
[11:06:59.360] init error: 94.753494 [13:47:35.003] init error: 1685.467973
[11:06:59.361] end error: 0.154880 [13:47:35.027] end error: 29.128400
[11:06:59.467] New key frame [13:47:35.379] New key frame
[11:06:59.467] BA loop run [13:47:35.379] BA loop run
[11:06:59.468] init error: 19.887575 [13:47:35.385] init error: 450.889359
[11:06:59.470] end error: 0.184039 [13:47:35.409] end error: 75.097102
[11:06:59.687] New key frame [13:47:35.820] New key frame
[11:06:59.687] BA loop run [13:47:35.820] BA loop run
[11:06:59.688] init error: 749.082843 [13:47:35.829] init error: 673.634333
[11:06:59.689] end error: 0.267130 [13:47:36.205] end error: 72.631437
[11:06:59.922] New key frame [13:47:36.434] New key frame
[11:06:59.923] BA loop run [13:47:36.434] BA loop run
[11:06:59.923] init error: 68.437401 [13:47:36.443] init error: 364.392224
[11:06:59.927] end error: 0.030856 [13:47:36.814] end error: 930.000115
[11:07:00.046] New key frame [13:47:37.109] New key frame
[11:07:00.046] BA loop run [13:47:37.109] BA loop run
[11:07:00.047] init error: 168.228993 [13:47:37.121] init error: 6323.348385
[11:07:00.050] end error: 0.680629 [13:47:37.405] end error: 102.211472
[11:07:00.177] New key frame [13:47:37.644] New key frame
[11:07:00.177] BA loop run [13:47:37.644] BA loop run
[11:07:00.178] init error: 226.456550 [13:47:37.655] init error: 1095.200795
[11:07:00.186] end error: 1.624141 [13:47:38.210] end error: 96.963625
[11:07:00.322] New key frame [13:47:38.393] New key frame
[11:07:00.322] BA loop run [13:47:38.393] BA loop run
[11:07:00.324] init error: 364.297420 [13:47:38.406] init error: 148174.041099
[11:07:00.331] end error: 6.241724 [13:47:38.653] end error: 622.209864
[11:07:00.592] New key frame [13:47:38.925] New key frame
[11:07:00.592] BA loop run [13:47:38.925] BA loop run
[11:07:00.595] init error: 2947.304509 [13:47:38.937] init error: 2647.081480
[11:07:00.605] end error: 14.801503 [13:47:39.260] end error: 116.262759
[11:07:00.750] New key frame [13:47:39.532] New key frame
[11:07:00.750] BA loop run [13:47:39.532] BA loop run
[11:07:00.753] init error: 173.564319 [13:47:39.545] init error: 9444.067840
[11:07:00.765] end error: 14.556827 [13:47:40.300] end error: 772.068988
[11:07:00.904] New key frame [13:47:40.348] New key frame
[11:07:00.904] BA loop run [13:47:40.348] BA loop run
[11:07:00.906] init error: 53039.896271 [13:47:40.381] init error: 1961.781686
[11:07:00.914] end error: 25.636613 [13:47:41.300] end error: 504.437870
[11:07:01.073] New key frame [13:47:41.315] New key frame
[11:07:01.073] BA loop run [13:47:41.315] BA loop run
[11:07:01.076] init error: 99896.971378 [13:47:41.331] init error: 55097.105814
[11:07:01.087] end error: 46.032739 [13:47:42.081] end error: 885.378834
[11:07:01.568] New key frame [13:47:42.241] New key frame
[11:07:01.568] BA loop run [13:47:42.241] BA loop run
[11:07:01.572] init error: 9966032.579275 [13:47:42.254] init error: 1434.417662
[11:07:01.587] end error: 95.730038 [13:47:43.180] end error: 149.152293
[11:07:03.035] New key frame [13:47:43.188] New key frame
[11:07:03.035] BA loop run [13:47:43.189] BA loop run
[11:07:03.040] init error: 606.022870 [13:47:43.213] init error: 14500.734445
[11:07:03.054] end error: 22.251060 [13:47:43.549] end error: 1186.435667
[11:07:03.288] New key frame [13:47:43.875] New key frame
[11:07:03.288] BA loop run [13:47:43.875] BA loop run
[11:07:03.294] init error: 10166.284486 [13:47:43.888] init error: 53845.436036
[11:07:03.310] end error: 14.188260 [13:47:44.139] end error: 504.437132
[11:07:03.800] New key frame [13:47:44.462] New key frame
[11:07:03.801] BA loop run [13:47:44.462] BA loop run
[11:07:03.808] init error: 3525.805163 [13:47:44.477] init error: 44036.338866
[11:07:03.829] end error: 20.503755 [13:47:44.837] end error: 292.357080
[11:07:04.084] New key frame [13:47:45.095] New key frame
[11:07:04.084] BA loop run [13:47:45.095] BA loop run
[11:07:04.088] init error: 14189.806864 [13:47:45.109] init error: 883.006827
[11:07:04.102] end error: 216.837482 [13:47:45.410] end error: 156.079028
[11:07:04.342] New key frame [13:47:45.636] New key frame
[11:07:04.342] BA loop run [13:47:45.637] BA loop run
[11:07:04.348] init error: 4400.882280 [13:47:45.656] init error: 2310.251462
[11:07:04.367] end error: 37.015205 [13:47:45.821] end error: 177.760928
[11:07:04.683] New key frame [13:47:46.218] New key frame
[11:07:04.683] BA loop run [13:47:46.218] BA loop run
[11:07:04.689] init error: 19475.042082 [13:47:46.231] init error: 46169.589454
[11:07:04.707] end error: 61.342958 [13:47:46.279] end error: 160.495468
[11:07:06.465] New key frame [13:47:46.701] New key frame
[11:07:06.465] BA loop run [13:47:46.701] BA loop run
[11:07:06.471] init error: 6078.668617 [13:47:46.723] init error: 133197.234069
[11:07:06.487] end error: 88.529552 [13:47:46.768] end error: 261.644316
[11:07:06.641] New key frame [13:47:47.175] New key frame
[11:07:06.641] BA loop run [13:47:47.176] BA loop run
[11:07:06.649] init error: 2713.465438 [13:47:47.193] init error: 1843.110346
[11:07:06.664] end error: 56.754345 [13:47:47.257] end error: 217.782825
[11:07:06.781] New key frame [13:47:47.619] New key frame
[11:07:06.781] BA loop run [13:47:47.619] BA loop run
[11:07:06.787] init error: 8196.651295 [13:47:47.641] init error: 3208.125969
[11:07:06.803] end error: 20.070581 [13:47:48.349] end error: 203.439628
[11:07:06.921] New key frame [13:47:48.588] New key frame
[11:07:06.922] BA loop run [13:47:48.588] BA loop run
[11:07:06.928] init error: 42486.824606 [13:47:48.609] init error: 738.226202
[11:07:06.943] end error: 13.026887 [13:47:49.264] end error: 218.201207
[11:07:07.048] New key frame [13:47:49.406] New key frame
[11:07:07.048] BA loop run [13:47:49.406] BA loop run
[11:07:07.054] init error: 15360.510680 [13:47:49.423] init error: 130088.351726
[11:07:07.068] end error: 20.866226 [13:47:49.477] end error: 168.185621
[11:07:07.179] New key frame [13:47:49.836] New key frame
[11:07:07.179] BA loop run [1
[11:0
GPU_version/vio/frame_handler_log.txt
+767 -728
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/frame_handler_mono.h
-1
View File
@@ -24,7 +24,6 @@
#include <vio/initialization.h> #include <vio/initialization.h>
#include <vio/ukf.h> #include <vio/ukf.h>
#include <vio/cl_class.h> #include <vio/cl_class.h>
#include <vio/sparse_img_align_gpu.h>
#include <vio/global_optimizer.h> #include <vio/global_optimizer.h>
namespace vio { namespace vio {
GPU_version/vio/include/vio/map.h
+21 -1
View File
@@ -22,6 +22,8 @@
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <vio/global.h> #include <vio/global.h>
#include <vio/point.h> #include <vio/point.h>
#include <vio/frame.h>
#include <vio/feature.h>
namespace vio { namespace vio {
@@ -54,7 +56,25 @@ public:
void addKeyframe(FramePtr new_keyframe); void addKeyframe(FramePtr new_keyframe);
/// Given a frame, return all keyframes which have an overlapping field of view. /// Given a frame, return all keyframes which have an overlapping field of view.
void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const; void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs){
for(auto kf : keyframes_)
{
// check if kf has overlaping field of view with frame, use therefore KeyPoints
for(auto keypoint : kf->key_pts_)
{
if(keypoint == nullptr)
continue;
if(keypoint->point==NULL)continue;
if(frame->isVisible(keypoint->point->pos_))
{
close_kfs.push_back(
std::make_pair(
kf, (frame->T_f_w_.se2().translation()-kf->T_f_w_.se2().translation()).norm()));
break; // this keyframe has an overlapping field of view -> add to close_kfs
}
}
}
}
/// Return the keyframe which is furthest apart from pos. /// Return the keyframe which is furthest apart from pos.
FramePtr getFurthestKeyframe(const Vector2d& pos); FramePtr getFurthestKeyframe(const Vector2d& pos);
GPU_version/vio/include/vio/matcher.h
+1
View File
@@ -93,6 +93,7 @@ public:
uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16))); uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16)));
uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16))); uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
uint8_t patch_with_border_cur_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
Matrix2d A_cur_ref_; //!< affine warp matrix Matrix2d A_cur_ref_; //!< affine warp matrix
Vector2d epi_dir_; Vector2d epi_dir_;
double epi_length_; //!< length of epipolar line segment in pixels (only used for epipolar search) double epi_length_; //!< length of epipolar line segment in pixels (only used for epipolar search)
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/src/feature_alignment.cpp
+1 -1
View File
@@ -202,7 +202,7 @@ bool align2D(
bool sign=true;// + bool sign=true;// +
// termination condition // termination condition
const float min_update_squared = 1.0;//0.001 const float min_update_squared = 0.1;//0.001
const int cur_step = cur_img.step.p[0]; const int cur_step = cur_img.step.p[0];
// float chi2 = 0; // float chi2 = 0;
Vector3f update; update.setZero(); Vector3f update; update.setZero();
GPU_version/vio/src/frame_handler_mono.cpp
-17
View File
@@ -21,7 +21,6 @@
#include <vio/feature.h> #include <vio/feature.h>
#include <vio/point.h> #include <vio/point.h>
#include <vio/pose_optimizer.h> #include <vio/pose_optimizer.h>
#include <vio/sparse_img_align.h>
#include <vio/global_optimizer.h> #include <vio/global_optimizer.h>
#include <vio/for_it.hpp> #include <vio/for_it.hpp>
#include <assert.h> #include <assert.h>
@@ -145,22 +144,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
auto init_f= ukfPtr_.get_location(); auto init_f= ukfPtr_.get_location();
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
std::unique_ptr<SparseImgAlignGpu> img_align=std::make_unique<SparseImgAlignGpu>(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
size_t err=img_align->run(last_frame_, new_frame_, log_);
if(err==-1){
#if VIO_DEBUG
fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
new_frame_=last_frame_;
return RESULT_FAILURE;
}else if(err==0){
#if VIO_DEBUG
fprintf(log_,"[%s] Image Alignment failed 0\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
new_frame_=last_frame_;
return RESULT_FAILURE;
}
reprojector_.reprojectMap(new_frame_, last_frame_,overlap_kfs_, gpu_fast_, log_); reprojector_.reprojectMap(new_frame_, last_frame_,overlap_kfs_, gpu_fast_, log_);
int n_point=0; int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second; for(auto i:overlap_kfs_)n_point+=i.second;
GPU_version/vio/src/map.cpp
-23
View File
@@ -103,29 +103,6 @@ void Map::addKeyframe(FramePtr new_keyframe)
keyframes_.push_back(new_keyframe); keyframes_.push_back(new_keyframe);
} }
void Map::getCloseKeyframes(
const FramePtr& frame,
std::list< std::pair<FramePtr,double> >& close_kfs) const
{
for(auto kf : keyframes_)
{
// check if kf has overlaping field of view with frame, use therefore KeyPoints
for(auto keypoint : kf->key_pts_)
{
if(keypoint == nullptr)
continue;
if(keypoint->point==NULL)continue;
if(frame->isVisible(keypoint->point->pos_))
{
close_kfs.push_back(
std::make_pair(
kf, (frame->T_f_w_.se2().translation()-kf->T_f_w_.se2().translation()).norm()));
break; // this keyframe has an overlapping field of view -> add to close_kfs
}
}
}
}
FramePtr Map::getFurthestKeyframe(const Vector2d& pos) FramePtr Map::getFurthestKeyframe(const Vector2d& pos)
{ {
GPU_version/vio/src/matcher.cpp
+45 -15
View File
@@ -126,6 +126,42 @@ void Matcher::createPatchFromPatchWithBorder()
ref_patch_ptr[x] = ref_patch_border_ptr[x]; ref_patch_ptr[x] = ref_patch_border_ptr[x];
} }
} }
cv::Scalar getMSSIM( const cv::Mat& i1, const cv::Mat& i2){
const double C1 = 6.5025, C2 = 58.5225;
/**************************** INITS *********************************/
int d = CV_32F;
cv::Mat I1, I2;
i1.convertTo(I1, d); // cannot calculate on one byte large values
i2.convertTo(I2, d);
cv::Mat I2_2 = I2.mul(I2); // I2^2
cv::Mat I1_2 = I1.mul(I1); // I1^2
cv::Mat I1_I2 = I1.mul(I2); // I1 * I2
/************************** END INITS *********************************/
cv::Mat mu1, mu2; // PRELIMINARY COMPUTING
GaussianBlur(I1, mu1, cv::Size(11, 11), 1.5);
GaussianBlur(I2, mu2, cv::Size(11, 11), 1.5);
cv::Mat mu1_2 = mu1.mul(mu1);
cv::Mat mu2_2 = mu2.mul(mu2);
cv::Mat mu1_mu2 = mu1.mul(mu2);
cv::Mat sigma1_2, sigma2_2, sigma12;
GaussianBlur(I1_2, sigma1_2, cv::Size(11, 11), 1.5);
sigma1_2 -= mu1_2;
GaussianBlur(I2_2, sigma2_2, cv::Size(11, 11), 1.5);
sigma2_2 -= mu2_2;
GaussianBlur(I1_I2, sigma12, cv::Size(11, 11), 1.5);
sigma12 -= mu1_mu2;
cv::Mat t1, t2, t3;
t1 = 2 * mu1_mu2 + C1;
t2 = 2 * sigma12 + C2;
t3 = t1.mul(t2); // t3 = ((2*mu1_mu2 + C1).*(2*sigma12 + C2))
t1 = mu1_2 + mu2_2 + C1;
t2 = sigma1_2 + sigma2_2 + C2;
t1 = t1.mul(t2); // t1 =((mu1_2 + mu2_2 + C1).*(sigma1_2 + sigma2_2 + C2))
cv::Mat ssim_map;
divide(t3, t1, ssim_map); // ssim_map = t3./t1;
cv::Scalar mssim = mean( ssim_map ); // mssim = average of ssim map
return mssim;
}
bool Matcher::findMatchDirect( bool Matcher::findMatchDirect(
const Point& pt, const Point& pt,
@@ -163,26 +199,20 @@ bool Matcher::findMatchDirect(
ref_ftr_->level, ref_ftr_->level, halfpatch_size_+1, patch_with_border_))return false; ref_ftr_->level, ref_ftr_->level, halfpatch_size_+1, patch_with_border_))return false;
createPatchFromPatchWithBorder(); createPatchFromPatchWithBorder();
// px_cur should be set // px_cur should be set
Vector2d px_scaled(px_cur);
bool success = false; bool success = false;
if(ref_ftr_->type == Feature::EDGELET) if(!warp::warpAffine(A_cur_ref_.inverse(), cur_frame.img_pyr_[ref_ftr_->level], px_cur,
{ ref_ftr_->level, ref_ftr_->level, halfpatch_size_+1, patch_with_border_cur_))return false;
Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); cv::Mat Patch_ref = cv::Mat(patch_size_+2, patch_size_+2, CV_8UC1, patch_with_border_);
dir_cur.normalize(); cv::Mat Patch_cur = cv::Mat(patch_size_+2, patch_size_+2, CV_8UC1, patch_with_border_cur_);
success = feature_alignment::align1D( cv::Scalar res = getMSSIM(Patch_cur, Patch_ref);
cur_frame.img_pyr_[ref_ftr_->level], dir_cur.cast<float>(), if(res[0] >= 0.75)
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); success = true;
}
else else
{ success = false;
success = feature_alignment::align2D(
cur_frame.img_pyr_[ref_ftr_->level], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
}
/* debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px, /* debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px,
px_cur,px_scaled ,success,patch_,patch_with_border_,(ref_ftr_->frame->se3().inverse()*pt.pos_).z());*/ px_cur,px_scaled ,success,patch_,patch_with_border_,(ref_ftr_->frame->se3().inverse()*pt.pos_).z());*/
px_cur = px_scaled; //px_cur = px_scaled;
return success; return success;
} }
GPU_version/vio/src/reprojector.cpp
+7 -4
View File
@@ -29,7 +29,7 @@
#include <vio/timer.h> #include <vio/timer.h>
#include <fstream> #include <fstream>
#include <vio/feature_detection.h> #include <vio/feature_detection.h>
#include <vio/sparse_img_align_gpu.h>
#include <opencv2/xfeatures2d.hpp> #include <opencv2/xfeatures2d.hpp>
#include <opencv2/features2d.hpp> #include <opencv2/features2d.hpp>
#include <vio/for_it.hpp> #include <vio/for_it.hpp>
@@ -81,11 +81,8 @@ namespace vio {
std::vector<cv::KeyPoint> keypoints_cur; std::vector<cv::KeyPoint> keypoints_cur;
list<std::shared_ptr<Feature>>::iterator it_cur=keypoints.begin(); list<std::shared_ptr<Feature>>::iterator it_cur=keypoints.begin();
cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1); cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1);
cv::Mat maskup = cv::Mat_<uchar>(1, keypoints.size()); cv::Mat maskup = cv::Mat_<uchar>(1, keypoints.size());
cv::Mat maskdown = cv::Mat_<uchar>(1, keypoints.size()); cv::Mat maskdown = cv::Mat_<uchar>(1, keypoints.size());
for (int i=0;i<keypoints.size() && it_cur !=keypoints.end();++i) { for (int i=0;i<keypoints.size() && it_cur !=keypoints.end();++i) {
keypoints_cur.push_back(cv::KeyPoint((*it_cur)->px.x(), (*it_cur)->px.y(), 7.f, (*it_cur)->score)); keypoints_cur.push_back(cv::KeyPoint((*it_cur)->px.x(), (*it_cur)->px.y(), 7.f, (*it_cur)->score));
memcpy(cur_des.data+(i*64),(*it_cur)->descriptor,sizeof(uint8_t)*64); memcpy(cur_des.data+(i*64),(*it_cur)->descriptor,sizeof(uint8_t)*64);
@@ -106,6 +103,10 @@ namespace vio {
last_frame->T_f_w_.se2().translation()).norm())); last_frame->T_f_w_.se2().translation()).norm()));
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) < close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2)); boost::bind(&std::pair<FramePtr, double>::second, _2));
std::unique_ptr<SparseImgAlignGpu> img_align=std::make_unique<SparseImgAlignGpu>(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
for (auto &&it_frame:_for(close_kfs)) {
img_align->run(it_frame.item.first, frame, log_);
}
overlap_kfs.reserve(options_.max_n_kfs); overlap_kfs.reserve(options_.max_n_kfs);
std::vector<int> added_keypoints; std::vector<int> added_keypoints;
for (auto &&it_frame:_for(close_kfs)) { for (auto &&it_frame:_for(close_kfs)) {
@@ -166,6 +167,8 @@ namespace vio {
++it_ref; ++it_ref;
} }
} }
/* std::cerr<<"here\n";
exit(0);*/
} }
} }
GPU_version/vio/src/sparse_img_align_gpu.cpp
+15 -19
View File
@@ -38,31 +38,25 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
reset(); reset();
cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()}; cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()};
cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()}; cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()};
cl_float3 features[ref_frame->fts_.size()]; cl_float3* features=(cl_float3*)calloc(ref_frame->fts_.size(), sizeof(cl_float3));
cl_float2 featue_px[ref_frame->fts_.size()]; cl_float2* featue_px=(cl_float2*)calloc(ref_frame->fts_.size(), sizeof(cl_float2));
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
for(auto it:_for(ref_frame->fts_)){ for(auto it=ref_frame->fts_.begin();it!=ref_frame->fts_.end();++it){
if(it.item->point == nullptr){ if((*it)->point == nullptr)continue;
features[it.index].x=0.f; Vector3d xyz_ref=(*it)->f*(ref_frame->se3().inverse()*(*it)->point->pos_).norm();
features[it.index].y=0.f; features[feature_counter_].x=xyz_ref(0);
features[it.index].z=0.f; features[feature_counter_].y=xyz_ref(1);
featue_px[it.index].x=0.f; features[feature_counter_].z=xyz_ref(2);
featue_px[it.index].y=0.f; featue_px[feature_counter_].x=(*it)->px(0);
continue; featue_px[feature_counter_].y=(*it)->px(1);
}
Vector3d xyz_ref=it.item->f*(ref_frame->se3().inverse()*it.item->point->pos_).norm();
features[it.index].x=xyz_ref(0);
features[it.index].y=xyz_ref(1);
features[it.index].z=xyz_ref(2);
featue_px[it.index].x=it.item->px(0);
featue_px[it.index].y=it.item->px(1);
++feature_counter_; ++feature_counter_;
} }
if(!feature_counter_) // more than 10 if(!feature_counter_) // more than 10
{ {
free(featue_px);
free(features);
return 0; return 0;
} }
feature_counter_=ref_frame->fts_.size();
residual_->load(1,2,1,cur_pos); residual_->load(1,2,1,cur_pos);
residual_->load(1,3,1,ref_pos); residual_->load(1,3,1,ref_pos);
residual_->load(1,4,feature_counter_,features); residual_->load(1,4,feature_counter_,features);
@@ -102,7 +96,9 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
free(H); free(H);
free(J); free(J);
free(chi2_); free(chi2_);
if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 1; free(featue_px);
free(features);
if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z) || fabs(pos[0].z-cur_pos[0].z)>M_PI_2)return 1;
cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z); cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
return 1; return 1;
} }