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

detected problem depth estimation some times goes too off

Browse Source
This commit is contained in:
gfuse
2021-09-22 19:39:13 +02:00
parent cf451572bf
commit 93587b7859
12 changed files with 4192 additions and 1004 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+2248 -15
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+1872 -848
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/frame_handler_mono.h
+2 -23
View File
@@ -40,28 +40,11 @@ public:
/// Provide an image.
void addImage(const cv::Mat& img, double timestamp,const ros::Time& time);
/// Set the first frame (used for synthetic datasets in benchmark node)
void setFirstFrame(const FramePtr& first_frame);
/// Get the last frame that has been processed.
FramePtr lastFrame() { return last_frame_; }
/// Get the set of spatially closest keyframes of the last frame.
const set<FramePtr>& coreKeyframes() { /*return core_kfs_;*/ }
/// Return the feature track to visualize the KLT tracking during initialization.
const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; }
const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_->px_cur_; }
/// Access the depth filter.
DepthFilter* depthFilter() const{ return depth_filter_; }
/// An external place recognition module may know where to relocalize.
bool relocalizeFrameAtPose(
const int keyframe_id,
const SE3& T_kf_f,
const cv::Mat& img,
const double timestamp);
void UpdateIMU(double* value,const ros::Time& time);
void UpdateCmd(double* value,const ros::Time& time);
UKF ukfPtr_;
@@ -93,17 +76,13 @@ protected:
/// Processes all frames after the first two keyframes.
virtual UpdateResult processFrame();
/// Try relocalizing the frame at relative position to provided keyframe.
virtual UpdateResult relocalizeFrame(
FramePtr ref_keyframe);
/// Reset the frame handler. Implement in derived class.
virtual void resetAll();
/// Keyframe selection criterion.
virtual bool needNewKf(double scene_depth_mean);
virtual bool needNewKf();
void setCoreKfs(size_t n_closest);
};
} // namespace vio
GPU_version/vio/include/vio/ukf.h
+7 -7
View File
@@ -30,7 +30,7 @@ public:
}
t_cmd=time;
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dt*(dy*cos(state_(2))-dx*sin(state_(2)));
state_h_(1)=state_(1)+dt*2.0*(dy*cos(state_(2))-dx*sin(state_(2)));
state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2)));
state_h_(3)=dpitch;
Eigen::Matrix<double,4,4> R;
@@ -40,7 +40,7 @@ public:
0,0,0,vio::Config::Cmd_Cov();
Eigen::Matrix<double,4,4> G;
G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),0.0,
0,1.0,dt*(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
0,1.0,dt*2.0*(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
0,0,1.0,dt,
0,0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
@@ -57,7 +57,7 @@ public:
Eigen::Matrix<double,4,1> E;
Eigen::Matrix<double,4,4> k;
E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
E(1)=state_(1)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
E(2)=state_(2)+dt*state_(3)-state_h_(2);
E(3)=dpitch-state_h_(3);
H<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0.0,
@@ -72,7 +72,7 @@ public:
state_=state_h_+k*E;
cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_;
state_h_(2)=state_(2)+dt*state_(3);
state_h_(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
state_h_(1)=state_(1)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)));
state_h_(3)=state_(3);
Eigen::Matrix<double,4,4> R;
@@ -82,7 +82,7 @@ public:
0,0,0,vio::Config::GYO_Noise();
Eigen::Matrix<double,4,4> G;
G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0,
0,1.0,pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0,
0,1.0,2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0,
0,0,1.0,dt,
0,0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
@@ -124,12 +124,12 @@ public:
void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
if(abs(x)<1.0)x=pow(x,3);//picked up from your code
if(abs(y)<1.0)y=pow(y,3);//picked up from your code
/* ++imu_syn_count_;
++imu_syn_count_;
if(imu_syn_[imu_syn_count_ % 7]){
imu_syn_[imu_syn_count_ % 7]=false;
}else{
return;
}*/
}
if(imu_syn_count_>50)imu_syn_count_=0;
boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->correct(x,y,theta,1e-9*time.toNSec());
GPU_version/vio/param/vo_fast.yaml
+1 -1
View File
@@ -13,4 +13,4 @@ svo:
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.5 #gyro white noise in continuous in EKF
svo_ekf: 0.05 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.1 #control vector noise in EKF
Cmd_Cov: 1.0 #control vector noise in EKF
GPU_version/vio/src/bundle_adjustment.cpp
+1 -1
View File
@@ -1,3 +1,4 @@
/*
//
// Created by root on 9/18/21.
//
@@ -20,7 +21,6 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
/*
#include <vio/math_utils.h>
#include <boost/thread.hpp>
#include <g2o/core/sparse_optimizer.h>
GPU_version/vio/src/depth_filter.cpp
+10 -9
View File
@@ -246,7 +246,7 @@ namespace vio {
lock_t lock(seeds_mut_);
std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin();
#if VIO_DEBUG
fprintf(log_,"[%s] update seed\n",vio::time_in_HH_MM_SS_MMM().c_str());
fprintf(log_,"[%s] update %d seeds\n",vio::time_in_HH_MM_SS_MMM().c_str(),seeds_.size());
#endif
const double focal_length = frame->cam_->errorMultiplier2();
double px_noise = 1.0;
@@ -254,8 +254,6 @@ namespace vio {
while( it!=seeds_.end())
{
// set this value true when seeds updating should be interrupted
if(seeds_updating_halt_)
return;
if((*it)->ftr->frame == NULL){
it = seeds_.erase(it);
@@ -295,21 +293,21 @@ namespace vio {
++n_failed_matches;
continue;
}
#if VIO_DEBUG
fprintf(log_,"[%s] calculated Z= %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z);
#endif
double tau = computeTau(T_cur_ref.inverse(), (*it)->ftr->f, z, px_error_angle);
double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
#if VIO_DEBUG
fprintf(log_,"[%s] calculated Z= %f tau: %f tau_inv: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z,
tau,tau_inverse);
#endif
// update the estimate
if(!updateSeed(1.0/z, tau_inverse*tau_inverse, *it)){
++it;
++n_failed_matches;
continue;
}
#if VIO_DEBUG
fprintf(log_,"[%s] update seeds 305\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
++n_updates;
if(frame->isKeyframe())
@@ -334,6 +332,9 @@ namespace vio {
}
else
++it;
#if VIO_DEBUG
fprintf(log_,"[%s] seeds updated\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
}
}
GPU_version/vio/src/feature_detection.cpp
+1 -1
View File
@@ -111,7 +111,7 @@ void FastDetector::detect(
if(score > corners.at(k).score){
corners.at(k) = Corner(fast_corners[i].x, fast_corners[i].y, score, L, 0.0f);
fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y)*scale, corners.at(k).level));
if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 1));
if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7));
}
}
}
GPU_version/vio/src/frame.cpp
+5 -3
View File
@@ -180,9 +180,11 @@ bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
{
if((*it)->point != NULL)
{
double z = frame->w2f((*it)->point->pos_).z();
depth_vec.push_back(z);
depth_min = fmin(z, depth_min);
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
if((e /= (1<<(*it)->level)).norm()>0.5)continue;
double z = frame->w2f((*it)->point->pos_).z();
depth_vec.push_back(z);
depth_min = fmin(z, depth_min);
}
}
GPU_version/vio/src/frame_handler_mono.cpp
+41 -93
View File
@@ -105,7 +105,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
}
// some cleanup from last iteration, can't do before because of visualization
//core_kfs_.clear();
overlap_kfs_.clear();
// create new frame
@@ -123,10 +122,11 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
last_frame_ = new_frame_;
// finish processing
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
if(stage_ == STAGE_RELOCALIZING){
depthFilter()->stopThread();
start();
}
#if VIO_DEBUG
fprintf(log_,"[%s] frame process finished the id is: %d the obs is:%d \n",vio::time_in_HH_MM_SS_MMM().c_str(),
last_frame_->id_,last_frame_->nObs());
#endif
}
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
@@ -161,10 +161,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
}
new_frame_->setKeyframe();
double depth_mean, depth_min;
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
frame_utils::getSceneDepth(klt_homography_init_->frame_ref_, depth_mean, depth_min);
depth_filter_->addKeyframe(klt_homography_init_->frame_ref_, depth_mean, 0.5*depth_min);
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
// add frame to map
map_.addKeyframe(new_frame_);
stage_ = STAGE_DEFAULT_FRAME;
@@ -191,46 +191,40 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
#if VIO_DEBUG
fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str());
#endif
new_frame_->T_f_w_=init_f.second;
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_->T_f_w_=init_f.second;
new_frame_=last_frame_;
return RESULT_FAILURE;
}
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_);
int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second;
#if VIO_DEBUG
fprintf(log_,"[%s] After Reprojection Map nPoint:%d ,nCell:%d ,nMatches:%d ,distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
fprintf(log_,"[%s] After Reprojection Map nPoint:%d ,nCell:%d ,nMatches:%d ,distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
n_point,
reprojector_.n_trials_,reprojector_.n_matches_,
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
#endif
if( reprojector_.n_trials_ < 10){
auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
return RESULT_FAILURE;
}
size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
Config::poseOptimThresh(), 70, false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);
#if VIO_DEBUG
fprintf(log_,"[%s] After pose optimization, distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
fprintf(log_,"[%s] After pose optimization, distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
#endif
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.1 ||
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2 || sfba_n_edges_final<10){
auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.1 ||
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<4){
new_frame_=last_frame_;
return RESULT_FAILURE;
}
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),
@@ -238,32 +232,22 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first;
#if VIO_DEBUG
fprintf(log_,"[%s] Update EKF and 3D points, distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
fprintf(log_,"[%s] Update EKF and 3D points the number of feature in the new frame: %d and number of obs: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->fts_.size(),new_frame_->nObs());
#endif
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
depth_filter_->addFrame(new_frame_);
/* if(sfba_n_edges_final < Config::qualityMinFts()){
return RESULT_FAILURE;
}*/
// select keyframe
//core_kfs_.insert(new_frame_);
setTrackingQuality(sfba_n_edges_final);
if(tracking_quality_ == TRACKING_INSUFFICIENT || tracking_quality_ == TRACKING_BAD)
if(!needNewKf())//edited
{
return RESULT_NO_KEYFRAME;
return RESULT_NO_KEYFRAME;
}
double depth_mean, depth_min;
double depth_mean=0.0, depth_min=0.0;
frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
if(!needNewKf(depth_mean))//edited
{
return RESULT_NO_KEYFRAME;
}
new_frame_->setKeyframe();
#if VIO_DEBUG
fprintf(log_,"[%s] Choose frame as a key frame Scene Depth mean:%f ,depth min:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
@@ -292,70 +276,34 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
return RESULT_IS_KEYFRAME;
}
FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
FramePtr ref_keyframe)
{
return RESULT_FAILURE;
}
bool FrameHandlerMono::relocalizeFrameAtPose(
const int keyframe_id,
const SE3& T_f_kf,
const cv::Mat& img,
const double timestamp)
{
/*
FramePtr ref_keyframe;
if(!map_.getKeyframeById(keyframe_id, ref_keyframe))
return false;
new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
UpdateResult res = relocalizeFrame(T_f_kf, ref_keyframe);
if(res != RESULT_FAILURE) {
last_frame_ = new_frame_;
return true;
}*/
return false;
}
void FrameHandlerMono::resetAll()
{
resetCommon();
last_frame_.reset();
new_frame_.reset();
//core_kfs_.clear();
overlap_kfs_.clear();
depth_filter_->reset();
}
void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
bool FrameHandlerMono::needNewKf()
{
resetAll();
last_frame_ = first_frame;
last_frame_->setKeyframe();
map_.addKeyframe(last_frame_);
stage_ = STAGE_DEFAULT_FRAME;
}
bool FrameHandlerMono::needNewKf(double scene_depth_mean)
{
if(scene_depth_mean >10.0)return true;
size_t com_obs=0;
SE2_5 closest_kfs(0,0,0);
for(auto&& it:overlap_kfs_)
{
if(fabs(it.first->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>0.0523599)return true;
if(fabs((it.first->T_f_w_.translation()-new_frame_->T_f_w_.translation()).norm()) > Config::kfSelectMinDist())
return true;
if(it.second>com_obs){
com_obs=it.second;
closest_kfs=SE2_5(it.first->T_f_w_.se2());
}
}
#if VIO_DEBUG
fprintf(log_,"[%s] key frame check new frame:%f %f %f, key close:%f %f %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x(),new_frame_->T_f_w_.se2().translation().y(),new_frame_->T_f_w_.pitch(),
closest_kfs.se2().translation().x(),closest_kfs.se2().translation().y(),closest_kfs.pitch());
#endif
if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.43 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.2)return true;
return false;
}
void FrameHandlerMono::setCoreKfs(size_t n_closest)
{
size_t n = min(n_closest, overlap_kfs_.size()-1);
std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(),
boost::bind(&pair<FramePtr, size_t>::second, _1) >
boost::bind(&pair<FramePtr, size_t>::second, _2));
//std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair<FramePtr,size_t>& i){ core_kfs_.insert(i.first); });
}
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){
if(value== nullptr)return;
ukfPtr_.UpdateIMU(value[0],value[1],value[2],time);
GPU_version/vio/src/pose_optimizer.cpp
+2 -2
View File
@@ -125,11 +125,11 @@ void optimizeGaussNewton(
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
e /= (1<<(*it)->level);
chi2_vec_final.push_back(e.norm());
if(e.norm() > 0.5)
if(e.norm() > 0.5/* 2.0 / frame->cam_->errorMultiplier2()*/)
{
// 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;
GPU_version/vio/src/reprojector.cpp
+2 -1
View File
@@ -97,7 +97,7 @@ namespace vio {
cv::Mat descriptors_ref;
overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0));
for (auto &&it_ftr:it_frame.item.first->fts_) {
keypoints_kfs.push_back(cv::KeyPoint(it_ftr->px.x(), it_ftr->px.y(), 1));
keypoints_kfs.push_back(cv::KeyPoint(it_ftr->px.x(), it_ftr->px.y(), 7));
}
extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref);
matcher->match(descriptors_ref, descriptors_cur, matches);
@@ -185,6 +185,7 @@ namespace vio {
if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30)
map_.point_candidates_.deleteCandidatePoint(it->pt);
it = cell.erase(it);
// ++it;
continue;
}
syn_=false;