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

removed remote_node

Browse Source
This commit is contained in:
Christian Forster
2014-03-12 22:28:31 +01:00
parent 6277bbfdc4
commit c2a52c68ad
3 changed files with 0 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo_ros/CMakeLists.txt
-2
View File
@@ -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)
svo_ros/include/svo_ros/dataset_imu.h
-70
View File
@@ -1,70 +0,0 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#ifndef DATASET_IMU_H_
#define DATASET_IMU_H_
namespace FileType {
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
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_ */
svo_ros/src/remote_node.cpp
-169
View File
@@ -1,169 +0,0 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (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 <http://www.gnu.org/licenses/>.
#include <map>
#include <set>
#include <ros/ros.h>
#include <vikit/user_input_thread.h>
#include <vikit/ringbuffer.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <svo_msgs/Info.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <vikit/output_helper.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
class RemoteHandler
{
public:
ros::NodeHandle& nh_;
vk::UserInputThread input_thread_;
ros::Publisher pub_key_;
ros::Publisher pub_frames_;
tf::TransformBroadcaster br_;
vk::RingBuffer<double> acc_timing_;
vk::RingBuffer<int> acc_obs_;
typedef std::map<size_t, std::pair<Eigen::Vector3d, Eigen::Matrix3d> > pose_map_t;
pose_map_t poses_;
std::set<int> published_kfs_;
std::set<int> 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<std_msgs::String> ("/svo/remote_key", 10)),
pub_frames_(nh_.advertise<visualization_msgs::Marker>("/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<size_t, std::pair<Eigen::Vector3d, Eigen::Matrix3d> >(
msg->header.seq,
std::pair<Eigen::Vector3d, Eigen::Matrix3d>(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<int> 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<int>::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<int> 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<int>::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<int32_t>::const_iterator it=msg->keyframes.begin(); it!=msg->keyframes.end(); ++it) {
current_kfs_.insert(static_cast<int>(*it));
if(*it == static_cast<int32_t>(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;
}