一、程序更改的思路（参考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 "@[email protected]") ####################################################### # linedesc build dir set( linedesc_DIR "@[email protected]") ####################################################### set( linedesc_INCLUDE_DIR "@[email protected]" ) set( linedesc_INCLUDE_DIRS "@[email protected]" ) set( linedesc_LIBRARIES "@[email protected]" ) set( linedesc_LIBRARY "@[email protected]" ) set( linedesc_LIBRARY_DIR "@[email protected]" ) set( linedesc_LIBRARY_DIRS "@[email protected]" )
最后重新编译一下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对比一下；然后读读程序，看参数还能不能改。
最后，目前的运行效果图：
原文地址：https://www.cnblogs.com/scripthome/p/8552114.html