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); ... }
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")); } ... ...
只要在上一步的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)