diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index 217602c..28ea580 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/GPU_version/vio/CMakeLists.txt @@ -74,7 +74,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for) ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\") -ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}/kernel\") +ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\") ADD_DEFINITIONS(-DVIO_DEBUG=true) diff --git a/GPU_version/vio/depth_filter_log.txt b/GPU_version/vio/depth_filter_log.txt new file mode 100755 index 0000000..60ebdcc --- /dev/null +++ b/GPU_version/vio/depth_filter_log.txt @@ -0,0 +1,9 @@ +[17:03:24.779] init depth filter +[17:03:46.024] DepthFilter: RESET. +[17:03:48.535] add key frame +[17:03:48.535] init seeds +[17:03:48.535] update seed +[17:03:48.535] init seeds +[17:03:48.539] DepthFilter: Initialized 28 new seeds +[17:03:48.540] DepthFilter: Initialized 63 new seeds +[17:04:17.931] DepthFilter stop thread invoked.[17:04:17.931] DepthFilter interrupt and join thread \ No newline at end of file diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt new file mode 100755 index 0000000..759f06d --- /dev/null +++ b/GPU_version/vio/frame_handler_log.txt @@ -0,0 +1,52 @@ +[17:03:47.527] Init: Selected first frame. +[17:03:47.627] Init: distance between two frames:0.000012 ,angle between two frames:0.022571 +[17:03:47.732] Init: distance between two frames:0.000004 ,angle between two frames:0.025557 +[17:03:47.828] Init: distance between two frames:0.000006 ,angle between two frames:0.022501 +[17:03:47.932] Init: distance between two frames:0.000004 ,angle between two frames:0.026059 +[17:03:48.032] Init: distance between two frames:0.000029 ,angle between two frames:0.013181 +[17:03:48.133] Init: distance between two frames:0.000002 ,angle between two frames:0.013368 +[17:03:48.232] Init: distance between two frames:0.000007 ,angle between two frames:0.016733 +[17:03:48.332] Init: distance between two frames:0.000009 ,angle between two frames:0.021861 +[17:03:48.535] Init: distance between two frames:0.046145 ,angle between two frames:-0.471021 +[17:03:48.539] Init: Selected first frame. +[17:03:48.816] Reprojection Map nPoint:17 ,nCell:12 ,nMatches:3 ,distance between two frames:0.009163 ,angle between two frames:0.049424 +[17:03:49.020] Reprojection Map nPoint:3 ,nCell:3 ,nMatches:3 ,distance between two frames:3.357603 ,angle between two frames:2.458354 +[17:03:49.097] Reprojection Map nPoint:3 ,nCell:3 ,nMatches:0 ,distance between two frames:2.656850 ,angle between two frames:3.589865 +[17:03:49.189] Reprojection Map nPoint:17 ,nCell:13 ,nMatches:9 ,distance between two frames:0.465308 ,angle between two frames:0.022542 +[17:03:49.420] Reprojection Map nPoint:8 ,nCell:7 ,nMatches:7 ,distance between two frames:9.331640 ,angle between two frames:2.368349 +[17:03:49.499] Reprojection Map nPoint:8 ,nCell:8 ,nMatches:0 ,distance between two frames:2.651696 ,angle between two frames:-2.697419 +[17:03:49.595] Reprojection Map nPoint:22 ,nCell:15 ,nMatches:4 ,distance between two frames:0.302242 ,angle between two frames:0.020434 +[17:03:49.820] Reprojection Map nPoint:4 ,nCell:4 ,nMatches:4 ,distance between two frames:2.140314 ,angle between two frames:1.703499 +[17:03:49.897] Reprojection Map nPoint:23 ,nCell:14 ,nMatches:6 ,distance between two frames:2.831562 ,angle between two frames:-2.702069 +[17:03:50.320] Reprojection Map nPoint:6 ,nCell:6 ,nMatches:6 ,distance between two frames:2.264570 ,angle between two frames:2.182084 +[17:03:50.399] Reprojection Map nPoint:23 ,nCell:13 ,nMatches:5 ,distance between two frames:2.664975 ,angle between two frames:-2.689325 +[17:03:50.503] Reprojection Map nPoint:4 ,nCell:4 ,nMatches:4 ,distance between two frames:192.802028 ,angle between two frames:2.732007 +[17:03:50.599] Reprojection Map nPoint:25 ,nCell:19 ,nMatches:5 ,distance between two frames:2.643489 ,angle between two frames:-2.703208 +[17:03:51.318] Reprojection Map nPoint:5 ,nCell:5 ,nMatches:5 ,distance between two frames:2.178774 ,angle between two frames:-2.804699 +[17:03:51.396] Reprojection Map nPoint:18 ,nCell:11 ,nMatches:4 ,distance between two frames:2.696336 ,angle between two frames:-2.715705 +[17:04:09.106] Reprojection Map nPoint:23 ,nCell:18 ,nMatches:12 ,distance between two frames:7.108872 ,angle between two frames:-3.644508 +[17:04:09.609] Reprojection Map nPoint:30 ,nCell:23 ,nMatches:14 ,distance between two frames:3.232156 ,angle between two frames:0.155299 +[17:04:09.724] Reprojection Map nPoint:33 ,nCell:26 ,nMatches:12 ,distance between two frames:23.977235 ,angle between two frames:0.070389 +[17:04:09.905] Reprojection Map nPoint:29 ,nCell:20 ,nMatches:11 ,distance between two frames:156200.421037 ,angle between two frames:0.181012 +[17:04:10.013] Reprojection Map nPoint:34 ,nCell:19 ,nMatches:11 ,distance between two frames:3.142497 ,angle between two frames:0.353249 +[17:04:10.105] Reprojection Map nPoint:37 ,nCell:21 ,nMatches:16 ,distance between two frames:3.432714 ,angle between two frames:-0.663393 +[17:04:10.209] Reprojection Map nPoint:36 ,nCell:15 ,nMatches:15 ,distance between two frames:2.417147 ,angle between two frames:-0.004748 +[17:04:10.428] Reprojection Map nPoint:36 ,nCell:19 ,nMatches:15 ,distance between two frames:1.455741 ,angle between two frames:0.065159 +[17:04:10.518] Reprojection Map nPoint:38 ,nCell:22 ,nMatches:16 ,distance between two frames:12.974222 ,angle between two frames:0.210486 +[17:04:10.609] Reprojection Map nPoint:40 ,nCell:22 ,nMatches:16 ,distance between two frames:8.860754 ,angle between two frames:0.039250 +[17:04:10.730] Reprojection Map nPoint:42 ,nCell:16 ,nMatches:15 ,distance between two frames:2.417891 ,angle between two frames:0.044953 +[17:04:10.822] Reprojection Map nPoint:41 ,nCell:15 ,nMatches:13 ,distance between two frames:2.485466 ,angle between two frames:0.041414 +[17:04:10.929] Reprojection Map nPoint:38 ,nCell:20 ,nMatches:13 ,distance between two frames:4.775227 ,angle between two frames:0.781172 +[17:04:11.026] Reprojection Map nPoint:43 ,nCell:24 ,nMatches:15 ,distance between two frames:17.026834 ,angle between two frames:0.085196 +[17:04:11.117] Reprojection Map nPoint:42 ,nCell:15 ,nMatches:14 ,distance between two frames:1.545554 ,angle between two frames:-0.414163 +[17:04:11.213] Reprojection Map nPoint:44 ,nCell:24 ,nMatches:13 ,distance between two frames:3.878066 ,angle between two frames:1.039956 +[17:04:11.308] Reprojection Map nPoint:47 ,nCell:15 ,nMatches:12 ,distance between two frames:1.927317 ,angle between two frames:0.243026 +[17:04:11.415] Reprojection Map nPoint:50 ,nCell:19 ,nMatches:14 ,distance between two frames:2.706551 ,angle between two frames:0.324650 +[17:04:11.509] Reprojection Map nPoint:51 ,nCell:22 ,nMatches:17 ,distance between two frames:1.650694 ,angle between two frames:0.217249 +[17:04:11.607] Reprojection Map nPoint:54 ,nCell:20 ,nMatches:16 ,distance between two frames:1.690322 ,angle between two frames:0.095136 +[17:04:11.707] Reprojection Map nPoint:51 ,nCell:17 ,nMatches:14 ,distance between two frames:1.243964 ,angle between two frames:-1.003445 +[17:04:11.828] Reprojection Map nPoint:54 ,nCell:22 ,nMatches:17 ,distance between two frames:1.727187 ,angle between two frames:-0.022257 +[17:04:11.920] Reprojection Map nPoint:51 ,nCell:29 ,nMatches:17 ,distance between two frames:6.053159 ,angle between two frames:-0.315177 +[17:04:12.010] Reprojection Map nPoint:54 ,nCell:22 ,nMatches:17 ,distance between two frames:0.968941 ,angle between two frames:0.005651 +[17:04:12.118] Reprojection Map nPoint:57 ,nCell:22 ,nMatches:18 ,distance between two frames:1.669894 ,angle between two frames:-0.416700 +[17:04:12.208] Reprojection Map nPoint:54 ,nCell:29 ,nMatches:17 ,distance between two frames:9.852925 ,angle between two frames:0.109482 diff --git a/GPU_version/vio/include/vio/frame_handler_mono.h b/GPU_version/vio/include/vio/frame_handler_mono.h index ef38847..bc4e770 100644 --- a/GPU_version/vio/include/vio/frame_handler_mono.h +++ b/GPU_version/vio/include/vio/frame_handler_mono.h @@ -65,7 +65,9 @@ public: void UpdateIMU(double* value,const ros::Time& time); void UpdateCmd(double* value,const ros::Time& time); UKF ukfPtr_; - +#if VIO_DEBUG + FILE* log_=nullptr; +#endif protected: vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). Reprojector reprojector_; //!< Projects points from other keyframes into the current frame @@ -76,9 +78,7 @@ protected: DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points. opencl* gpu_fast_; ros::Time time_; -#if VIO_DEBUG - FILE* log_=nullptr; -#endif + /// Initialize the visual odometry algorithm. diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index 2bc144a..00c579c 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -50,6 +50,30 @@ namespace vio const double EPS = 0.0000000001; const double PI = 3.14159265; + static std::string time_in_HH_MM_SS_MMM() + { + using namespace std::chrono; + + // get current time + auto now = system_clock::now(); + + // get number of milliseconds for the current second + // (remainder after division into seconds) + auto ms = duration_cast(now.time_since_epoch()) % 1000; + + // convert to std::time_t in order to convert to std::tm (broken time) + auto timer = system_clock::to_time_t(now); + + // convert to broken time + std::tm bt = *std::localtime(&timer); + + std::ostringstream oss; + + oss << std::put_time(&bt, "%H:%M:%S"); // HH:MM:SS + oss << '.' << std::setfill('0') << std::setw(3) << ms.count(); + + return oss.str(); + }; class SE2_5: public SE2{ public: SE2_5(SE2&& se2){ diff --git a/GPU_version/vio/include/vio/params_helper.h b/GPU_version/vio/include/vio/params_helper.h index e593c3e..6316f5a 100644 --- a/GPU_version/vio/include/vio/params_helper.h +++ b/GPU_version/vio/include/vio/params_helper.h @@ -15,6 +15,13 @@ #include #include +#if VIO_DEBUG +#include +#include +#include +#include +#include +#endif namespace vk { diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index d7cce98..7456bb3 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -58,9 +58,9 @@ namespace vio { new_keyframe_mean_depth_(0.0) { #if VIO_DEBUG - log_ =fopen("/root/Project/log.txt","w+"); + log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+"); assert(log_); - chmod("/root/Project/log.txt", ACCESSPERMS); + chmod((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(), ACCESSPERMS); #endif } @@ -68,7 +68,7 @@ namespace vio { { stopThread(); #if VIO_DEBUG - fprintf(log_,"DepthFilter destructed.\n"); + fprintf(log_,"[%s] DepthFilter destructed.\n",vio::time_in_HH_MM_SS_MMM().c_str()); #endif } @@ -76,19 +76,19 @@ namespace vio { { thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this); #if VIO_DEBUG - fprintf(log_,"init depth filter\n"); + 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_,"DepthFilter stop thread invoked."); + 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_,"DepthFilter interrupt and join thread "); + 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(); @@ -104,7 +104,7 @@ namespace vio { void DepthFilter::addFrame(FramePtr frame) { #if VIO_DEBUG - fprintf(log_,"add new frame\n"); + fprintf(log_,"[%s] add new frame\n",vio::time_in_HH_MM_SS_MMM().c_str()); #endif if(thread_ != NULL) { @@ -127,7 +127,7 @@ namespace vio { new_keyframe_min_depth_ = depth_min; new_keyframe_mean_depth_ = depth_mean; #if VIO_DEBUG - fprintf(log_,"add key frame\n"); + fprintf(log_,"[%s] add key frame\n",vio::time_in_HH_MM_SS_MMM().c_str()); #endif if(thread_ != NULL) @@ -144,7 +144,7 @@ namespace vio { { Features new_features; #if VIO_DEBUG - fprintf(log_,"init seeds\n"); + fprintf(log_,"[%s] init seeds\n",vio::time_in_HH_MM_SS_MMM().c_str()); #endif feature_detector_->setExistingFeatures(frame->fts_); feature_detector_->detect(frame.get(), frame->img_pyr_, @@ -159,7 +159,7 @@ namespace vio { }); #if VIO_DEBUG - fprintf(log_,"DepthFilter: Initialized %d new seeds\n",new_features.size()); + fprintf(log_,"[%s] DepthFilter: Initialized %d new seeds\n",vio::time_in_HH_MM_SS_MMM().c_str(),new_features.size()); #endif seeds_updating_halt_ = false; seeds_mut_.unlock(); @@ -169,7 +169,7 @@ namespace vio { { seeds_updating_halt_ = true; #if VIO_DEBUG - fprintf(log_,"remove key frame\n"); + fprintf(log_,"[%s] remove key frame\n",vio::time_in_HH_MM_SS_MMM().c_str()); #endif seeds_mut_.lock(); std::list::iterator it=seeds_.begin(); @@ -201,7 +201,7 @@ namespace vio { seeds_updating_halt_ = false; #if VIO_DEBUG - fprintf(log_,"DepthFilter: RESET.\n"); + fprintf(log_,"[%s] DepthFilter: RESET.\n",vio::time_in_HH_MM_SS_MMM().c_str()); #endif } @@ -241,7 +241,7 @@ namespace vio { lock_t lock(seeds_mut_); std::list::iterator it=seeds_.begin(); #if VIO_DEBUG - fprintf(log_,"update seed\n"); + fprintf(log_,"[%s] update seed\n",vio::time_in_HH_MM_SS_MMM().c_str()); #endif const double focal_length = frame->cam_->errorMultiplier2(); double px_noise = 1.0; diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 4a04554..272cd30 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -25,9 +25,11 @@ #include #include #include +#if VIO_DEBUG +#include +#endif namespace vio { - FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix& init) : FrameHandlerBase(), cam_(cam), @@ -65,6 +67,11 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrixwrite_buf(1,10,300,chi2); klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); initialize(); +#if VIO_DEBUG + log_ =fopen((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(),"w+"); + assert(log_); + chmod((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(), ACCESSPERMS); +#endif } void FrameHandlerMono::initialize() @@ -105,8 +112,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const else if(stage_ == STAGE_FIRST_FRAME) res = processFirstFrame(); - // set last frame - //if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME) last_frame_ = new_frame_; // finish processing finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); @@ -127,7 +132,9 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() map_.reset(); map_.addKeyframe(new_frame_); stage_ = STAGE_SECOND_FRAME; - SVO_INFO_STREAM("Init: Selected first frame."); +#if VIO_DEBUG + fprintf(log_,"[%s] Init: Selected first frame. \n",vio::time_in_HH_MM_SS_MMM().c_str()); +#endif return RESULT_IS_KEYFRAME; } @@ -137,6 +144,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() new_frame_->T_f_w_=init_f.second; new_frame_->Cov_ = init_f.first; initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_); +#if VIO_DEBUG + fprintf(log_,"[%s] Init: distance between two frames:%f ,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(), + (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm(), + last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch()); +#endif auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); new_frame_->T_f_w_ = result.second; new_frame_->Cov_ = result.first; @@ -156,7 +168,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() map_.addKeyframe(new_frame_); stage_ = STAGE_DEFAULT_FRAME; klt_homography_init_->reset(); - SVO_INFO_STREAM("Init: triangulated initial map. Vio is starting ..."); +#if VIO_DEBUG + fprintf(log_,"[%s] Init: Selected first frame. \n",vio::time_in_HH_MM_SS_MMM().c_str()); +#endif return RESULT_IS_KEYFRAME; } @@ -172,11 +186,13 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_); int n_point=0; for(auto i:overlap_kfs_)n_point+=i.second; - std::cerr<<"Reprojection Map nPoint: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<< - "\t angle between two frames:"<T_f_w_.pitch()-new_frame_->T_f_w_.pitch()<<'\n'; +#if VIO_DEBUG + fprintf(log_,"[%s] Reprojection Map nPoint:%d ,nCell:%d ,nMatches:%d ,distance between two frames:%f ,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(), + n_point, + reprojector_.n_trials_,reprojector_.n_matches_, + (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm(), + last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch()); +#endif if( reprojector_.n_trials_ < 10)return RESULT_FAILURE; size_t sfba_n_edges_final=0; double sfba_thresh, sfba_error_init, sfba_error_final; @@ -191,6 +207,11 @@ 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; +#if VIO_DEBUG + fprintf(log_,"[%s] Update EKF and 3D points, distance between two frames:%f ,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(), + (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm(), + last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch()); +#endif optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); depth_filter_->addFrame(new_frame_); if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()/*10*/){ @@ -213,7 +234,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() 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(), + depth_mean, + depth_min); +#endif // new keyframe selected for(auto&& it:new_frame_->fts_){ if(it->point != NULL)it->point->addFrameRef(it); diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 34e29ff..9b1ed08 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -61,6 +61,9 @@ public: if(req.off==1 && imu_the_!=NULL){ start_=false; vo_->depthFilter()->stopThread(); +#if VIO_DEBUG + fclose(vo_->log_); +#endif imu_the_->interrupt(); if(imu_the_->get_id()==boost::this_thread::get_id()) imu_the_->detach(); @@ -114,7 +117,6 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg) { if(!start_)return; try { - auto start=std::chrono::steady_clock::now(); cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image; if(img.empty())return; cv::Mat imgbul;