correct frame and add log file for depth-filter

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.

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())
+            q = AngleAxisd(0.261799, Vector3d::UnitX()) // roll
-                * AngleAxisd(pitch(), Vector3d::UnitY())
+                * AngleAxisd(pitch(), Vector3d::UnitY())// pitch
-                * AngleAxisd(0.0, Vector3d::UnitZ());
+                * 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

@@ -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<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_(1),-filter_->state_(0),M_PI-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),-filter_->state_(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_(1),-filter_->state_(0),M_PI-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),-filter_->state_(2)));
     }
 private:
      Base* filter_= nullptr;

GPU_version/vio/launch/debug.launch

@@ -0,0 +1,21 @@
+<launch>
+
+  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"  launch-prefix="xterm -e gdb --args"/>
+
+    <!-- Camera topic to subscribe to -->
+    <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
+    <param name="vio/imu_topic" value="/SensorNode/imu" type="str" />
+    <param name="vio/cmd_topic" value="/cmd_vel" type="str" />
+    <param name="vio/start" value="/vio/start" type="str" />
+    <param name="vio/stop" value="/vio/stop" type="str" />
+    <param name="vio/odom" value="/vio/getOdom" type="str" />
+    
+    <!-- Camera calibration file -->
+    <rosparam file="$(find vio)/param/px30.yaml" />
+
+  <!-- /node -->
+
+  <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+  <rosparam file="$(find vio)/param/vo_fast.yaml" />
+
+</launch>

GPU_version/vio/launch/live.launch

@@ -1,16 +0,0 @@
-<launch>
-  
-    <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
-    
-        <!-- Camera topic to subscribe to -->
-        <param name="cam_topic" value="/camera/image_raw" type="str" />
-        
-        <!-- Camera calibration file -->
-        <rosparam file="$(find svo_ros)/param/camera_atan.yaml" />
-        
-        <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
-        <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
-
-    </node>
-        
-</launch>

GPU_version/vio/src/depth_filter.cpp

@@ -28,6 +28,8 @@
 #include <vio/matcher.h>
 #include <vio/config.h>
 #include <vio/feature_detection.h>
+#include <sys/types.h>
+#include <sys/stat.h>
 
 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_.empty())lock_t lock(frame_queue_mut_);
-                if(frame_queue_.size() > 2)
+                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 "<<new_features.size()<<" new seeds");
         seeds_updating_halt_ = false;
+        seeds_mut_.unlock();
     }
 
     void DepthFilter::removeKeyframe(FramePtr frame)
     {
         seeds_updating_halt_ = true;
-        lock_t lock(seeds_mut_);
+        fprintf(log_,"remove key frame\n");
+        seeds_mut_.lock();
         std::list<Seed>::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<Seed>::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)

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();

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.x = odom.second.se2().translation()(1);
-      msg_pose_with_cov->pose.pose.position.y = 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));
     }
   }

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)};