pl-svo在ROS下运行笔记

Posted MANONG_CODE

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了pl-svo在ROS下运行笔记相关的知识,希望对你有一定的参考价值。

一、程序更改的思路(参考svo_ros的做法):

1.在ROS下将pl-svo链接成库需要更改相应的CMakeLists.txt文件,添加package.xml文件;

2.注册一个ROS节点使用svo那个ATAN的数据集测试pl-svo;

3.显示部分也是参考svo_ros(visualizer.cpp)并进行相应简化(不必链接成库);

4.程序运行时参数要改(亲测svo的两个参数文件(vo_accurate.yaml,vo_fast.yaml)并不适用于pl-svo,不知道如何选择参数,使用的默认值);

二、具体代码和遇到的问题

1.将pl-svo链接成ROS下的库(change in CMakeLists.txt)

SET(USE_ROS true)
...
...
IF(USE_ROS)
  FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules vikit_common vikit_ros)
  catkin_package(
      DEPENDS Eigen OpenCV Sophus Boost fast linedesc
      CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros
      INCLUDE_DIRS include
      LIBRARIES plsvo
  )
ELSE()
  FIND_PACKAGE(vikit_common REQUIRED)
  SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
  SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
ENDIF()
...

之后,我遇到一个问题,pl-svo找不到Line_Descript了

终端提示如下(大概是这样,记不清楚了):cannot find file named Findlinedesc.cmake or linedescConfig.cmake ...

解决:

(1)将line_descript链接成SHARED LIBRARIES,重新编译

add_library( linedesc SHARED ${all_source_files} )

(2)做法1没有用的话,可以尝试生成.cmake文件,参考fast的做法在line_descript里的CMakeLists.txt文件添加如下指令

