worked on visualization

svo/src/frame_handler_mono.cpp

@@ -174,6 +174,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   SVO_STOP_TIMER("point_optimizer");
 
   // select keyframe
+  core_kfs_.insert(new_frame_);
   setTrackingQuality(sfba_n_edges_final);
   if(tracking_quality_ == TRACKING_INSUFFICIENT)
     return RESULT_FAILURE;
@@ -194,7 +195,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   map_.point_candidates_.addCandidatePointToFrame(new_frame_);
 
   // optional bundle adjustment
-  core_kfs_.insert(new_frame_);
 #ifdef USE_BUNDLE_ADJUSTMENT
   if(Config::lobaNumIter() > 0)
   {

svo_analysis/scripts/comparison.py

@@ -42,6 +42,7 @@ def distances_along_trajectory(traj):
 def compare_results(comp_params, results_dir, comparison_dir):
     print('run comparison: '+comp_params['comparison_name'])
            
+    line_styles = ['-','--',':']
     
     # -------------------------------------------------------------------------
     # plot trajectory:
@@ -53,10 +54,10 @@ def compare_results(comp_params, results_dir, comparison_dir):
         for i, exp in enumerate(exp_set['experiments']):
             data = np.loadtxt(os.path.join(results_dir, exp, 'traj_estimate.txt'))
             if i == 0:
-                base_plot, = ax.plot(data[:,1], data[:,2], label=exp_set['label'])
+                base_plot, = ax.plot(data[:,1], data[:,2], label=exp_set['label'], linestyle=line_styles[np.mod(i, len(line_styles))])
             else:
-                ax.plot(data[:,1], data[:,2], color=base_plot.get_color())
+                ax.plot(data[:,1], data[:,2], color=base_plot.get_color(), linestyle= line_styles[np.mod(i, len(line_styles))])
-    ax.legend(loc='upper left')
+    ax.legend(loc='upper left', ncol=2)
     save_figure(fig, 'trajectory', comparison_dir)
     
     
@@ -81,15 +82,18 @@ def compare_results(comp_params, results_dir, comparison_dir):
                       +str(np.shape(e)[0]-np.shape(distances)[0]))
                 e = e[0:np.shape(distances)[0]]
                 
-            distances = distances[0:np.shape(e)[0]+1]
+            distances = distances[0:np.shape(e)[0]]
-           
+            
+            print '--'
+            print np.shape(e)
+            print np.shape(distances)
             
             if i == 0:
-                base_plot, = ax.plot(distances, e*1000, label=exp_set['label'])
+                base_plot, = ax.plot(distances, e*1000, label=exp_set['label'], linestyle= line_styles[np.mod(i, len(line_styles))])
             else:
-                ax.plot(distances, e*1000, color=base_plot.get_color())
+                ax.plot(distances, e*1000, color=base_plot.get_color(), linestyle= line_styles[np.mod(i, len(line_styles))])
                 
-    ax.legend(loc='upper left')
+    ax.legend(loc='upper left', ncol=2)
     save_figure(fig, 'translation_error', comparison_dir)
   
 if __name__ == '__main__':

svo_ros/include/svo_ros/visualizer.h

@@ -59,6 +59,8 @@ public:
   image_transport::Publisher pub_images_;
   tf::TransformBroadcaster br_;
   bool publish_world_in_cam_frame_;
+  bool publish_map_every_frame_;
+  ros::Duration publish_points_display_time_;
   SE3 T_world_from_vision_;
 
   Visualizer();

svo_ros/src/benchmark_node.cpp

@@ -46,6 +46,7 @@ class BenchmarkNode
   std::ofstream trace_est_pose_;
   std::ofstream trace_trans_error_;
   std::ofstream trace_rot_error_;
+  std::ofstream trace_depth_error_;
   vk::AbstractCamera* cam_;
 
 public:
@@ -53,6 +54,7 @@ public:
   ~BenchmarkNode();
   void tracePose(const SE3& T_w_f, const double timestamp);
   void tracePoseError(const SE3& T_f_gt, const double timestamp);
