From c2a52c68adfef57701e90602f20e3d70d9e28633 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Wed, 12 Mar 2014 22:28:31 +0100 Subject: [PATCH] removed remote_node --- svo_ros/CMakeLists.txt | 2 - svo_ros/include/svo_ros/dataset_imu.h | 70 ----------- svo_ros/src/remote_node.cpp | 169 -------------------------- 3 files changed, 241 deletions(-) delete mode 100644 svo_ros/include/svo_ros/dataset_imu.h delete mode 100644 svo_ros/src/remote_node.cpp diff --git a/svo_ros/CMakeLists.txt b/svo_ros/CMakeLists.txt index 8fcd739..192de3c 100644 --- a/svo_ros/CMakeLists.txt +++ b/svo_ros/CMakeLists.txt @@ -58,5 +58,3 @@ ADD_EXECUTABLE(vo src/vo_node.cpp) TARGET_LINK_LIBRARIES(vo svo_visualizer) ADD_EXECUTABLE(benchmark src/benchmark_node.cpp) TARGET_LINK_LIBRARIES(benchmark svo_visualizer) -ADD_EXECUTABLE(remote src/remote_node.cpp) -TARGET_LINK_LIBRARIES(remote svo_visualizer) \ No newline at end of file diff --git a/svo_ros/include/svo_ros/dataset_imu.h b/svo_ros/include/svo_ros/dataset_imu.h deleted file mode 100644 index 9ee5e66..0000000 --- a/svo_ros/include/svo_ros/dataset_imu.h +++ /dev/null @@ -1,70 +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 . - -#ifndef DATASET_IMU_H_ -#define DATASET_IMU_H_ - -namespace FileType { - -#include -#include -#include - -using ::Eigen::Quaterniond; -using ::Eigen::Vector3d; - -/// Specification of Dataset which contains: Camera Image, Gyroscope Rotation -class DatasetImu -{ -public: - DatasetImu() {} - virtual ~DatasetImu() {} - - double stamp_; - Quaterniond q_imu_; - - friend std::ostream& operator <<(std::ostream& out, const DatasetImu& pair); - friend std::istream& operator >>(std::istream& in, DatasetImu& pair); -}; - -std::ostream& operator <<(std::ostream& out, const DatasetImu& gt) -{ - out - << gt.stamp_ << " " - << gt.q_imu_.x() << " " - << gt.q_imu_.y() << " " - << gt.q_imu_.z() << " " - << gt.q_imu_.w() << " " - << std::endl; - return out; -} - -std::istream& operator >>(std::istream& in, DatasetImu& gt) -{ - in >> gt.stamp_; - double x,y,z,w; - in >> x; - in >> y; - in >> z; - in >> w; - gt.q_imu_ = Quaterniond(w, x, y, z); - return in; -} - -} // end namespace FileType - - -#endif /* DATASET_IMU_H_ */ diff --git a/svo_ros/src/remote_node.cpp b/svo_ros/src/remote_node.cpp deleted file mode 100644 index 8952b21..0000000 --- a/svo_ros/src/remote_node.cpp +++ /dev/null @@ -1,169 +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 - -class RemoteHandler -{ -public: - ros::NodeHandle& nh_; - vk::UserInputThread input_thread_; - - ros::Publisher pub_key_; - ros::Publisher pub_frames_; - tf::TransformBroadcaster br_; - - vk::RingBuffer acc_timing_; - vk::RingBuffer acc_obs_; - typedef std::map > pose_map_t; - pose_map_t poses_; - std::set published_kfs_; - std::set current_kfs_; - - RemoteHandler(ros::NodeHandle& nh); - ~RemoteHandler(); - void handleUserInput(); - void poseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr msg); - void infoCb(const svo_msgs::InfoConstPtr msg); -}; - -RemoteHandler::RemoteHandler(ros::NodeHandle& nh) : - nh_(nh), - pub_key_(nh_.advertise ("/svo/remote_key", 10)), - pub_frames_(nh_.advertise("/svo/vis/frame", 100)), - acc_timing_(50), - acc_obs_(50) -{} - -RemoteHandler::~RemoteHandler() -{} - -void RemoteHandler::handleUserInput() -{ - char key = input_thread_.getInput(); - std_msgs::StringPtr msg(new std_msgs::String); - switch(key) - { - case 'q': msg->data = 'q'; pub_key_.publish(msg); break; // quit - case 'r': msg->data = 'r'; pub_key_.publish(msg); break; // reset - case 'p': msg->data = 'p'; pub_key_.publish(msg); break; // pause - case 's': msg->data = 's'; pub_key_.publish(msg); break; // start - default: ; - } -} - -void RemoteHandler::poseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr msg) -{ - Eigen::Vector3d p(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); - Eigen::Quaterniond q(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); - Eigen::Matrix3d R_world_from_frame(q.inverse()); - Eigen::Vector3d p_frame_in_world(-R_world_from_frame*p); - - poses_.insert(std::pair >( - msg->header.seq, - std::pair(p_frame_in_world, R_world_from_frame))); - - // publish trajectory - vk::output_helper::publishPointMarker(pub_frames_, p_frame_in_world, "trajectory_points", - msg->header.stamp, msg->header.seq, 0, 0.006, Eigen::Vector3d(0.,0.,0.5)); - - // publish camera pose tf - tf::StampedTransform transform_msg; - transform_msg.frame_id_ = "cam"; - transform_msg.child_frame_id_ = "world"; - transform_msg.stamp_ = msg->header.stamp; - transform_msg.setOrigin(tf::Vector3(p.x(), p.y(), p.z())); - transform_msg.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w())); - br_.sendTransform(transform_msg); - - // publish keyframes - std::set deleted_kfs; - std::set_difference(published_kfs_.begin(), published_kfs_.end(), current_kfs_.begin(), current_kfs_.end(), std::inserter(deleted_kfs, deleted_kfs.end())); - for(std::set::iterator it=deleted_kfs.begin(); it!=deleted_kfs.end(); ++it) - { - pose_map_t::iterator kf = poses_.find(*it); - if(kf != poses_.end()) - vk::output_helper::publishFrameMarker(pub_frames_, kf->second.second, kf->second.first, "kfs", ros::Time::now(), (*it)*10, 2, 0.015); - } - - std::set unpublished_kfs; - std::set_difference(current_kfs_.begin(), current_kfs_.end(), published_kfs_.begin(), published_kfs_.end(), std::inserter(unpublished_kfs, unpublished_kfs.end())); - for(std::set::iterator it=unpublished_kfs.begin(); it!=unpublished_kfs.end(); ++it) - { - pose_map_t::iterator kf = poses_.find(*it); - if(kf != poses_.end()) - vk::output_helper::publishFrameMarker(pub_frames_, kf->second.second, kf->second.first, "kfs", ros::Time::now(), (*it)*10, 0, 0.015); - } - published_kfs_.swap(current_kfs_); - - - // publish hexacopter - vk::output_helper::publishHexacopterMarker(pub_frames_, "cam", "hexacopter", msg->header.stamp, 1, 0, 0.3, Eigen::Vector3d(0.,0.,1.)); -} - -void RemoteHandler::infoCb(const svo_msgs::InfoConstPtr msg) -{ - std::string kfmsg(""); - current_kfs_.clear(); - for(std::vector::const_iterator it=msg->keyframes.begin(); it!=msg->keyframes.end(); ++it) { - current_kfs_.insert(static_cast(*it)); - if(*it == static_cast(msg->header.seq)) { - kfmsg = "Keyframe!"; - break; - } - } - acc_timing_.push_back(msg->processing_time); - acc_obs_.push_back(msg->num_matches); - - printf("[%i] t=%.2fhz (mean=%.2f) \t n_obs=%i (mean=%i) \t %s\n", - msg->header.seq, 1./msg->processing_time, 1./acc_timing_.getMean(), - msg->num_matches, acc_obs_.getMean(), kfmsg.c_str()); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "svo_remote"); - ros::NodeHandle nh; - RemoteHandler node(nh); - - printf("Listening to User-Input...\n"); - - ros::Subscriber sub_pose = nh.subscribe("/svo/pose", 10, &RemoteHandler::poseCb, &node); - ros::Subscriber sub_info = nh.subscribe("/svo/info", 10, &RemoteHandler::infoCb, &node); - ros::Rate r(100); - while (ros::ok()) - { - node.handleUserInput(); - ros::spinOnce(); - r.sleep(); - } - return 0; -}