diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index 3fd1a7a..b06db86 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/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) { diff --git a/svo_analysis/scripts/comparison.py b/svo_analysis/scripts/comparison.py index 1651247..8f99f01 100755 --- a/svo_analysis/scripts/comparison.py +++ b/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.legend(loc='upper left') + 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', 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__': diff --git a/svo_ros/include/svo_ros/visualizer.h b/svo_ros/include/svo_ros/visualizer.h index aa6146a..dc8936d 100644 --- a/svo_ros/include/svo_ros/visualizer.h +++ b/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(); diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp index 0c643eb..d14f703 100644 --- a/svo_ros/src/benchmark_node.cpp +++ b/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((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 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 "<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); } } diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp index e207bbe..117165a 100644 --- a/svo_ros/src/visualizer.cpp +++ b/svo_ros/src/visualizer.cpp @@ -44,8 +44,10 @@ Visualizer() : img_pub_level_(vk::getParam("svo/image_publishing_level", 0)), img_pub_nth_(vk::getParam("svo/publish_every_nth_img", 1)), dense_pub_nth_(vk::getParam("svo/publish_every_nth_dense_input", 1)), - T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()), - publish_world_in_cam_frame_(vk::getParam("svo/publish_world_in_cam_frame", true)) + publish_world_in_cam_frame_(vk::getParam("svo/publish_world_in_cam_frame", true)), + publish_map_every_frame_(vk::getParam("svo/publish_map_every_frame", false)), + publish_points_display_time_(vk::getParam("svo/publish_point_display_time", 0)), + T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()) { // Init ROS Marker Publishers pub_frames_ = pnh_.advertise("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& 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( - 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::publishHexacopterMarker( - pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_), - 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)); + vk::output_helper::publishHexacopterMarker( + pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_), + 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)); + if(frame->isKeyframe() || publish_map_every_frame_) 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; } }