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

add triangulation in reprojection

Browse Source
This commit is contained in:
gfuse
2021-09-24 11:07:03 +02:00
parent 3fca9926bc
commit b9a45107b6
8 changed files with 786 additions and 1125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/frame_handler_log.txt
+757 -1096
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/initialization.h
+6
View File
@@ -84,6 +84,12 @@ protected:
vector<int> outliers_hom, inlier_hom;
vector<int> outliers_ekf,inlier_ekf;
assert(ukf_!= nullptr);
/*#if VIO_DEBUG
for(int i=0;i<f_cur.size();++i)fprintf(log_,"[%s] cur: x=%f, y=%f, z=%f ref: x=%f y=%f z=%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),
f_cur.at(i).x(),f_cur.at(i).y(),f_cur.at(i).z(),
f_ref.at(i).x(),f_ref.at(i).y(),f_ref.at(i).z());
#endif*/
vk::computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
reprojection_threshold, focal_length,
GPU_version/vio/src/depth_filter.cpp
+2 -13
View File
@@ -67,29 +67,18 @@ namespace vio {
DepthFilter::~DepthFilter()
{
stopThread();
#if VIO_DEBUG
fprintf(log_,"[%s] DepthFilter destructed.\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
}
void DepthFilter::startThread()
{
thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this);
#if VIO_DEBUG
fprintf(log_,"[%s] init depth filter\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
}
void DepthFilter::stopThread()
{
#if VIO_DEBUG
fprintf(log_,"[%s] DepthFilter stop thread invoked.\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
if(thread_ != NULL)
{
#if VIO_DEBUG
fprintf(log_,"[%s] DepthFilter interrupt and join thread \n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
seeds_updating_halt_ = true;
thread_->interrupt();
usleep(5000);
@@ -322,7 +311,7 @@ namespace vio {
if(sqrt((*it)->sigma2) < (*it)->z_range/options_.seed_convergence_sigma2_thresh)
{
assert((*it)->ftr->point == NULL); // TODO this should not happen anymore
Vector3d xyz_world((*it)->ftr->frame->getSE3Inv() * ((*it)->ftr->f/(*it)->mu));
Vector3d xyz_world((*it)->ftr->frame->se3() * ((*it)->ftr->f/(*it)->mu));
std::shared_ptr<Point> point = std::make_shared<Point>(xyz_world, (*it)->ftr);
(*it)->ftr->point = point;
seed_converged_cb_(point, (*it)->sigma2); // put in candidate list
GPU_version/vio/src/feature_detection.cpp
+5 -1
View File
@@ -98,7 +98,7 @@ void FastDetector::detect(
}
for(uint i=0;i<count[0];++i)
{
if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0){
if(fast_corners[i].x<1 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){
continue;
}
const int k = static_cast<int>((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_
@@ -115,6 +115,10 @@ void FastDetector::detect(
}
}
}
if(fts.size()<1){
ROS_ERROR("GPU Driver crash try again!");
exit(0);
}
if(descriptors){
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
extractor->compute(frame->img(), keypoints, *descriptors);
GPU_version/vio/src/frame_handler_mono.cpp
+2
View File
@@ -173,6 +173,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d depth mean:%f min:%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min);
#endif
ROS_INFO("VIO initialized :)");
ROS_INFO("Running ...");
return RESULT_IS_KEYFRAME;
}
GPU_version/vio/src/initialization.cpp
+2 -2
View File
@@ -100,12 +100,12 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
if(frame_cur->cam_->isInFrame(Vector2d(px_cur_.at(id).x,px_cur_.at(id).y).cast<int>(), 10) &&
frame_ref_->cam_->isInFrame((*first)->px.cast<int>(), 10)){
Vector3d pos = frame_cur->se3() * (xyz_in_cur_.at(id)/* *scale */);
#if VIO_DEBUG
/*#if VIO_DEBUG
fprintf(log_,"[%s] inlier: x=%f, y=%f, z=%f pos: x=%f y=%f z=%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),
xyz_in_cur_.at(id).x(),xyz_in_cur_.at(id).y(),xyz_in_cur_.at(id).z(),
pos.x(),pos.y(),pos.z());
#endif
#endif*/
std::shared_ptr<Point> new_point = std::make_shared<Point>(pos);
std::shared_ptr<Feature> ftr_cur=std::make_shared<Feature>(frame_cur, new_point, Vector2d(px_cur_.at(id).x,px_cur_.at(id).y),
frame_cur->c2f(px_cur_.at(id).x,px_cur_.at(id).y), (*first)->level);
GPU_version/vio/src/math_utils.cpp
+6 -10
View File
@@ -64,10 +64,10 @@ reprojError(const Vector3d& f1,
}
double
computeInliers(const vector<Vector3d>& features1, // c1
const vector<Vector3d>& features2, // c2
const Matrix3d& R, // R_c1_c2
const Vector3d& t, // c1_t
computeInliers(const vector<Vector3d>& features1, // c2
const vector<Vector3d>& features2, // c1
const Matrix3d& R, // R_c1_to_c2
const Vector3d& t, // T_c1_to_c2
const double reproj_thresh,
double error_multiplier2,
vector<Vector3d>& xyz_vec, // in frame c1
@@ -81,21 +81,17 @@ computeInliers(const vector<Vector3d>& features1, // c1
//triangulate all features and compute reprojection errors and inliers
for(size_t j=0; j<features1.size(); ++j)
{
if(t.z()>0.0){
if(t.z()>0.0)
xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
if(e1 > reproj_thresh || e2 > reproj_thresh)
if(e1 > reproj_thresh || e2 > reproj_thresh || xyz_vec.back().z()<0.0)
outliers.push_back(j);
else
{
inliers.push_back(j);
tot_error += e1+e2;
}
}else{
ROS_ERROR("Backward motion is not supported for initialization, the robot needs to start its motion with a forward maneuver");
}
}
return tot_error;
}
GPU_version/vio/src/reprojector.cpp
+6 -3
View File
@@ -116,10 +116,13 @@ namespace vio {
assert(grid_.cells.at(k) != nullptr);
Vector2d px((int) (*point)->px.x(),
(int) (*point)->px.y());
SE3 T_ref_cur=it_frame.item.first->getSE3Inv()*frame->se3();
Vector3d new_point=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
frame->c2f(px),f.item->f);
if(new_point.z()<0.0)break;
frame->fts_.push_back(std::make_shared<Feature>(it_frame.item.first,
std::make_shared<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),(*point)->level));
std::make_shared<Point>(frame->se3()*new_point,f.item),
px,f.item->f,(*point)->level));
frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++;