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

replace kernel fast with opencv

Browse Source
This commit is contained in:
gfuse
2021-09-14 20:18:52 +02:00
parent 8686725536
commit 2b4e9abfe3
5 changed files with 13340 additions and 16024 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+12143 -14093
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+1175 -1856
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/src/point.cpp
+1
View File
@@ -104,6 +104,7 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) cons
Vector3d obs_dir(Vector3d(framepos(0),1e-9,framepos(1)) + pos_); obs_dir.normalize();
double max_cos_angle = 1.0;
for(auto&& ob:obs_){
if(!ob->frame)continue;
if(ob->frame->id_==id){
ftr = ob;
return true;
GPU_version/vio/src/pose_optimizer.cpp
+7 -9
View File
@@ -55,7 +55,7 @@ void optimizeGaussNewton(
Vector2d e = vk::project2d((*it)->f)
- vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
e *= 1.0 / (1<<(*it)->level);
chi2_vec_init.push_back(e.squaredNorm()); // just for debug
chi2_vec_init.push_back(e.norm()); // just for debug
errors.push_back(e.norm());
}
@@ -68,8 +68,6 @@ void optimizeGaussNewton(
#endif
num_obs = errors.size();
chi2_vec_init.reserve(num_obs);
chi2_vec_final.reserve(num_obs);
double scale = estimated_scale;
for(size_t iter=0; iter<n_iter; iter++)
{
@@ -95,7 +93,7 @@ void optimizeGaussNewton(
double weight = weight_function.value(e.norm()/scale);
A.noalias() += J.transpose()*J*weight;
b.noalias() -= J.transpose()*e*weight;
new_chi2 += e.squaredNorm();
new_chi2 += e.norm();
}
// solve linear system
@@ -134,14 +132,15 @@ void optimizeGaussNewton(
continue;
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
e /= (1<<(*it)->level);
chi2_vec_final.push_back(e.squaredNorm());
if(e.norm() > 0.01)
chi2_vec_final.push_back(e.norm());
if(e.norm() > 0.5)
{
// we don't need to delete a reference in the point since it was not created yet
(*it)->point = NULL;
++n_deleted_refs;
}
}
num_obs -= n_deleted_refs;
#if VIO_DEBUG
error_init=0.0;
error_final=0.0;
@@ -149,10 +148,9 @@ void optimizeGaussNewton(
error_init /=chi2_vec_init.size();
if(!chi2_vec_final.empty())for(auto&& i:chi2_vec_final)error_final+=i;
error_final /= chi2_vec_final.size();
fprintf(log,"[%s] n deleted obs = %d \t scale =%f \t error init =%f \t error end=%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,estimated_scale,error_init,error_final);
fprintf(log,"[%s] n deleted obs = %d \t n obs felt =%d \t error init =%f \t error end=%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,num_obs,error_init,error_final);
#endif
num_obs -= n_deleted_refs;
}
} // namespace pose_optimizer
GPU_version/vio/src/reprojector.cpp
+14 -66
View File
@@ -64,61 +64,6 @@ namespace vio {
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - Original implementation
void Reprojector::reprojectMap(
FramePtr frame,
std::vector<std::pair<FramePtr, std::size_t> > &overlap_kfs) {
resetGrid();
list<pair<FramePtr, double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
size_t n = 0;
overlap_kfs.reserve(options_.max_n_kfs);
for (auto it_frame = close_kfs.begin(), ite_frame = close_kfs.end();
it_frame != ite_frame && n < options_.max_n_kfs; ++it_frame, ++n) {
FramePtr ref_frame = it_frame->first;
overlap_kfs.push_back(pair<FramePtr, size_t>(ref_frame, 0));
for (auto it_ftr = ref_frame->fts_.begin(), ite_ftr = ref_frame->fts_.end();
it_ftr != ite_ftr; ++it_ftr) {
if ((*it_ftr)->point == NULL)
continue;
if ((*it_ftr)->point->last_frame_overlap_id_ == frame->id_)
continue;
(*it_ftr)->point->last_frame_overlap_id_ = frame->id_;
if (reprojectPoint(frame, (*it_ftr)->point)) {
overlap_kfs.back().second++;
}
}
}
{
boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
auto it = map_.point_candidates_.candidates_.begin();
while (it != map_.point_candidates_.candidates_.end()) {
if (!reprojectPoint(frame, it->first)) {
it->first->n_failed_reproj_ += 3;
if (it->first->n_failed_reproj_ > 30) {
map_.point_candidates_.deleteCandidate(*it);
it = map_.point_candidates_.candidates_.erase(it);
continue;
}
}
++it;
}
}
for (size_t i = 0; i < grid_.cells.size(); ++i) {
if (reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
++n_matches_;
if (n_matches_ > (size_t) Config::maxFts())
break;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - second reimplementation
void Reprojector::reprojectMap2(
FramePtr frame,
@@ -128,11 +73,12 @@ namespace vio {
if(frame->id_<1)return;
resetGrid();
cv::Mat descriptors_cur;
std::vector<cv::KeyPoint> keypoints_cur;
Features keypoints_cur;
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
fast_detector(frame->img(), gpu_fast_, keypoints_cur);
extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
feature_detection::FastDetector detector(
frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
list<pair<FramePtr, double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);
if (!last_frame->fts_.empty())
@@ -157,13 +103,15 @@ namespace vio {
for (auto &&match:matches) {
if (match.queryIdx != f.index)continue;
if(keypoints_cur.size() < match.trainIdx)continue;
list<Feature*>::iterator point=keypoints_cur.begin();
std::advance(point,match.trainIdx);
if (f.item->point == NULL){
const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
const int k = static_cast<int>((*point)->px.y() / grid_.cell_size) *
grid_.grid_n_cols
+ static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
+ static_cast<int>((*point)->px.x() / grid_.cell_size);
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
(int) keypoints_cur.at(match.trainIdx).pt.y);
Vector2d px((int) (*point)->px.x(),
(int) (*point)->px.y());
frame->fts_.push_back(new Feature(it_frame.item.first.get(),
new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
px,it_frame.item.first->cam_->cam2world(f.item->px),0));
@@ -174,12 +122,12 @@ namespace vio {
}else{
if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
f.item->point->last_frame_overlap_id_ = frame->id_;
const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
const int k = static_cast<int>((*point)->px.y()/ grid_.cell_size) *
grid_.grid_n_cols
+ static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
+ static_cast<int>((*point)->px.x() / grid_.cell_size);
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
(int) keypoints_cur.at(match.trainIdx).pt.y);
Vector2d px((int) (*point)->px.x(),
(int) (*point)->px.y());
//f.item->point->addFrameRef(f.item);
grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
overlap_kfs.back().second++;