void BenchmarkNode::run()
{
// setup visualizer
// dvo::visualization::Switch pause_switch(false), dummy_switch(true);
dvo::visualization::CameraTrajectoryVisualizerInterface* visualizer;
dvo_slam::visualization::GraphVisualizer* graph_visualizer;
if(cfg_.VisualizationRequired())
{
visualizer = new dvo_ros::visualization::RosCameraTrajectoryVisualizer(nh_vis_);
graph_visualizer = new dvo_slam::visualization::GraphVisualizer(*dynamic_cast<dvo_ros::visualization::RosCameraTrajectoryVisualizer*>(visualizer));
//((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(pause_switch, "p");
//((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(dump_camera_pose_, "s");
//
//if(cfg_.RenderVideo)
//{
// ((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(load_camera_pose_, "l");
// ((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->visualizer().getRenderWindow()->SetSize(1280, 960);
//}
}
else
{
visualizer = new dvo::visualization::NoopCameraTrajectoryVisualizer();
}
dvo::util::IdGenerator frame_ids(cfg_.VideoFolder + std::string("/frame_"));
// setup camera parameters
// TODO: load from file
//dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(525.0f, 525.0f, 320.0f, 240.0f);
//fr1
dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(517.3, 516.5, 318.6, 255.3);
//fr2
//dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(520.9f, 521.0f, 325.1f, 249.7f);
//fr3
//dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(535.4f, 539.2f, 320.1f, 247.6f);
dvo::core::RgbdCameraPyramid camera(640, 480, intrinsics);
// setup tracker configuration
dvo_ros::CameraDenseTrackerConfig dynreconfg_cfg = dvo_ros::CameraDenseTrackerConfig::__getDefault__();
dynreconfg_cfg.__fromServer__(nh_private_);
dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();
dvo_ros::util::updateConfigFromDynamicReconfigure(dynreconfg_cfg, cfg);
dvo_slam::KeyframeSlamConfig dynreconfg_slam_cfg = dvo_slam::KeyframeSlamConfig::__getDefault__();
dynreconfg_slam_cfg.__fromServer__(nh_private_);
dvo_slam::KeyframeTrackerConfig frontend_cfg;
dvo_slam::KeyframeGraphConfig backend_cfg;
dvo_slam::updateConfigFromDynamicReconfigure(dynreconfg_slam_cfg, frontend_cfg, backend_cfg);
// setup tracker
//dvo::DenseTracker dense_tracker(intrinsics, cfg);
camera.build(cfg.getNumLevels());
dvo_slam::KeyframeTracker keyframe_tracker(graph_visualizer);
keyframe_tracker.configureTracking(cfg);
keyframe_tracker.configureKeyframeSelection(frontend_cfg);
keyframe_tracker.configureMapping(backend_cfg);
ROS_WARN_STREAM_NAMED("config", "tracker config: \"" << keyframe_tracker.trackingConfiguration() << "\"");
ROS_WARN_STREAM_NAMED("config", "frontend config: \"" << keyframe_tracker.keyframeSelectionConfiguration() << "\"");
ROS_WARN_STREAM_NAMED("config", "backend config: \"" << keyframe_tracker.mappingConfiguration() << "\"");
// initialize first pose
Eigen::Affine3d trajectory, relative;
if(groundtruth_reader_ != 0)
{
dvo_benchmark::findClosestEntry(*groundtruth_reader_, rgbdpair_reader_->entry().RgbTimestamp());
dvo_benchmark::toPoseEigen(groundtruth_reader_->entry(), trajectory);
}
else
{
trajectory.setIdentity();
}
std::string optimized_trajectory_file = cfg_.RgbdPairFile.substr(0, cfg_.RgbdPairFile.find_last_of(".")) + "_opt_traj_final.txt";
std::string edge_error_file = cfg_.RgbdPairFile.substr(0, cfg_.RgbdPairFile.find_last_of(".")) + "_error.txt";
keyframe_tracker.init(trajectory);
std::string folder = cfg_.RgbdPairFile.substr(0, cfg_.RgbdPairFile.find_last_of("/") + 1);
std::vector<dvo_benchmark::RgbdPair> pairs;
rgbdpair_reader_->readAllEntries(pairs);
dvo::core::RgbdImagePyramid::Ptr current;
ROS_INFO("Start interating dataset");
dvo::util::stopwatch sw_online("online", 1), sw_postprocess("postprocess", 1);
sw_online.start();
int loop_counter = 0;
clock_t time_start = clock();
ROS_INFO("cfg_.ShowGroundtruth %i", cfg_.ShowGroundtruth);
ROS_INFO("cfg_.EstimateTrajectory %i", cfg_.EstimateTrajectory);
ROS_INFO("cfg_.ShowEstimate %i", cfg_.ShowEstimate);
for(std::vector<dvo_benchmark::RgbdPair>::iterator it = pairs.begin(); ros::ok() && it != pairs.end(); ++it)
{
loop_counter++;
if(loop_counter)
{
ROS_INFO("loop %i", loop_counter);
}
if (loop_counter == 100){
ROS_INFO("loop 100 time %i",(clock()-time_start)/CLOCKS_PER_SEC);
}
current = load(camera, folder + it->RgbFile(), folder + it->DepthFile());
if(!current) continue;
// pause in the beginning
// renderWhileSwitchAndNotTerminated(visualizer, pause_switch);
// processInput(visualizer);
if(cfg_.ShowGroundtruth)
{
ROS_INFO("enter cfg_.ShowGroundtruth");
Eigen::Affine3d groundtruth_pose;
dvo_benchmark::findClosestEntry(*groundtruth_reader_, it->RgbTimestamp());
dvo_benchmark::toPoseEigen(groundtruth_reader_->entry(), groundtruth_pose);
visualizer->trajectory("groundtruth")->
color(dvo::visualization::Color(255,255,0)).
add(groundtruth_pose);
visualizer->camera("groundtruth")->
color(dvo::visualization::Color(255,255,0)).
update(current->level(0), groundtruth_pose).
show(cfg_.ShowEstimate ? dvo::visualization::CameraVisualizer::ShowCamera : dvo::visualization::CameraVisualizer::ShowCameraAndCloud);
}
if(cfg_.EstimateRequired())
{
if((pairs.end() - it) == 1)
{
ROS_WARN("forcing keyframe");
keyframe_tracker.forceKeyframe();
}
static dvo::util::stopwatch sw_match("match", 100);
sw_match.start();
{
//ROS_INFO("keyframe_tracker before update");
keyframe_tracker.update(current, it->RgbTimestamp(), trajectory);
//ROS_INFO("keyframe_tracker after update");
}
sw_match.stopAndPrint();
if(cfg_.EstimateTrajectory)
{
Eigen::Quaterniond q(trajectory.rotation());
(*trajectory_out_)
<< it->RgbTimestamp() << " "
<< trajectory.translation()(0) << " "
<< trajectory.translation()(1) << " "
<< trajectory.translation()(2) << " "
<< q.x() << " "
<< q.y() << " "
<< q.z() << " "
<< q.w() << " "
<< std::endl;
}
if(cfg_.ShowEstimate)
{
//visualizer->trajectory("estimate")->
// color(dvo::visualization::Color::red()).
// add(trajectory);
visualizer->camera("estimate")->
color(dvo::visualization::Color::red()).
update(current->level(0), trajectory).
show(dvo::visualization::CameraVisualizer::ShowCamera);
}
}
ros::spinOnce();
//if(cfg_.RenderVideo)
//{
// ((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->render(5);
// ((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->visualizer().saveScreenshot(frame_ids.next() + std::string(".png"));
//}
}
ROS_INFO("interating dataset finished");
sw_online.stop();
//std::cerr << "input:" << std::endl;
//std::string tmp;
//std::cin >> tmp;
ROS_INFO("sw_postprocess.start();");
sw_postprocess.start();
sw_postprocess.stop();
ROS_INFO("sw_postprocess.stop();");
sw_online.print();sw_postprocess.print();
// keep visualization alive
if(cfg_.KeepAlive)
{
renderWhileSwitchAndNotTerminated(visualizer/*, dummy_switch*/);
}
ROS_INFO("before serializing");
dvo_slam::serialization::FileSerializer<dvo_slam::serialization::TrajectorySerializer> serializer(optimized_trajectory_file);
keyframe_tracker.serializeMap(serializer);
ROS_INFO("after keyframe_tracker.serializeMap(serializer);");
dvo_slam::serialization::FileSerializer<dvo_slam::serialization::EdgeErrorSerializer> error_serializer(edge_error_file);
keyframe_tracker.serializeMap(error_serializer);
ROS_INFO("after keyframe_tracker.serializeMap(error_serializer);");
}
void BenchmarkNode::run()
{
// setup visualizer
dvo::visualization::Switch pause_switch(true), dummy_switch(true);
dvo::visualization::CameraTrajectoryVisualizerInterface* visualizer;
if(cfg_.VisualizationRequired())
{
visualizer = new dvo::visualization::PclCameraTrajectoryVisualizer(!cfg_.RenderVideo);
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(pause_switch, "p");
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(dump_camera_pose_, "s");
if(cfg_.RenderVideo)
{
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(load_camera_pose_, "l");
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->visualizer().getRenderWindow()->SetSize(1280, 960);
}
}
else
{
visualizer = new dvo::visualization::NoopCameraTrajectoryVisualizer();
}
dvo::util::IdGenerator frame_ids(cfg_.VideoFolder + std::string("/frame_"));
// configure debugging visualizer
dvo::visualization::Visualizer::instance()
.useExternalWaitKey(false)
.enabled(false)
.save(false)
;
// setup camera parameters
// TODO: load from file
//dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(525.0f, 525.0f, 320.0f, 240.0f);
dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(517.3, 516.5, 318.6, 255.3);
// setup tracker configuration
dvo_ros::CameraDenseTrackerConfig dynreconfg_cfg = dvo_ros::CameraDenseTrackerConfig::__getDefault__();
dynreconfg_cfg.__fromServer__(nh_private_);
dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();
dvo_ros::util::updateConfigFromDynamicReconfigure(dynreconfg_cfg, cfg);
ROS_WARN_STREAM_NAMED("config", "config: \"" << cfg << "\"");
// setup tracker
dvo::DenseTracker dense_tracker(intrinsics, cfg);
// initialize first pose
Eigen::Affine3d trajectory, relative;
if(groundtruth_reader_ != 0)
{
dvo_benchmark::findClosestEntry(*groundtruth_reader_, rgbdpair_reader_->entry().RgbTimestamp());
dvo_benchmark::toPoseEigen(groundtruth_reader_->entry(), trajectory);
}
else
{
trajectory.setIdentity();
}
std::string folder = cfg_.RgbdPairFile.substr(0, cfg_.RgbdPairFile.find_last_of("/") + 1);
std::vector<dvo_benchmark::RgbdPair> pairs;
rgbdpair_reader_->readAllEntries(pairs);
dvo::core::RgbdImagePyramid::Ptr reference, current;
for(std::vector<dvo_benchmark::RgbdPair>::iterator it = pairs.begin(); ros::ok() && it != pairs.end(); ++it)
{
reference = current;
current = load(folder + it->RgbFile(), folder + it->DepthFile());
if(!reference)
{
createReferenceCamera(visualizer, current->level(0), intrinsics, trajectory);
}
// pause in the beginning
renderWhileSwitchAndNotTerminated(visualizer, pause_switch);
processInput(visualizer);
if(!reference)
{
continue;
}
if((it->RgbTimestamp() - pairs.front().RgbTimestamp()).toSec() < 5 || (pairs.back().RgbTimestamp() - it->RgbTimestamp()).toSec() < 5)
{
visualizer->camera("reference")->show();
}
else
{
visualizer->camera("reference")->hide();
}
if(cfg_.ShowGroundtruth)
{
Eigen::Affine3d groundtruth_pose;
dvo_benchmark::findClosestEntry(*groundtruth_reader_, it->RgbTimestamp());
dvo_benchmark::toPoseEigen(groundtruth_reader_->entry(), groundtruth_pose);
visualizer->trajectory("groundtruth")->
color(dvo::visualization::Color::green()).
add(groundtruth_pose);
visualizer->camera("groundtruth")->
color(dvo::visualization::Color::green()).
update(current->level(0), intrinsics, groundtruth_pose).
show(cfg_.ShowEstimate ? dvo::visualization::CameraVisualizer::ShowCamera : dvo::visualization::CameraVisualizer::ShowCameraAndCloud);
}
if(cfg_.EstimateRequired())
{
static dvo::util::stopwatch sw_match("match", 100);
sw_match.start();
{
dense_tracker.match(*reference, *current, relative);
}
sw_match.stopAndPrint();
trajectory = trajectory * relative;
if(cfg_.EstimateTrajectory)
{
Eigen::Quaterniond q(trajectory.rotation());
(*trajectory_out_)
<< it->RgbTimestamp() << " "
<< trajectory.translation()(0) << " "
<< trajectory.translation()(1) << " "
<< trajectory.translation()(2) << " "
<< q.x() << " "
<< q.y() << " "
<< q.z() << " "
<< q.w() << " "
<< std::endl;
}
if(cfg_.ShowEstimate)
{
visualizer->trajectory("estimate")->
color(dvo::visualization::Color::red()).
add(trajectory);
visualizer->camera("estimate")->
color(dvo::visualization::Color::red()).
update(current->level(0), intrinsics, trajectory).
show(dvo::visualization::CameraVisualizer::ShowCameraAndCloud);
}
}
if(cfg_.RenderVideo)
{
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->render(5);
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->visualizer().saveScreenshot(frame_ids.next() + std::string(".png"));
}
}
// keep visualization alive
renderWhileSwitchAndNotTerminated(visualizer, dummy_switch);
}
