add imu sub

2021-04-07 19:19:35 +02:00
parent e9256c9903
commit 651dcdfb2f
37 changed files with 786 additions and 310 deletions
@@ -0,0 +1,128 @@
+// 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 <vio_svo/visualizer.h>
+#include <svo/frame_handler_mono.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/map.h>
+#include <svo/feature.h>
+#include <svo_msgs/Info.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <vikit/timer.h>
+#include <vikit/output_helper.h>
+#include <deque>
+
+namespace svo {
+
+Visualizer::
+Visualizer() :
+    pnh_("~"),
+    trace_id_(0),
+    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
+{
+  // Init ROS Marker Publishers
+  pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
+  pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
+  pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
+  pub_info_ = pnh_.advertise<svo_msgs::Info>("info", 10);
+}
+
+void Visualizer::publishMinimal(
+    const cv::Mat& img,
+    const FramePtr& frame,
+    const FrameHandlerMono& slam,
+    const double timestamp)
+{
+  ++trace_id_;
+  std_msgs::Header header_msg;
+  header_msg.frame_id = "/cam";
+  header_msg.seq = trace_id_;
+  header_msg.stamp = ros::Time(timestamp);
+
+  // publish info msg.
+  if(pub_info_.getNumSubscribers() > 0)
+  {
+    svo_msgs::Info msg_info;
+    msg_info.header = header_msg;
+    msg_info.processing_time = slam.lastProcessingTime();
+    msg_info.keyframes.reserve(slam.map().keyframes_.size());
+    for(list<FramePtr>::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it)
+      msg_info.keyframes.push_back((*it)->id_);
+    msg_info.stage = static_cast<int>(slam.stage());
+    msg_info.tracking_quality = static_cast<int>(slam.trackingQuality());
+    if(frame != NULL)
+      msg_info.num_matches = slam.lastNumObservations();
+    else
+      msg_info.num_matches = 0;
+    pub_info_.publish(msg_info);
+  }
+  if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
+  {
+    Quaterniond q;
+    Vector3d p;
+    Matrix<double,6,6> Cov;
+    // publish cam in world frame (Estimated odometry in the worls frame)
+    SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
+    q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+    p = T_world_from_cam.translation();
+    Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
+    geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
+    msg_pose->header = header_msg;
+    msg_pose->pose.pose.position.x = p[0];
+    msg_pose->pose.pose.position.y = p[1];
+    msg_pose->pose.pose.position.z = p[2];
+    msg_pose->pose.pose.orientation.x = q.x();
+    msg_pose->pose.pose.orientation.y = q.y();
+    msg_pose->pose.pose.orientation.z = q.z();
+    msg_pose->pose.pose.orientation.w = q.w();
+    for(size_t i=0; i<36; ++i)
+      msg_pose->pose.covariance[i] = Cov(i%6, i/6);
+    pub_pose_.publish(msg_pose);
+  }
+}
+
+void Visualizer::visualizeMarkers(
+    const FramePtr& frame,
+    const set<FramePtr>& core_kfs,
+    const Map& map)
+{
+  if(frame == NULL)
+    return;
+
+  vk::output_helper::publishTfTransform(
+      frame->T_f_w_*T_world_from_vision_.inverse(),
+      ros::Time(frame->timestamp_), "cam_pos", "world", br_);
+
+  if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
+  {
+    vk::output_helper::publishPointMarker(
+        pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
+        ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
+    removeDeletedPts(map);
+  }
+}
+
+void Visualizer::removeDeletedPts(const Map& map)
+{
+  if(pub_points_.getNumSubscribers() > 0)
+  {
+    for(list<Point*>::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it)
+      vk::output_helper::publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d());
+  }
+}
+
+} // end namespace svo
@@ -0,0 +1,142 @@
+// 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 <ros/package.h>
+#include <string>
+#include <svo/frame_handler_mono.h>
+#include <svo/map.h>
+#include <vio_svo/visualizer.h>
+#include <vikit/params_helper.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/String.h>
+#include <message_filters/synchronizer.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <Eigen/Core>
+#include <vikit/abstract_camera.h>
+#include <vikit/camera_loader.h>
+#include <vikit/user_input_thread.h>
+
+
+namespace svo {
+
+/// SVO Interface
+class VoNode
+{
+public:
+      svo::FrameHandlerMono* vo_;
+      svo::Visualizer visualizer_;
+      bool publish_markers_;                 //!< publish only the minimal amount of info (choice for embedded devices)
+      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<bool>("vio/publish_markers", true)),
+      cam_(NULL),
+      quit_(false)
+{
+      // Create Camera
+      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<double>("vio/init_rx", 0.0),
+                               vk::getParam<double>("vio/init_ry", 0.0),
+                               vk::getParam<double>("vio/init_rz", 0.0))),
+          Eigen::Vector3d(vk::getParam<double>("vio/init_tx", 0.0),
+                          vk::getParam<double>("vio/init_ty", 0.0),
+                          vk::getParam<double>("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_;
+}
+
+void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
+{
+      try {
+          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<double>(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<std::string>("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, "vio");
+  svo::VoNode vo_node;
+  ros::NodeHandle nh;
+  ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("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!
+  }
+  printf("SVO terminated.\n");
+  return 0;
+}