GET_TARGET_PROPERTY( FULL_LIBRARY_NAME ${PROJECT_NAME} LOCATION )
SET(linedesc_LIBRARIES ${FULL_LIBRARY_NAME} )
SET(linedesc_LIBRARY_DIR ${PROJECT_BINARY_DIR} )
SET(linedesc_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include")
#生成linedescConfig.cmake文件的指令 CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/linedescConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/linedescConfig.cmake @ONLY IMMEDIATE ) export( PACKAGE linedesc )

并创建文件linedescConfig.cmake.in,文件内容如下:

#######################################################
# linedesc source dir
set( linedesc_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@")

#######################################################
# linedesc build dir
set( linedesc_DIR "@CMAKE_CURRENT_BINARY_DIR@")

#######################################################
set( linedesc_INCLUDE_DIR  "@linedesc_INCLUDE_DIR@" )
set( linedesc_INCLUDE_DIRS "@linedesc_INCLUDE_DIR@" )

set( linedesc_LIBRARIES    "@linedesc_LIBRARIES@" )
set( linedesc_LIBRARY      "@linedesc_LIBRARIES@" )

set( linedesc_LIBRARY_DIR  "@linedesc_LIBRARY_DIR@" )
set( linedesc_LIBRARY_DIRS "@linedesc_LIBRARY_DIR@" )

最后重新编译一下line_descript,在build文件夹中就生成了linedescConfig.cmake文件。

然后在pl-svo的CMakeLists.txt中作相应更改:

#add:
FIND_PACKAGE(linedesc REQUIRED)
...
...
INCLUDE_DIRECTORIES(
...
...
  # delete:  
  #${PROJECT_SOURCE_DIR}/3rdparty/line_descriptor/include/
  #add:
  ${linedesc_INCLUDE_DIRS}
)

添加相应的package.xml文件,在ROS的worksapce下使用catkin_make即可编译成功,生成pl-svo的共享库。

心得: packagenameConfig.cmake文件存储的是已安装的程序包的绝对安装路径。

 

 2.创建一个ROS下的程序包测试上一步生成的pl-svo的库

catkin_create_pkg plsvo_ros ...

下面写程序

先说思路:

  (1)照例先注册好ROS的node,定义nodehandle;

  (2)然后读取ATAN的yaml文件加载相机的参数使用vikit工具定义相机;

  (3)定义一个VoNode类作为pl-svo算法部分的接口,它的属性和子函数大致有plsvo::FrameHandlerMono*,vk::AbstractCamera*,void imgcb(const sensor_msgs::ImageConstPtr&);

  (4)订阅rosbag发布的图像,在回调函数中调用plsvo::FrameHandleMono::addImage()进行解算;

先上部分代码:

//plsvo interface
class VoNode
{
public:
  //Frame handle    
  FrameHandlerMono* vo_;
   //camera
   vk::AbstractCamera* cam_;
   //show in rviz
  plsvo::Visualizer visualizer_;

  VoNode();
  //initialize by camera
  VoNode(vk::AbstractCamera *cam_);
  ~VoNode();
  //function for image callback
  void imgCb(const sensor_msgs::ImageConstPtr& msg);
};

...
//The body of VoNode::imgCb()
void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
{
  //read image
  cv::Mat img;
  try {
    img = cv_bridge::toCvShare(msg, "mono8")->image;
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
  }

  //Compute
  vo_->addImage(img, msg->header.stamp.toSec());
  //Show
  visualizer_.publishMinimal(img,vo_,msg->header.stamp.toSec());
  //print in terminal when run vo
  if (vo_->lastFrame() != NULL){
    std::cout << "Frame-Id: "<< vo_->lastFrame()->id_ << " \\t"
      <<"#PointFeatures: "<<vo_->lastNumPtObservations()<<" \\t"
      <<"#LineFeatures: "<<vo_->lastNumLsObservations()<< " \\t"
      <<"Proc. Time: "<<vo_->lastProcessingTime()*1000 << "ms" << std::endl << std::endl;
  }
} 

...
int main(int argc, char **argv)
{
  //ROS initialize
  ros::init(argc, argv, "plsvo");
  ros::NodeHandle nh;

  YAML::Node dset_config = YAML::LoadFile("The path to parameter.");     
  ...
  //create VoNode
  plsvo::VoNode vo_node(cam_atan_und);
  //subscribe the topic publish image.
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber it_sub = it.subscribe("camera/image_raw",10, &plsvo::VoNode::imgCb, &vo_node);
  ...

}
  
plsvo_node.cpp

 

3.显示部分(visualization.h,visualization.cpp)

功能:

  把pl-svo每一帧跟踪的特征点显示在图片上;

  在rviz里用tf表示pl-svo解算的位姿;

  Path的话还没加进去(应该可以),下一步可以加进去跟GroundTruth对比一下;

相应代码:

...
...
// Publish features in image.
if(pub_images_.getNumSubscribers()>0){
  const int scale=(1>>img_pub_level_);
  cv::Mat img_rgb(vo->lastFrame()->img_pyr_[img_pub_level_].size(), CV_8UC3);
  cv::cvtColor(vo->lastFrame()->img_pyr_[img_pub_level_], img_rgb, CV_GRAY2RGB);
  if(img_pub_level_ == 0)
  {
    std::vector<cv::Point2f> points2d;
    for(std::list<PointFeat*>::iterator it=vo->lastFrame()->pt_fts_.begin();it!=vo->lastFrame()->pt_fts_.end();++it){
    //if((*it)->type == Feature::EDGELET)
    cv::rectangle(img_rgb,
      cv::Point2f((*it)->px[0]-2,
      (*it)->px[1]-2),
      cv::Point2f((*it)->px[0]+2,
      (*it)->px[1]+2),
      cv::Scalar(0,255,0),
      CV_FILLED);
    }
  }
  cv_bridge::CvImage img_msg;
  img_msg.header=header_msg;
  img_msg.image=img_rgb;
  img_msg.encoding=sensor_msgs::image_encodings::BGR8;
  pub_images_.publish(img_msg.toImageMsg());
}

...
//Publish tansform tf and rotation.
if(vo->stage() ==FrameHandlerBase::STAGE_DEFAULT_FRAME)
{
  Quaterniond q;
  Vector3d p;
  Matrix<double,6,6> Cov;
  // publish world in cam frame
  SE3 T_cam_from_world(vo->lastFrame()->T_f_w_* T_world_from_vision_.inverse());
  q = Quaterniond(T_cam_from_world.rotation_matrix());
  p = T_cam_from_world.translation();
  Cov = vo->lastFrame()->Cov_;
  
  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);
  br_.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(q.x(),q.y(),q.z(),q.w()),
    tf::Vector3(p[0],p[1],p[2])),
    ros::Time(timestamp), 
    "world", 
    "cam"));
}
...
...
visualization.cpp

只要在上一步的imgCb()中调用发布带有特征的图像,tf,位姿的函数,就可以在rviz中显示pl-svo跟踪和解算的结果。

没遇到过的bug:

这是因为形参对象plsvo::FrameHandlerMono是const的,而它的属性是非const的,当使用\'vo->lastFrame()\'时,gcc就会报错

两种解决方法: (1)形参改为非const的; (2)添加形参;

 

4.程序运行时参数

最后一个问题,也是还没有解决的问题,由于还没有认真阅读pl-svo的代码,github上也只提供了相机的参数,所以很多参数没有自己设置,使用了

config.cpp中的默认值。

下一步,先想办法在rviz里画下path跟GroundTruth对比一下;然后读读程序,看参数还能不能改。

最后,目前的运行效果图:

 

以上是关于pl-svo在ROS下运行笔记的主要内容,如果未能解决你的问题,请参考以下文章

ROS学习笔记-log日志以及输出工具(rqt_console)

ROS学习笔记之——DVXplorer测试(降低噪声以及不同的敏感度下的测试效果)

ROS学习笔记之——ESVO复现及DAVIS346测试

ROS实验笔记之——无人机在VICION下试飞

ROS实验笔记之——无人机在VICION下试飞

ROS学习笔记-导入并启动导航用的仿真地图