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()* << "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",, &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()>){
　　const int scale=(>>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_ == )
　　{
　　　　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[]-,
　　　　　　(*it)->px[]-),
　　　　　　cv::Point2f((*it)->px[]+,
　　　　　　(*it)->px[]+),
　　　　　　cv::Scalar(,,),
　　　　　　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,,> 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[];
　　msg_pose->pose.pose.position.y=p[];
　　msg_pose->pose.pose.position.z=p[];

　　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=;i<;++i)
　　　　msg_pose->pose.covariance[i]=Cov(i%,i/);
　　pub_pose_.publish(msg_pose);
　　br_.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(q.x(),q.y(),q.z(),q.w()),
　　　　tf::Vector3(p[],p[],p[])),
　　　　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对比一下；然后读读程序，看参数还能不能改。

最后，目前的运行效果图：