public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

worked on visualization

Browse Source
This commit is contained in:
Christian Forster
2014-05-16 14:38:13 +02:00
parent 0570f17dc6
commit 11b5b62c9e
5 changed files with 65 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/src/frame_handler_mono.cpp
+1 -1
View File
@@ -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
+12 -8
View File
@@ -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__':
svo_ros/include/svo_ros/visualizer.h
+2
View File
@@ -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
+27 -12
View File
@@ -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;
}
// Read ground-truth pose
cv::Mat depthmap;
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
+23 -18
View File
@@ -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(
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;
}
}