+  void traceDepthError(const FramePtr& frame, const cv::Mat& depthmap);
   void runBenchmark(const std::string& dataset_dir);
   void runBlenderBenchmark(const std::string& dataset_dir);
 };
@@ -91,7 +93,7 @@ void BenchmarkNode::tracePose(const SE3& T_w_f, const double timestamp)
   trace_est_pose_ << timestamp << " ";
   trace_est_pose_.precision(6);
   trace_est_pose_ << p.x() << " " << p.y() << " " << p.z() << " "
-                  << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
+                  << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;
 }
 
 void BenchmarkNode::tracePoseError(const SE3& T_f_gt, const double timestamp)
@@ -101,13 +103,27 @@ void BenchmarkNode::tracePoseError(const SE3& T_f_gt, const double timestamp)
   trace_trans_error_.setf(std::ios::fixed, std::ios::floatfield );
   trace_trans_error_ << timestamp << " ";
   trace_trans_error_.precision(6);
-  trace_trans_error_ << et.x() << " " << et.y() << " " << et.z() << " " << endl;
+  trace_trans_error_ << et.x() << " " << et.y() << " " << et.z() << " " << std::endl;
   Vector3d er(vk::dcm2rpy(T_f_gt.rotation_matrix())); // rotation error in roll-pitch-yaw
   trace_rot_error_.precision(15);
   trace_rot_error_.setf(std::ios::fixed, std::ios::floatfield );
   trace_rot_error_ << timestamp << " ";
   trace_rot_error_.precision(6);
-  trace_rot_error_ << er.x() << " " << er.y() << " " << er.z() << " " << endl;
+  trace_rot_error_ << er.x() << " " << er.y() << " " << er.z() << " " << std::endl;
+}
+
+void BenchmarkNode::traceDepthError(const FramePtr& frame, const cv::Mat& depthmap)
+{
+  trace_depth_error_.precision(6);
+  std::for_each(frame->fts_.begin(), frame->fts_.end(), [&](Feature* ftr){
+    if(ftr->point != NULL)
+    {
+      double depth_estimated = (ftr->point->pos_-frame->pos()).norm();
+      double depth_measured = depthmap.at<float>((int) ftr->px[1], (int) ftr->px[0]);
+      trace_depth_error_ << frame->id_ << " "
+                         << depth_estimated-depth_estimated << std::endl;
+    }
+  });
 }
 
 void BenchmarkNode::runBenchmark(const std::string& dataset_dir)
@@ -156,23 +172,26 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
   std::vector<vk::blender_utils::file_format::ImageNameAndPose> dataset;
   dataset_reader.readAllEntries(dataset);
 
+  // create tracefiles
   trace_trans_error_.open(Config::traceDir() + "/translation_error.txt");
   trace_rot_error_.open(Config::traceDir() + "/orientation_error.txt");
-  if(trace_trans_error_.fail() || trace_rot_error_.fail())
+  trace_depth_error_.open(Config::traceDir() + "/depth_error.txt");
+  if(trace_trans_error_.fail() || trace_rot_error_.fail() || trace_depth_error_.fail())
     throw std::runtime_error("Could not create tracefile. Does folder exist?");
 
   // process dataset
   for(auto it = dataset.begin(); it != dataset.end() && ros::ok(); ++it, ++frame_count_)
   {
-    // Read image
+    // Read image, ground-truth depth-map and ground-truth pose
     std::string img_filename(dataset_dir + "/img/" + it->image_name_ + "_0.png");
     cv::Mat img(cv::imread(img_filename, 0));
     if(img.empty()) {
       SVO_ERROR_STREAM("Reading image "<<img_filename<<" failed.");
       return;
     }
-
+    cv::Mat depthmap;
-    // Read ground-truth pose
+    vk::blender_utils::loadBlenderDepthmap(
+        dataset_dir+"/depth/"+it->image_name_+"_0.depth", *cam_, depthmap);
     Sophus::SE3 T_w_gt(it->q_, it->t_);
 
     // Set reference frame with depth
@@ -182,11 +201,6 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
       FramePtr frame_ref(new Frame(cam_, img, it->timestamp_));
       frame_ref->T_f_w_ = T_w_gt.inverse();
 
-      // load ground-truth depth
-      cv::Mat depthmap;
-      vk::blender_utils::loadBlenderDepthmap(
-          dataset_dir+"/depth/"+it->image_name_+"_0.depth", *cam_, depthmap);
-
       // extract features, generate features with 3D points
       svo::feature_detection::FastDetector detector(
           cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels());
@@ -219,6 +233,7 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
     Sophus::SE3 T_f_gt(vo_->lastFrame()->T_f_w_*T_w_gt);
     tracePoseError(T_f_gt, it->timestamp_);
     tracePose(vo_->lastFrame()->T_f_w_.inverse(), it->timestamp_);
+    traceDepthError(vo_->lastFrame(), depthmap);
   }
 }
 

