diff --git a/svo/.idea/.gitignore b/svo/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/svo/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/svo/.idea/misc.xml b/svo/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/svo/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/svo/.idea/modules.xml b/svo/.idea/modules.xml new file mode 100644 index 0000000..4c8cf2b --- /dev/null +++ b/svo/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/svo/.idea/svo.iml b/svo/.idea/svo.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/svo/.idea/svo.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/svo/.idea/vcs.xml b/svo/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/svo/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index 4facc51..b8bba92 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/svo/src/frame_handler_mono.cpp @@ -156,7 +156,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() return RESULT_FAILURE; } - // pose optimization + // pose optimization subject to change for adding IMU SVO_START_TIMER("pose_optimizer"); size_t sfba_n_edges_final; double sfba_thresh, sfba_error_init, sfba_error_final; diff --git a/svo_msgs/.idea/.gitignore b/svo_msgs/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/svo_msgs/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/svo_msgs/.idea/misc.xml b/svo_msgs/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/svo_msgs/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/svo_msgs/.idea/modules.xml b/svo_msgs/.idea/modules.xml new file mode 100644 index 0000000..668cc83 --- /dev/null +++ b/svo_msgs/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/svo_msgs/.idea/svo_msgs.iml b/svo_msgs/.idea/svo_msgs.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/svo_msgs/.idea/svo_msgs.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/svo_msgs/.idea/vcs.xml b/svo_msgs/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/svo_msgs/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/svo_ros/.idea/modules.xml b/svo_ros/.idea/modules.xml new file mode 100644 index 0000000..eb3cacf --- /dev/null +++ b/svo_ros/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/svo_ros/.idea/svo_ros.iml b/svo_ros/.idea/svo_ros.iml new file mode 100644 index 0000000..bc2cd87 --- /dev/null +++ b/svo_ros/.idea/svo_ros.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/svo_ros/.idea/vcs.xml b/svo_ros/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/svo_ros/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/svo_ros/.idea/workspace.xml b/svo_ros/.idea/workspace.xml new file mode 100644 index 0000000..09c1504 --- /dev/null +++ b/svo_ros/.idea/workspace.xml @@ -0,0 +1,627 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1615974669888 + + + 1617193017745 + + + 1617640897291 + + + + + + + + + + + + \ No newline at end of file diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp deleted file mode 100644 index a1c96e1..0000000 --- a/svo_ros/src/benchmark_node.cpp +++ /dev/null @@ -1,272 +0,0 @@ -// This file is part of SVO - Semi-direct Visual Odometry. -// -// Copyright (C) 2014 Christian Forster -// (Robotics and Perception Group, University of Zurich, Switzerland). -// -// SVO is free software: you can redistribute it and/or modify it under the -// terms of the GNU General Public License as published by the Free Software -// Foundation, either version 3 of the License, or any later version. -// -// SVO is distributed in the hope that it will be useful, but WITHOUT ANY -// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS -// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program. If not, see . - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace svo { - -class BenchmarkNode -{ - svo::FrameHandlerMono* vo_; - svo::Visualizer visualizer_; - int frame_count_; - std::ofstream trace_est_pose_; - std::ofstream trace_trans_error_; - std::ofstream trace_rot_error_; - std::ofstream trace_depth_error_; - vk::AbstractCamera* cam_; - double img_noise_sigma_; - -public: - BenchmarkNode(ros::NodeHandle& nh); - ~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 addNoiseToImage(cv::Mat& img, double sigma); - void runBenchmark(const std::string& dataset_dir); - void runBlenderBenchmark(const std::string& dataset_dir); -}; - -BenchmarkNode::BenchmarkNode(ros::NodeHandle& nh) : - vo_(NULL), - frame_count_(0), - img_noise_sigma_(vk::getParam("svo/dataset_noise_sigma", 0.0)) -{ - // Create Camera - if(!vk::camera_loader::loadFromRosNs("svo", cam_)) - throw std::runtime_error("Camera model not correctly specified."); - - // create pose tracefile - std::string trace_est_name(Config::traceDir() + "/traj_estimate.txt"); - trace_est_pose_.open(trace_est_name.c_str()); - if(trace_est_pose_.fail()) - throw std::runtime_error("Could not create tracefile. Does folder exist?"); - - // Initialize VO - vo_ = new svo::FrameHandlerMono(cam_); - vo_->start(); -} - -BenchmarkNode::~BenchmarkNode() -{ - delete vo_; - delete cam_; -} - -void BenchmarkNode::tracePose(const SE3& T_w_f, const double timestamp) -{ - Quaterniond q(T_w_f.unit_quaternion()); - Vector3d p(T_w_f.translation()); - trace_est_pose_.precision(15); - trace_est_pose_.setf(std::ios::fixed, std::ios::floatfield ); - 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() << std::endl; -} - -void BenchmarkNode::tracePoseError(const SE3& T_f_gt, const double timestamp) -{ - Vector3d et(T_f_gt.translation()); // translation error - trace_trans_error_.precision(15); - 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() << " " << 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() << " " << 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_true = depthmap.at((int) ftr->px[1], (int) ftr->px[0]); - trace_depth_error_ << frame->id_ << " " - << depth_estimated-depth_true << std::endl; - } - }); -} - -void BenchmarkNode::runBenchmark(const std::string& dataset_dir) -{ - // create image reader and load dataset - std::string filename_benchmark(dataset_dir + "/images.txt"); - vk::FileReader dataset_reader(filename_benchmark); - dataset_reader.skipComments(); - if(!dataset_reader.next()) { - SVO_ERROR_STREAM("Failed opening dataset: "< dataset; - dataset_reader.readAllEntries(dataset); - - // process dataset - int first_frame = vk::getParam("svo/dataset_first_frame", 0); - for(auto it = dataset.begin(); it != dataset.end() && ros::ok(); ++it, ++frame_count_) - { - if(frame_count_ < first_frame) - continue; - std::string img_filename(dataset_dir + "/" + it->image_name_); - cv::Mat img(cv::imread(img_filename, 0)); - if(img.empty()) { - SVO_ERROR_STREAM("Reading image "<addImage(img, it->timestamp_); - visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_); - visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map()); - if(vo_->stage() == svo::FrameHandlerMono::STAGE_DEFAULT_FRAME) - tracePose(vo_->lastFrame()->T_f_w_.inverse(), it->timestamp_); - } -} - -void BenchmarkNode::addNoiseToImage(cv::Mat& img, double sigma) -{ - uint8_t* p = (uint8_t*) img.data; - uint8_t* p_end = img.ptr(img.rows, img.cols); - while(p != p_end) - { - int val = *p + vk::Sample::gaussian(sigma) + 0.5; - *p = std::max(std::min(val, 255), 0); - ++p; - } -} - -void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir) -{ - // create image reader and load dataset - std::string filename_benchmark(dataset_dir + "/trajectory.txt"); - vk::FileReader dataset_reader(filename_benchmark); - dataset_reader.skipComments(); - if(!dataset_reader.next()) { - SVO_ERROR_STREAM("Failed opening dataset: "< 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"); - 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, 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 "< 0) - addNoiseToImage(img, img_noise_sigma_); - 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 - if(frame_count_ == 0) - { - // set reference frame at ground-truth pose - FramePtr frame_ref(new Frame(cam_, img, it->timestamp_)); - frame_ref->T_f_w_ = T_w_gt.inverse(); - - // extract features, generate features with 3D points - svo::feature_detection::FastDetector detector( - cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()); - detector.detect(frame_ref.get(), frame_ref->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref->fts_); - std::for_each(frame_ref->fts_.begin(), frame_ref->fts_.end(), [&](Feature* ftr) { - Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at(ftr->px[1], ftr->px[0]); - Eigen::Vector3d pt_pos_world = frame_ref->T_f_w_.inverse()*pt_pos_cur; - svo::Point* point = new svo::Point(pt_pos_world, ftr); - ftr->point = point; - }); - SVO_INFO_STREAM("Added "<nObs()<<" 3d pts to the reference frame."); - vo_->setFirstFrame(frame_ref); - SVO_INFO_STREAM("Set reference frame."); - } - else - { - SVO_DEBUG_STREAM("Processing image "<image_name_<<"."); - vo_->addImage(img, it->timestamp_); - visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_); - visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map()); - } - - if(vo_->stage() != svo::FrameHandlerMono::STAGE_DEFAULT_FRAME) - { - SVO_ERROR_STREAM("SVO failed before entire dataset could be processed."); - break; - } - - // Compute pose error and trace to file - 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); - } -} - -} // namespace svo - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "svo"); - ros::NodeHandle nh; - svo::BenchmarkNode benchmark(nh); - std::string benchmark_dir(vk::getParam("svo/dataset_directory")); - if(vk::getParam("svo/dataset_is_blender", false)) - benchmark.runBlenderBenchmark(benchmark_dir); - else - benchmark.runBenchmark(benchmark_dir); - printf("BenchmarkNode finished.\n"); - return 0; -} diff --git a/vio_svo/.idea/.gitignore b/vio_svo/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/vio_svo/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/vio_svo/.idea/misc.xml b/vio_svo/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/vio_svo/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/vio_svo/.idea/modules.xml b/vio_svo/.idea/modules.xml new file mode 100644 index 0000000..eb3cacf --- /dev/null +++ b/vio_svo/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/vio_svo/.idea/svo_ros.iml b/vio_svo/.idea/svo_ros.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/vio_svo/.idea/svo_ros.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/vio_svo/.idea/vcs.xml b/vio_svo/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/vio_svo/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/svo_ros/CMakeLists.txt b/vio_svo/CMakeLists.txt similarity index 98% rename from svo_ros/CMakeLists.txt rename to vio_svo/CMakeLists.txt index d0ce982..059a8bf 100644 --- a/svo_ros/CMakeLists.txt +++ b/vio_svo/CMakeLists.txt @@ -1,4 +1,4 @@ -SET(PROJECT_NAME svo_ros) +SET(PROJECT_NAME vio_svo) PROJECT(${PROJECT_NAME}) CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo diff --git a/svo_ros/include/svo_ros/dataset_img.h b/vio_svo/include/vio_svo/dataset_img.h similarity index 100% rename from svo_ros/include/svo_ros/dataset_img.h rename to vio_svo/include/vio_svo/dataset_img.h diff --git a/svo_ros/include/svo_ros/visualizer.h b/vio_svo/include/vio_svo/visualizer.h similarity index 100% rename from svo_ros/include/svo_ros/visualizer.h rename to vio_svo/include/vio_svo/visualizer.h diff --git a/svo_ros/launch/Px30.launch b/vio_svo/launch/Px30.launch similarity index 65% rename from svo_ros/launch/Px30.launch rename to vio_svo/launch/Px30.launch index 53f9317..6ff3dab 100644 --- a/svo_ros/launch/Px30.launch +++ b/vio_svo/launch/Px30.launch @@ -1,15 +1,16 @@ - + + - + - + diff --git a/svo_ros/launch/live.launch b/vio_svo/launch/live.launch similarity index 100% rename from svo_ros/launch/live.launch rename to vio_svo/launch/live.launch diff --git a/svo_ros/launch/test.launch b/vio_svo/launch/test.launch similarity index 71% rename from svo_ros/launch/test.launch rename to vio_svo/launch/test.launch index f83d479..388b536 100644 --- a/svo_ros/launch/test.launch +++ b/vio_svo/launch/test.launch @@ -1,15 +1,15 @@ - + - + - + diff --git a/svo_ros/package.xml b/vio_svo/package.xml similarity index 98% rename from svo_ros/package.xml rename to vio_svo/package.xml index aaa8fd3..ff9eab3 100644 --- a/svo_ros/package.xml +++ b/vio_svo/package.xml @@ -1,6 +1,6 @@ - svo_ros + vio_svo 0.1.0 Monocular Visual Odometry - ROS Nodes diff --git a/svo_ros/param/camera_atan.yaml b/vio_svo/param/camera_atan.yaml similarity index 100% rename from svo_ros/param/camera_atan.yaml rename to vio_svo/param/camera_atan.yaml diff --git a/svo_ros/param/camera_pinhole.yaml b/vio_svo/param/camera_pinhole.yaml similarity index 100% rename from svo_ros/param/camera_pinhole.yaml rename to vio_svo/param/camera_pinhole.yaml diff --git a/svo_ros/param/px30.yaml b/vio_svo/param/px30.yaml similarity index 100% rename from svo_ros/param/px30.yaml rename to vio_svo/param/px30.yaml diff --git a/svo_ros/param/vo_accurate.yaml b/vio_svo/param/vo_accurate.yaml similarity index 100% rename from svo_ros/param/vo_accurate.yaml rename to vio_svo/param/vo_accurate.yaml diff --git a/svo_ros/param/vo_fast.yaml b/vio_svo/param/vo_fast.yaml similarity index 100% rename from svo_ros/param/vo_fast.yaml rename to vio_svo/param/vo_fast.yaml diff --git a/svo_ros/rqt.perspective b/vio_svo/rqt.perspective similarity index 100% rename from svo_ros/rqt.perspective rename to vio_svo/rqt.perspective diff --git a/svo_ros/rviz_config.rviz b/vio_svo/rviz_config.rviz similarity index 100% rename from svo_ros/rviz_config.rviz rename to vio_svo/rviz_config.rviz diff --git a/svo_ros/src/visualizer.cpp b/vio_svo/src/visualizer.cpp similarity index 99% rename from svo_ros/src/visualizer.cpp rename to vio_svo/src/visualizer.cpp index 3cda024..511c167 100644 --- a/svo_ros/src/visualizer.cpp +++ b/vio_svo/src/visualizer.cpp @@ -14,7 +14,7 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include +#include #include #include #include diff --git a/svo_ros/src/vo_node.cpp b/vio_svo/src/vo_node.cpp similarity index 64% rename from svo_ros/src/vo_node.cpp rename to vio_svo/src/vo_node.cpp index b7cdde4..8fd503f 100644 --- a/svo_ros/src/vo_node.cpp +++ b/vio_svo/src/vo_node.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include #include @@ -44,40 +44,45 @@ public: vk::AbstractCamera* cam_; FILE* logtime; bool quit_; + boost::thread* vio_th; VoNode(); ~VoNode(); void imgCb(const sensor_msgs::ImageConstPtr& msg); + void imuCb(const sensor_msgs::ImuPtr& imu); + void vio(); }; VoNode::VoNode() : vo_(NULL), - publish_markers_(vk::getParam("svo/publish_markers", true)), + publish_markers_(vk::getParam("vio/publish_markers", true)), cam_(NULL), quit_(false) { // Create Camera - if(!vk::camera_loader::loadFromRosNs("svo", cam_)) + if(!vk::camera_loader::loadFromRosNs("vio", cam_)) throw std::runtime_error("Camera model not correctly specified."); - // Get initial position and orientation visualizer_.T_world_from_vision_ = Sophus::SE3( - vk::rpy2dcm(Vector3d(vk::getParam("svo/init_rx", 0.0), - vk::getParam("svo/init_ry", 0.0), - vk::getParam("svo/init_rz", 0.0))), - Eigen::Vector3d(vk::getParam("svo/init_tx", 0.0), - vk::getParam("svo/init_ty", 0.0), - vk::getParam("svo/init_tz", 0.0))); - + vk::rpy2dcm(Vector3d(vk::getParam("vio/init_rx", 0.0), + vk::getParam("vio/init_ry", 0.0), + vk::getParam("vio/init_rz", 0.0))), + Eigen::Vector3d(vk::getParam("vio/init_tx", 0.0), + vk::getParam("vio/init_ty", 0.0), + vk::getParam("vio/init_tz", 0.0))); // Init VO and start vo_ = new svo::FrameHandlerMono(cam_); vo_->start(); logtime = fopen((std::string(getenv("HOME"))+"/Project/time.txt").c_str(), "w+"); + vio_th=new boost::thread(&VoNode::vio,this); } VoNode::~VoNode() { + quit_=true; fclose(logtime); + vio_th->join(); + delete vio_th; delete logtime; delete vo_; delete cam_; @@ -85,44 +90,53 @@ VoNode::~VoNode() void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) { - auto start=std::chrono::steady_clock::now(); try { - cv::Mat img; - img = cv_bridge::toCvShare(msg, "mono8")->image; + ROS_INFO("image back"); + auto start=std::chrono::steady_clock::now(); + cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image; vo_->addImage(img, msg->header.stamp.toSec()); visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec()); - - if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED) visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map()); - if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED) usleep(100000); fprintf(logtime, "%s\n",std::to_string(std::chrono::duration(std::chrono::steady_clock::now()-start).count()).c_str()); + } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); } } +void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) { + ROS_INFO("imu back"); +} +void VoNode::vio(){ + ros::NodeHandle nh; + std::cout << "create vo_node" << std::endl; + // subscribe to cam msgs + image_transport::ImageTransport it(nh); + image_transport::Subscriber it_sub = it.subscribe(vk::getParam("vio/cam_topic", "camera/image_raw"), 5, &svo::VoNode::imgCb, this); + // start processing callbacks + while(ros::ok() && !quit_) + { + ros::spinOnce(); + // TODO check when last image was processed. when too long ago. publish warning that no msgs are received! + } + +} } // namespace svo int main(int argc, char **argv) { - ros::init(argc, argv, "svo"); - ros::NodeHandle nh; - std::cout << "create vo_node" << std::endl; + ros::init(argc, argv, "vio"); svo::VoNode vo_node; - - // subscribe to cam msgs - image_transport::ImageTransport it(nh); - image_transport::Subscriber it_sub = it.subscribe(vk::getParam("svo/cam_topic", "camera/image_raw"), 5, &svo::VoNode::imgCb, &vo_node); - + ros::NodeHandle nh; + ros::Subscriber imu_sub=nh.subscribe(vk::getParam("vio/imu_topic", "imu/raw"),10,&svo::VoNode::imuCb, &vo_node); // start processing callbacks while(ros::ok() && !vo_node.quit_) { - ros::spinOnce(); - // TODO check when last image was processed. when too long ago. publish warning that no msgs are received! + ros::spinOnce(); + // TODO check when last image was processed. when too long ago. publish warning that no msgs are received! } - printf("SVO terminated.\n"); return 0; }