标签：SVO 13 2021.2 svo VoNode vo _- input include

SVO-13-vo_node–2021.2.22

vo_node.cpp

#include <ros/package.h>
#include <string>
#include <svo/frame_handler_mono.h>
#include <svo/map.h>
#include <svo/config.h>
#include <svo_ros/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/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.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{
  
class VoNode{
public:
  svo::FrameHandlerMono* vo_;
  svo::Visualizer visualizer_;
  bool publish_markers_;
  
  bool publish_dense_input_;
  
  //首先开辟了一个线程,用于监听控制台输入
  boost::shared_ptr<vk::UserInputThread> user_input_thread_;
  ros::Subscriber sub_remote_key_;
  std::string remote_input_;
  vk::AbstractCamera* cam_;//加载摄像机参数
  bool quit_;//是否退出
  VoNode();
  ~VoNode();
  
  void imgCb(const sensor_msgs::ImageConstPtr& msg);
  void processUserActions();
  void remoteKeyCb(const std_msgs::StringConstPtr& key_input);
};

svo::VoNode::VoNode():
  vo_(NULL),
  publish_markers_(vk::getParam<bool>("svo/publish_markers_",true)),
  publish_dense_input_(vk::getParam<bool>("svo/publish_dense_input_",false)),
  remote_input_(""),
  cam_(NULL),
  quit_(false){
    if(vk::getParam<bool>("svo/accept_console_user_input",true))
      user_input_thread_ = boost::make_shared<vk::UserInputThread>();
    
    if(!vk::camera_loader::loadFromRosNs("svo",cam_))
      throw std::runtime_error("camera model not correctlt specified");
    
    //初始化位姿,Sophus::SE3()用来构建一个欧式群
    visualizer_.T_world_from_vision_ = Sophus::SE3(
      
      //将欧拉角转化为旋转矩阵
      vk::rpy2dcm(Vector3d(vk::getParam<double>("svo/init_rx", 0.0),
                           vk::getParam<double>("svo/init_ry", 0.0),
                           vk::getParam<double>("svo/init_rz", 0.0))),
      Eigen::Vector3d(vk::getParam<double>("svo/init_tx", 0.0),
                      vk::getParam<double>("svo/init_ty", 0.0),
                      vk::getParam<double>("svo/init_tz", 0.0)));
    
    vo_ = new svo::FrameHandlerMono(cam_);//初始化视觉前端VO
    //对应视觉前端VO开始运行系统
    vo_ -> start();//开始
}

VoNode::~VoNode()
{
  delete vo_;
  delete cam_;
  if(user_input_thread_ != NULL)
    user_input_thread_->stop();
}

void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
{
  cv::Mat img;
  try {
    //完成图片的读取
    //将ROS数据转换为opencv中的图像数据
    img = cv_bridge::toCvShare(msg, "mono8")->image;
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
  }
  processUserActions();//执行此函数,开辟控制台输入线程,根据输入字母进行操作
  
  //调用视觉前端类传入图像
  vo_->addImage(img, msg->header.stamp.toSec());//参数2可以获得系统时间
  
  //进行ROS消息有关的设置
  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(publish_dense_input_)
    visualizer_.exportToDense(vo_->lastFrame());//稠密显示特征点

  if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
    usleep(100000);
}

void VoNode::processUserActions()
{
  char input = remote_input_.c_str()[0];
  remote_input_ = "";

  if(user_input_thread_ != NULL)
  {
    char console_input = user_input_thread_->getInput();
    if(console_input != 0)
      input = console_input;
  }

  switch(input)
  {
    case 'q':
      quit_ = true;
      printf("SVO user input: QUIT\n");
      break;
    case 'r':
      vo_->reset();
      printf("SVO user input: RESET\n");
      break;
    case 's':
      vo_->start();
      printf("SVO user input: START\n");
      break;
    default: ;
  }
}

void VoNode::remoteKeyCb(const std_msgs::StringConstPtr& key_input)
{
  remote_input_ = key_input->data;
}

} // namespace svo

int main(int argc, char **argv)
{
  ros::init(argc, argv, "svo");//初始化ROS
  ros::NodeHandle nh;//创建句柄
  std::cout << "create vo_node" << std::endl;
  svo::VoNode vo_node;//创建节点vo_node

  // subscribe to cam msgs
  //订阅相机消息
  //先定义topic名称
  std::string cam_topic(vk::getParam<std::string>("svo/cam_topic", "camera/image_raw"));
  image_transport::ImageTransport it(nh);//创建图片的发布/订阅器,名为it
  
  //调用Image_transport::subscriber函数
  //对于节点vo_node,一旦有图像发布到主题cam_topic上,执行回调函数imgCb
  image_transport::Subscriber it_sub = it.subscribe(cam_topic, 5, &svo::VoNode::imgCb, &vo_node);

  // subscribe to remote input
  //订阅远程输入消息
  vo_node.sub_remote_key_ = nh.subscribe("svo/remote_key", 5, &svo::VoNode::remoteKeyCb, &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;
}

标签：SVO,13,2021.2,svo,VoNode,vo,_-,input,include
来源： https://blog.csdn.net/qq_43303411/article/details/113950993

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。