diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index abc1a59..217602c 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/GPU_version/vio/CMakeLists.txt @@ -74,5 +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(-DVIO_DEBUG=true) diff --git a/GPU_version/vio/include/vio/depth_filter.h b/GPU_version/vio/include/vio/depth_filter.h index 1308e70..861aa71 100644 --- a/GPU_version/vio/include/vio/depth_filter.h +++ b/GPU_version/vio/include/vio/depth_filter.h @@ -20,7 +20,6 @@ #include #include #include -//#include #include #include #include @@ -150,7 +149,9 @@ 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_; +#if VIO_DEBUG + FILE* log_=nullptr; +#endif //uint seeds_cont_=0; /// Initialize new seeds from a frame. diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index bf21bd6..629995b 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -132,7 +132,7 @@ namespace vio { SE2 inv=T_f_w_.inverse(); Quaterniond q; q = AngleAxisd(0.261799, Vector3d::UnitX()) - * AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY()) + * AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real())-M_PI_2, Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1))); return out; diff --git a/GPU_version/vio/include/vio/frame_handler_mono.h b/GPU_version/vio/include/vio/frame_handler_mono.h index 6185fc5..ef38847 100644 --- a/GPU_version/vio/include/vio/frame_handler_mono.h +++ b/GPU_version/vio/include/vio/frame_handler_mono.h @@ -47,7 +47,7 @@ public: FramePtr lastFrame() { return last_frame_; } /// Get the set of spatially closest keyframes of the last frame. - const set& coreKeyframes() { return core_kfs_; } + const set& coreKeyframes() { /*return core_kfs_;*/ } /// Return the feature track to visualize the KLT tracking during initialization. const vector& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; } @@ -62,7 +62,6 @@ public: const SE3& T_kf_f, const cv::Mat& img, const double timestamp); - //std::shared_ptr imu_integPtr_; //Integration of IMU void UpdateIMU(double* value,const ros::Time& time); void UpdateCmd(double* value,const ros::Time& time); UKF ukfPtr_; @@ -72,12 +71,14 @@ protected: Reprojector reprojector_; //!< Projects points from other keyframes into the current frame FramePtr new_frame_; //!< Current frame. FramePtr last_frame_; //!< Last frame, not necessarily a keyframe. - set core_kfs_; //!< Keyframes in the closer neighbourhood. vector< pair > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!? initialization::KltHomographyInit* klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography. 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 bbc9a5f..2bc144a 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -68,6 +68,7 @@ namespace vio } SE2_5(SE3& se3){ Quaterniond q=se3.unit_quaternion().normalized(); + //Camera frame z front, x right, y down -> right hands auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw T2_=new SE2(SO2(euler(1)),Vector2d(se3.translation().x(),se3.translation().z())); assert(T2_!= nullptr); @@ -98,7 +99,7 @@ namespace vio //Camera frame z front, x right, y down -> right hands Quaterniond q; q = AngleAxisd(0.261799, Vector3d::UnitX()) // roll - * AngleAxisd(pitch(), Vector3d::UnitY())// pitch + * AngleAxisd(pitch()-M_PI_2, 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 ef1a828..a0970bb 100644 --- a/GPU_version/vio/include/vio/ukf.h +++ b/GPU_version/vio/include/vio/ukf.h @@ -126,16 +126,16 @@ public: 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) + //Camera frame z front, x right, y down -> right hands (pitch counts from x) 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_(0),filter_->state_(1),-filter_->state_(2))); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)+M_PI_2))); }; std::pair,vio::SE2_5> get_location(){ - return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-filter_->state_(2))); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)+M_PI_2))); } private: Base* filter_= nullptr; diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index c48cc99..adaf07d 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -25,7 +25,7 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature) ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z; float3 error=ref+cur; float yaw=0.0; - float pitch=error.z; + float pitch=error.z-1.57079632679489661923; float roll=0.26; float R00=cos(yaw)*cos(pitch);//cos(pitch) float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0 diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index ca076ee..d7cce98 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -28,9 +28,10 @@ #include #include #include +#if VIO_DEBUG #include #include - +#endif namespace vio { int Seed::batch_counter = 0; @@ -56,41 +57,55 @@ namespace vio { new_keyframe_min_depth_(0.0), new_keyframe_mean_depth_(0.0) { +#if VIO_DEBUG log_ =fopen("/root/Project/log.txt","w+"); assert(log_); chmod("/root/Project/log.txt", ACCESSPERMS); +#endif } DepthFilter::~DepthFilter() { stopThread(); - SVO_INFO_STREAM("DepthFilter destructed."); +#if VIO_DEBUG + fprintf(log_,"DepthFilter destructed.\n"); +#endif } void DepthFilter::startThread() { thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this); - fprintf(log_,"init\n"); +#if VIO_DEBUG + fprintf(log_,"init depth filter\n"); +#endif } void DepthFilter::stopThread() { - SVO_INFO_STREAM("DepthFilter stop thread invoked."); +#if VIO_DEBUG + fprintf(log_,"DepthFilter stop thread invoked."); +#endif if(thread_ != NULL) { - SVO_INFO_STREAM("DepthFilter interrupt and join thread... "); +#if VIO_DEBUG + fprintf(log_,"DepthFilter interrupt and join thread "); +#endif seeds_updating_halt_ = true; thread_->interrupt(); usleep(5000); thread_->join(); thread_ = NULL; } +#if VIO_DEBUG fclose(log_); +#endif } void DepthFilter::addFrame(FramePtr frame) { +#if VIO_DEBUG fprintf(log_,"add new frame\n"); +#endif if(thread_ != NULL) { { @@ -111,7 +126,9 @@ namespace vio { { new_keyframe_min_depth_ = depth_min; new_keyframe_mean_depth_ = depth_mean; +#if VIO_DEBUG fprintf(log_,"add key frame\n"); +#endif if(thread_ != NULL) { @@ -126,7 +143,9 @@ namespace vio { void DepthFilter::initializeSeeds(FramePtr frame) { Features new_features; +#if VIO_DEBUG fprintf(log_,"init seeds\n"); +#endif feature_detector_->setExistingFeatures(frame->fts_); feature_detector_->detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features); @@ -139,8 +158,9 @@ namespace vio { seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_)); }); - if(options_.verbose) - SVO_INFO_STREAM("DepthFilter: Initialized "<::iterator it=seeds_.begin(); size_t n_removed = 0; @@ -178,16 +200,15 @@ namespace vio { frame_queue_.pop(); seeds_updating_halt_ = false; - if(options_.verbose) - SVO_INFO_STREAM("DepthFilter: RESET."); +#if VIO_DEBUG + fprintf(log_,"DepthFilter: RESET.\n"); +#endif } void DepthFilter::updateSeedsLoop() { while(!boost::this_thread::interruption_requested()) { - - fprintf(log_,"loop 182\n"); FramePtr frame; { lock_t lock(frame_queue_mut_); @@ -219,8 +240,9 @@ namespace vio { size_t n_updates=0, n_failed_matches=0; lock_t lock(seeds_mut_); std::list::iterator it=seeds_.begin(); - +#if VIO_DEBUG fprintf(log_,"update seed\n"); +#endif 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) @@ -239,7 +261,7 @@ namespace vio { ///TODO add 15 degrees roll orientation Quaterniond q; q = AngleAxisd(0.261799, Vector3d::UnitX()) - * AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY()) + * AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real())-M_PI_2, Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1))); const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) ); @@ -298,7 +320,6 @@ namespace vio { } else if(isnan(z_min)) { - SVO_WARN_STREAM("z_min is NaN"); it = seeds_.erase(it); } else diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 5e1a11b..4a04554 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -90,7 +90,7 @@ 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(); + //core_kfs_.clear(); overlap_kfs_.clear(); // create new frame @@ -170,13 +170,13 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); if(!img_align.run(last_frame_, new_frame_))new_frame_->T_f_w_=init_f.second; reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_); - //assert (false); 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()<<'\n'; + (last_frame_->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( reprojector_.n_trials_ < 10)return RESULT_FAILURE; size_t sfba_n_edges_final=0; double sfba_thresh, sfba_error_init, sfba_error_final; @@ -184,33 +184,32 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() Config::poseOptimThresh(), Config::poseOptimNumIter(), false, new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 || - fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){ + fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI_4){ return RESULT_FAILURE; } 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(),sfba_n_edges_final,time_); new_frame_->T_f_w_ =result.second; new_frame_->Cov_ = result.first; + optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); depth_filter_->addFrame(new_frame_); - if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ + if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()/*10*/){ return RESULT_FAILURE; } - optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); // select keyframe - core_kfs_.insert(new_frame_); + //core_kfs_.insert(new_frame_); setTrackingQuality(sfba_n_edges_final); - if(tracking_quality_ == TRACKING_INSUFFICIENT) + if(tracking_quality_ == TRACKING_INSUFFICIENT || tracking_quality_ == TRACKING_BAD) { - return RESULT_FAILURE; + return RESULT_NO_KEYFRAME; } double depth_mean, depth_min; frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); - if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited + if(!needNewKf(depth_mean))//edited { - //depth_filter_->addFrame(new_frame_); return RESULT_NO_KEYFRAME; } new_frame_->setKeyframe(); @@ -234,7 +233,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() // add keyframe to map map_.addKeyframe(new_frame_); - //std::cout<<"Svo pipline was finished\n"; return RESULT_IS_KEYFRAME; } @@ -268,7 +266,7 @@ void FrameHandlerMono::resetAll() resetCommon(); last_frame_.reset(); new_frame_.reset(); - core_kfs_.clear(); + //core_kfs_.clear(); overlap_kfs_.clear(); depth_filter_->reset(); } @@ -300,7 +298,7 @@ void FrameHandlerMono::setCoreKfs(size_t n_closest) std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(), boost::bind(&pair::second, _1) > boost::bind(&pair::second, _2)); - std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair& i){ core_kfs_.insert(i.first); }); + //std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair& i){ core_kfs_.insert(i.first); }); } void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){ if(value== nullptr)return; diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 56e8709..34e29ff 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -1,19 +1,5 @@ -// This file is part of SVO - Semi-direct Visual Odometry. -// -// Copyright (C) 2014 Christian Forster -// (Robotics and Perception Group, University of Zurich, Switzerland). -// -// SVO is free software: you can redistribute it and/or modify it under the -// terms of the GNU General Public License as published by the Free Software -// Foundation, either version 3 of the License, or any later version. -// -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY -// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS -// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . -#define VIO_DEBUG true +// 0.1 m/s linear velocity +// 0.2 radian/s angular velocity #include #include #include