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