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

add log files

Browse Source
This commit is contained in:
gfuse
2021-09-04 17:05:52 +02:00
parent 99a5d45a6f
commit 752b94d1ae
9 changed files with 149 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+1 -1
View File
@@ -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)
GPU_version/vio/depth_filter_log.txt
Executable
+9
View File
@@ -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
GPU_version/vio/frame_handler_log.txt
Executable
+52
View File
@@ -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
GPU_version/vio/include/vio/frame_handler_mono.h
+4 -4
View File
@@ -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.
GPU_version/vio/include/vio/global.h
+24
View File
@@ -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<milliseconds>(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){
GPU_version/vio/include/vio/params_helper.h
+7
View File
@@ -15,6 +15,13 @@
#include <string>
#include <ros/ros.h>
#if VIO_DEBUG
#include <chrono>
#include <ctime>
#include <iomanip>
#include <sstream>
#include <string>
#endif
namespace vk {
GPU_version/vio/src/depth_filter.cpp
+13 -13
View File
@@ -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<Seed>::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<Seed>::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;
GPU_version/vio/src/frame_handler_mono.cpp
+36 -11
View File
@@ -25,9 +25,11 @@
#include <vio/depth_filter.h>
#include <vio/for_it.hpp>
#include <assert.h>
#if VIO_DEBUG
#include <sys/stat.h>
#endif
namespace vio {
FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init) :
FrameHandlerBase(),
cam_(cam),
@@ -65,6 +67,11 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
gpu_fast_->write_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: "<<n_point
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<
"\t angle between two frames:"<<last_frame_->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);
GPU_version/vio/src/vo_node.cpp
+3 -1
View File
@@ -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;