add M_PI_2 to go from 2d to 3d

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)
 
 

GPU_version/vio/include/vio/depth_filter.h

@@ -20,7 +20,6 @@
 #include <queue>
 #include <boost/thread.hpp>
 #include <boost/function.hpp>
-//#include <vikit/performance_monitor.h>
 #include <vio/global.h>
 #include <vio/feature_detection.h>
 #include <vio/matcher.h>
@@ -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.

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;

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<FramePtr>& coreKeyframes() { return core_kfs_; }
+  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_; }
@@ -62,7 +62,6 @@ public:
       const SE3& T_kf_f,
       const cv::Mat& img,
       const double timestamp);
-  //std::shared_ptr<Imu_Integration> 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<FramePtr> core_kfs_;                      //!< Keyframes in the closer neighbourhood.
   vector< pair<FramePtr,size_t> > 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.

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;

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<Eigen::Matrix<double,3,3>,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<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-filter_->state_(2)));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)+M_PI_2)));
     };
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-filter_->state_(2)));
+        return std::pair<Eigen::Matrix<double,3,3>,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;

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

GPU_version/vio/src/depth_filter.cpp

@@ -28,9 +28,10 @@
 #include <vio/matcher.h>
 #include <vio/config.h>
 #include <vio/feature_detection.h>
+#if VIO_DEBUG
 #include <sys/types.h>
 #include <sys/stat.h>
-
+#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)
+#if VIO_DEBUG
-            SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
+        fprintf(log_,"DepthFilter: Initialized %d new seeds\n",new_features.size());
+#endif
         seeds_updating_halt_ = false;
         seeds_mut_.unlock();
     }
@@ -148,7 +168,9 @@ namespace vio {
     void DepthFilter::removeKeyframe(FramePtr frame)
     {
         seeds_updating_halt_ = true;
+#if VIO_DEBUG
         fprintf(log_,"remove key frame\n");
+#endif
         seeds_mut_.lock();
         std::list<Seed>::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)
+#if VIO_DEBUG
-            SVO_INFO_STREAM("DepthFilter: RESET.");
+        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<Seed>::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

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: "<<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()<<'\n';
+             (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( 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<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); });
+  //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;

GPU_version/vio/src/vo_node.cpp

@@ -1,19 +1,5 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// 0.1 m/s linear velocity
-//
+// 0.2 radian/s angular velocity
-// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
-// (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 <http://www.gnu.org/licenses/>.
-#define VIO_DEBUG true
 #include <ros/ros.h>
 #include <string>
 #include <chrono>