svo_ros/src/visualizer.cpp

@@ -44,8 +44,10 @@ Visualizer() :
     img_pub_level_(vk::getParam<int>("svo/image_publishing_level", 0)),
     img_pub_nth_(vk::getParam<int>("svo/publish_every_nth_img", 1)),
     dense_pub_nth_(vk::getParam<int>("svo/publish_every_nth_dense_input", 1)),
-    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()),
+    publish_world_in_cam_frame_(vk::getParam<bool>("svo/publish_world_in_cam_frame", true)),
-    publish_world_in_cam_frame_(vk::getParam<bool>("svo/publish_world_in_cam_frame", true))
+    publish_map_every_frame_(vk::getParam<bool>("svo/publish_map_every_frame", false)),
+    publish_points_display_time_(vk::getParam<double>("svo/publish_point_display_time", 0)),
+    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
 {
   // Init ROS Marker Publishers
   pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
@@ -89,7 +91,7 @@ void Visualizer::publishMinimal(
     pub_info_.publish(msg_info);
   }
 
-  if(frame == 0)
+  if(frame == NULL)
   {
     if(pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_PAUSED)
     {
@@ -200,22 +202,24 @@ void Visualizer::visualizeMarkers(
     const set<FramePtr>& core_kfs,
     const Map& map)
 {
-  if(frame != NULL)
+  if(frame == NULL)
+    return;
+
+  vk::output_helper::publishTfTransform(
+      frame->T_f_w_*T_world_from_vision_.inverse(),
+      ros::Time(frame->timestamp_), "cam_pos", "world", br_);
+
+  if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
   {
-    vk::output_helper::publishTfTransform(
+    vk::output_helper::publishHexacopterMarker(
-        frame->T_f_w_*T_world_from_vision_.inverse(),
+        pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_),
-        ros::Time(frame->timestamp_), "cam_pos", "world", br_);
+        1, 0, 0.3, Vector3d(0.,0.,1.));
-    if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
+    vk::output_helper::publishPointMarker(
-    {
+        pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
-      vk::output_helper::publishHexacopterMarker(
+        ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
-          pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_),
+    if(frame->isKeyframe() || publish_map_every_frame_)
-          1, 0, 0.3, Vector3d(0.,0.,1.));
-      vk::output_helper::publishPointMarker(
-          pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
-          ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
       publishMapRegion(core_kfs);
-      removeDeletedPts(map);
+    removeDeletedPts(map);
-    }
   }
 }
 
@@ -257,7 +261,8 @@ void Visualizer::displayKeyframeWithMps(const FramePtr& frame, int ts)
 
     vk::output_helper::publishPointMarker(
         pub_points_, T_world_from_vision_*(*it)->point->pos_, "pts",
-        ros::Time::now(), (*it)->point->id_, 0, 0.005, Vector3d(1.0, 0., 1.0));
+        ros::Time::now(), (*it)->point->id_, 0, 0.005, Vector3d(1.0, 0., 1.0),
+        publish_points_display_time_);
     (*it)->point->last_published_ts_ = ts;
   }
 }