From e754978e3e227740b2a2704e5116479942b11f60 Mon Sep 17 00:00:00 2001 From: gfuse Date: Sat, 4 Sep 2021 11:32:59 +0200 Subject: [PATCH] correct frame and add log file for depth-filter --- GPU_version/vio/include/vio/depth_filter.h | 1 + GPU_version/vio/include/vio/global.h | 7 +++-- GPU_version/vio/include/vio/ukf.h | 14 +++++---- GPU_version/vio/launch/debug.launch | 21 +++++++++++++ GPU_version/vio/launch/live.launch | 16 ---------- GPU_version/vio/src/depth_filter.cpp | 34 +++++++++++++++++----- GPU_version/vio/src/frame_handler_mono.cpp | 3 +- GPU_version/vio/src/visualizer.cpp | 11 ++++--- GPU_version/vio/src/vo_node.cpp | 5 +++- 9 files changed, 73 insertions(+), 39 deletions(-) create mode 100644 GPU_version/vio/launch/debug.launch delete mode 100644 GPU_version/vio/launch/live.launch diff --git a/GPU_version/vio/include/vio/depth_filter.h b/GPU_version/vio/include/vio/depth_filter.h index 8c9eee2..1308e70 100644 --- a/GPU_version/vio/include/vio/depth_filter.h +++ b/GPU_version/vio/include/vio/depth_filter.h @@ -150,6 +150,7 @@ protected: double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds. double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds. Matcher matcher_; + FILE* log_; //uint seeds_cont_=0; /// Initialize new seeds from a frame. diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index 2c037f0..bbc9a5f 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -95,10 +95,11 @@ namespace vio SE3 se3() const{ assert(T2_!= nullptr); //Todo add 15 roll orientation + //Camera frame z front, x right, y down -> right hands Quaterniond q; - q = AngleAxisd(0.261799, Vector3d::UnitX()) - * AngleAxisd(pitch(), Vector3d::UnitY()) - * AngleAxisd(0.0, Vector3d::UnitZ()); + q = AngleAxisd(0.261799, Vector3d::UnitX()) // roll + * AngleAxisd(pitch(), Vector3d::UnitY())// pitch + * AngleAxisd(0.0, Vector3d::UnitZ()); //yaw SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1))); return tem; } diff --git a/GPU_version/vio/include/vio/ukf.h b/GPU_version/vio/include/vio/ukf.h index 0e9ee01..ef1a828 100644 --- a/GPU_version/vio/include/vio/ukf.h +++ b/GPU_version/vio/include/vio/ukf.h @@ -111,29 +111,31 @@ public: virtual ~UKF() { delete filter_; }; - void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double pitch/*in imu frame*/,const ros::Time& time) { + void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) { while(lock)usleep(5); lock=true; 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 - if(filter_->predict_up)filter_->correct(x,y,pitch,1e-9*time.toNSec()); + if(filter_->predict_up)filter_->correct(x,y,theta,1e-9*time.toNSec()); lock=false; }; - void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double pitch/*in imu frame*/,const ros::Time& time) { + //IMU frame y front, x right, z up -> left hands (theta counts from y) + void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) { while(lock)usleep(5); lock=true; - filter_->predict(x,y,pitch,1e-9*time.toNSec()); + filter_->predict(x,y,theta,1e-9*time.toNSec()); lock=false; }; + //Camera frame z front, x right, y down -> right hands (pitch counts from z) std::pair,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/,size_t match,ros::Time& time) { while(lock)usleep(5); lock=true; //if(filter_->predict_up)filter_->correct(z,x,pitch,match,1e-9*time.toNSec()); lock=false; - return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),M_PI-filter_->state_(2))); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-filter_->state_(2))); }; std::pair,vio::SE2_5> get_location(){ - return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),M_PI-filter_->state_(2))); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-filter_->state_(2))); } private: Base* filter_= nullptr; diff --git a/GPU_version/vio/launch/debug.launch b/GPU_version/vio/launch/debug.launch new file mode 100644 index 0000000..378fe95 --- /dev/null +++ b/GPU_version/vio/launch/debug.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/GPU_version/vio/launch/live.launch b/GPU_version/vio/launch/live.launch deleted file mode 100644 index 7cd0ad8..0000000 --- a/GPU_version/vio/launch/live.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index 398af6c..ca076ee 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -28,6 +28,8 @@ #include #include #include +#include +#include namespace vio { @@ -53,7 +55,11 @@ namespace vio { new_keyframe_set_(false), new_keyframe_min_depth_(0.0), new_keyframe_mean_depth_(0.0) - {} + { + log_ =fopen("/root/Project/log.txt","w+"); + assert(log_); + chmod("/root/Project/log.txt", ACCESSPERMS); + } DepthFilter::~DepthFilter() { @@ -64,6 +70,7 @@ namespace vio { void DepthFilter::startThread() { thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this); + fprintf(log_,"init\n"); } void DepthFilter::stopThread() @@ -78,15 +85,17 @@ namespace vio { thread_->join(); thread_ = NULL; } + fclose(log_); } void DepthFilter::addFrame(FramePtr frame) { + fprintf(log_,"add new frame\n"); if(thread_ != NULL) { { - lock_t lock(frame_queue_mut_); - if(frame_queue_.size() > 2) + if(frame_queue_.empty())lock_t lock(frame_queue_mut_); + if(frame_queue_.size() > 10) frame_queue_.pop(); frame_queue_.push(frame); } @@ -102,6 +111,7 @@ namespace vio { { new_keyframe_min_depth_ = depth_min; new_keyframe_mean_depth_ = depth_mean; + fprintf(log_,"add key frame\n"); if(thread_ != NULL) { @@ -116,13 +126,14 @@ namespace vio { void DepthFilter::initializeSeeds(FramePtr frame) { Features new_features; + fprintf(log_,"init seeds\n"); feature_detector_->setExistingFeatures(frame->fts_); feature_detector_->detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features); // initialize a seed for every new feature seeds_updating_halt_ = true; - lock_t lock(seeds_mut_); // by locking the updateSeeds function stops + seeds_mut_.lock(); ++Seed::batch_counter; std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_)); @@ -131,12 +142,14 @@ namespace vio { if(options_.verbose) SVO_INFO_STREAM("DepthFilter: Initialized "<::iterator it=seeds_.begin(); size_t n_removed = 0; while(it!=seeds_.end()) @@ -150,16 +163,17 @@ namespace vio { ++it; } seeds_updating_halt_ = false; + seeds_mut_.unlock(); } void DepthFilter::reset() { seeds_updating_halt_ = true; { - lock_t lock(seeds_mut_); + seeds_mut_.lock(); seeds_.clear(); + seeds_mut_.unlock(); } - lock_t lock(); while(!frame_queue_.empty()) frame_queue_.pop(); seeds_updating_halt_ = false; @@ -172,11 +186,14 @@ namespace vio { { while(!boost::this_thread::interruption_requested()) { + + fprintf(log_,"loop 182\n"); FramePtr frame; { lock_t lock(frame_queue_mut_); while(frame_queue_.empty() && new_keyframe_set_ == false) frame_queue_cond_.wait(lock); + lock.unlock(); if(new_keyframe_set_) { new_keyframe_set_ = false; @@ -185,7 +202,7 @@ namespace vio { } else { - frame = frame_queue_.front(); + frame = frame_queue_.back(); frame_queue_.pop(); } } @@ -203,6 +220,7 @@ namespace vio { lock_t lock(seeds_mut_); std::list::iterator it=seeds_.begin(); + fprintf(log_,"update seed\n"); const double focal_length = frame->cam_->errorMultiplier2(); double px_noise = 1.0; double px_error_angle = atan2(px_noise,(2.0*focal_length))*2.0; // law of chord (sehnensatz) diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 21daefa..5e1a11b 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -191,6 +191,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_); new_frame_->T_f_w_ =result.second; new_frame_->Cov_ = result.first; + depth_filter_->addFrame(new_frame_); if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ return RESULT_FAILURE; } @@ -209,7 +210,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited { - depth_filter_->addFrame(new_frame_); + //depth_filter_->addFrame(new_frame_); return RESULT_NO_KEYFRAME; } new_frame_->setKeyframe(); diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index e04a74c..ee6e448 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -52,11 +52,14 @@ void Visualizer::publishMinimal( { // publish cam in world frame (Estimated odometry in the worls frame) auto odom=ukf.get_location(); - Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ())); + //Rviz frame z up, x forward, y left -> right hands (yaw counts from x) + //Camera frame z front, x right, y down -> right hands (pitch counts from z) + //IMU frame y front, x right, z up -> left hands (theta counts from y) + Quaterniond q(AngleAxisd(-odom.second.pitch(), Vector3d::UnitZ())); geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped); msg_pose_with_cov->header = header_msg; - msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(0); - msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(1); + msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(1); + msg_pose_with_cov->pose.pose.position.y = -odom.second.se2().translation()(0); msg_pose_with_cov->pose.pose.position.z = 0.0; msg_pose_with_cov->pose.pose.orientation.x = q.x(); msg_pose_with_cov->pose.pose.orientation.y = q.y(); @@ -71,7 +74,7 @@ void Visualizer::publishMinimal( pub_pose_with_cov_.publish(msg_pose_with_cov); if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){ publishPointMarker( - pub_points_, Eigen::Vector3d(odom.second.se2().translation()(0),odom.second.se2().translation()(1),0.0), "trajectory", + pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),-odom.second.se2().translation()(0),0.0), "trajectory", ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5)); } } diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 7474c3d..56e8709 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -115,6 +115,7 @@ VioNode::VioNode() : VioNode::~VioNode() { + vo_->depthFilter()->stopThread(); start_=false; usleep(5000); imu_the_->join(); @@ -187,9 +188,11 @@ bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) { res.header.frame_id = "/world"; res.header.seq = trace_id_; auto odom=vo_->ukfPtr_.get_location(); + //Camera frame z front, x right, y down -> right hands (pitch counts from z) + //IMU frame y front, x right, z up -> left hands (theta counts from y) res.x=odom.second.se2().translation()(0); res.y=odom.second.se2().translation()(1); - res.yaw=odom.second.pitch(); + res.yaw=-odom.second.pitch(); res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2), odom.first(1,0),odom.first(1,1),odom.first(1,2), odom.first(2,0),odom.first(2,1),odom.first(2,2)};