first commit

svo_ros/include/svo_ros/dataset_img.h

@@ -0,0 +1,62 @@
+// 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_IMG_H
+#define DATASET_IMG_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 DatasetImg
+{
+public:
+  DatasetImg() {}
+  virtual ~DatasetImg() {}
+
+  double stamp_;
+  std::string imgname_;
+
+  friend std::ostream& operator <<(std::ostream& out, const DatasetImg& pair);
+  friend std::istream& operator >>(std::istream& in, DatasetImg& pair);
+};
+
+std::ostream& operator <<(std::ostream& out, const DatasetImg& gt)
+{
+  out
+    << gt.stamp_ << " "
+    << gt.imgname_ << " "
+    << std::endl;
+  return out;
+}
+
+std::istream& operator >>(std::istream& in, DatasetImg& gt)
+{
+  in >> gt.stamp_;
+  in >> gt.imgname_;
+  return in;
+}
+
+} // end namespace FileType
+
+
+#endif /* DATASET_IMG_H */

svo_ros/include/svo_ros/dataset_imu.h

@@ -0,0 +1,70 @@
+// 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/include/svo_ros/visualizer.h

@@ -0,0 +1,75 @@
+// 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 SVO_VISUALIZER_H_
+#define SVO_VISUALIZER_H_
+
+#include <queue>
+#include <ros/ros.h>
+#include <svo/global.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <nav_msgs/Odometry.h>
+#include <std_msgs/ColorRGBA.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <boost/lexical_cast.hpp>
+#include <tf/transform_broadcaster.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <iostream>
+
+namespace svo {
+
+class Frame;
+class Point;
+class Map;
+class FrameHandlerMono;
+
+typedef boost::shared_ptr<Frame> FramePtr;
+
+/// This class bundles all functions to publish visualisation messages,
+/// trace data and output debugging information.
+class Visualizer
+{
+public:
+  ros::NodeHandle pnh_;
+  size_t trace_id_;
+  size_t img_pub_level_;
+  size_t img_pub_nth_;
+  size_t dense_pub_nth_;
+  ros::Publisher pub_frames_;
+  ros::Publisher pub_points_;
+  ros::Publisher pub_pose_;
+  ros::Publisher pub_info_;
+  ros::Publisher pub_dense_;
+  image_transport::Publisher pub_images_;
+  tf::TransformBroadcaster br_;
+
+  Visualizer();
+  ~Visualizer() {};
+  void visualizeMarkers(const FramePtr& frame, const set<FramePtr>& core_kfs, const Map& map);
+  void publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono& slam, const double timestamp);
+  void publishMapRegion(set<FramePtr> frames);
+  void removeDeletedPts(const Map& map);
+  void displayKeyframeWithMps(const FramePtr& frame, int ts);
+  void exportToDense(const FramePtr& frame);
+};
+
+} // end namespace svo
+
+#endif /* SVO_VISUALIZER_H_ */