一种通过模仿SIFT特征实现基于二维地图的初定位方法

一种通过模仿SIFT特征实现基于二维地图的初定位方法

当年进入SLAM领域的时候最开始接触的是二维激光导航机器人，它在建立了二维栅格地图之后通过amcl进行定位，但是amcl是一个动态的过程，需要机器人不停地运动以求收敛，因此如何在机器人开机后进行直接定位成为了一个有趣的问题，最简单的方法是用rviz发一个initial pose，但这也不是我们想要的。另外，机器人运行途中，由于传感器等多方面原因定位漂移，自身是可以知晓这一点，但是如何恢复也是一个难点。原来面试的时候也遇到过类似的问题，看来是一个普适性的问题啊。

在之前的实践中，可以采取暴力遍历的匹配方法，查看匹配评分较低的位置，但是花费时间过长，如果能将位置与量化的特征相结合，将大大降低耗时与自动定位那段时间的CPU。

因此，在犯下无数日夜的拖延症之后，我们今天来通过非暴力的方法解决，主要的思路是模仿SIFT特征点与描述子的思想，针对点云轮廓的变化提取特征，同时计算主成分方向，以追求旋转不变性。通过这种方法，我们只需将地图遍历计算后，存入文件，等待自定位时直接读文件再计算。

通过测试我们可以发现，评分较高的自定位位置在真实位置富集在附近。

测试代码的头文件如下：

#ifndef AUTO_LOCATION_H
#define AUTO_LOCATION_H#include "ros/ros.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/PoseStamped.h"
#include "sensor_msgs/PointCloud.h"#include "opencv2/opencv.hpp"#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>   
#include <pcl/sample_consensus/model_types.h>   
#include <pcl/segmentation/sac_segmentation.h>   
#include <pcl/filters/extract_indices.h>
#include <pcl/common/transforms.h>#include <queue>typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> PointCloud;
typedef pcl::PointCloud<PointType>::Ptr PointPtr;struct LineInfo
{float A, B, C = 1.0;//直线的方程参数float vec_x, vec_y;//检测点到该直线的垂足的方向向量float distance;//检测点到该直线的距离float angle = 0.0;//本方向向量与主成分方向的角度（确保旋转不变性）//优先级队列求距离最远的直线作为主成分bool operator< (const LineInfo& other) const{return (distance<other.distance);}
};struct Descriptor
{geometry_msgs::Pose pose;std::vector<LineInfo> line_info_vec_;//检查本描述子是否与另一个描述子相似//应该可用梯度下降法求解，这里先粗暴处理了bool similar(const Descriptor& other){int err = 0;for(auto& info:line_info_vec_){bool found = false;for(auto& other_info:other.line_info_vec_){bool c1 = (fabs(info.angle-other_info.angle)<2.0);bool c2 = (fabs(info.distance-other_info.distance)<0.1);if(c1 && c2){found = true;}}if(!found){err += 1;}}if(err<2)return true;elsereturn false;}
};namespace auto_location
{
class AutoLocation
{
public:AutoLocation(ros::NodeHandle&);
private:void getMap();void restoreDescriptor();std::vector<Descriptor> map_descriptor_vec_;private:ros::Publisher potential_pos_pub_;geometry_msgs::PoseArray potential_pos_;ros::Publisher actual_pose_pub_;geometry_msgs::PoseStamped actual_pose_;ros::Publisher fake_laser_pub_;sensor_msgs::PointCloud fake_laser_cloud_;PointPtr laser_cloud_ptr_, line_cloud_ptr_;ros::Publisher map_pub_;sensor_msgs::PointCloud map_cloud_;ros::Subscriber initial_pose_sub_;void initialPoseCb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&);private:void getFakeLaser(geometry_msgs::Pose);Descriptor parseLaserCloud();void computeDescriptor();LineInfo computeLine(pcl::PointIndices::Ptr&);float computeAngle(LineInfo&, LineInfo&);void getProperOrientation(Descriptor&, const Descriptor&, geometry_msgs::Pose&);
private:cv::Mat map_img_;std::string map_path_ = "/home/ruoyu/location_ws/src/auto_location/maps/map.png";float scale_ = 0.02;
};
}/* end of namespace */
#endif

 源文件如下：

#include "auto_location.h"using namespace std;namespace auto_location
{
AutoLocation::AutoLocation(ros::NodeHandle& nh)
{map_pub_ = nh.advertise<sensor_msgs::PointCloud>("/map_cloud", 1, true);fake_laser_pub_ = nh.advertise<sensor_msgs::PointCloud>("/fake_laser", 1, true);potential_pos_pub_ = nh.advertise<geometry_msgs::PoseArray>("/potential_pos", 1, true);actual_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/actual_pose", 1, true);initial_pose_sub_ = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, &AutoLocation::initialPoseCb, this);laser_cloud_ptr_.reset(new PointCloud);line_cloud_ptr_.reset(new PointCloud);getMap();restoreDescriptor();cout<<"auto location initialized"<<endl;
}void AutoLocation::getMap()
{map_img_ = cv::imread(map_path_);cv::cvtColor(map_img_, map_img_, CV_BGR2GRAY);cout<<map_img_.size()<<endl;geometry_msgs::Point32 p;for(int u = 0; u < map_img_.cols; ++u){for(int v = 0; v < map_img_.rows; ++v){int val = map_img_.at<unsigned char>(v,u);if(val==0){p.x = u*scale_;p.y = v*scale_;p.z = 0.0;map_cloud_.points.push_back(p);}}}map_cloud_.header.frame_id = "map";fake_laser_cloud_.header.frame_id = "map";actual_pose_.header.frame_id = "map";potential_pos_.header.frame_id = "map";map_pub_.publish(map_cloud_);
}void AutoLocation::restoreDescriptor()
{//在整个地图上遍历采样，存储所有的描述子float sample_step = 0.1;for(float x = 0.6; x < 9.4; x+=sample_step){for(float y = 0.6; y < 6.4; y+=sample_step){geometry_msgs::Pose pose;pose.position.x = x;pose.position.y = ientation.w = 1;//在地图上遍历时，代入计算的位姿，四元数为初始方向getFakeLaser(pose);Descriptor d = parseLaserCloud();d.pose = pose;map_descriptor_vec_.push_back(d);}}
}void AutoLocation::initialPoseCb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{actual_pose_.pose = msg->pose.pose;actual_pose_pub_.publish(actual_pose_);getFakeLaser(msg->pose.pose);Descriptor curr_descriptor = parseLaserCloud();potential_pos_.poses.clear();if(!curr_descriptor.line_info_vec_.empty()){for(const auto& d:map_descriptor_vec_){if(curr_descriptor.similar(d)){geometry_msgs::Pose pose = d.pose;//根据主成分向量的方向角，getProperOrientation(curr_descriptor, d, pose);potential_pos_.poses.push_back(pose);}}}cout<<"potential_pos_.poses:"<<potential_pos_.poses.size()<<endl;potential_pos_pub_.publish(potential_pos_);
}void AutoLocation::getProperOrientation(Descriptor& curr, const Descriptor& pot, geometry_msgs::Pose& pose)
{//通过潜在位置的主成分向量角度，与当前点云的主成分向量角度，求解当前位姿的四元数auto& curr_principal = curr.line_info_vec_[0];auto& pot_principal = pot.line_info_vec_[0];float angle;angle = computeAngle(curr_principal, const_cast<LineInfo&>(pot_principal))/180.0*M_ientation.z = sin(angle*0.5);ientation.w = cos(angle*0.5);
}Descriptor AutoLocation::parseLaserCloud()
{Descriptor descriptor;int line_thresh = 5;//依次提取点云中的直线特征，并求出直线点云的参数std::priority_queue<LineInfo> line_distance_queue;cout<<"linear points size:";while(laser_cloud_ptr_->size()>line_thresh){pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);pcl::PointIndices::Ptr inliers(new pcl::PointIndices);pcl::SACSegmentation<pcl::PointXYZ> seg;seg.setOptimizeCoefficients(true);seg.setModelType(pcl::SACMODEL_LINE);seg.setMethodType(pcl::SAC_RANSAC);seg.setDistanceThreshold(0.1);seg.setInputCloud(laser_cloud_ptr_);seg.segment(*inliers, *coefficients);cout<<inliers->indices.size()<<" ";if(inliers->indices.size()<line_thresh)break;//计算本条直线的方程Ax+By+C=0LineInfo this_info = computeLine(inliers);line_distance_queue.push(this_info);pcl::ExtractIndices<PointType> extract;extract.setInputCloud(laser_cloud_ptr_);extract.setIndices(inliers);extract.setNegative(true);extract.filter(*laser_cloud_ptr_);}cout<<endl;//auto principal_component = line_p();line_distance_queue.pop();descriptor.line_info_vec_.push_back(principal_component);//以主成分方向为0角度while(!line_pty()){auto this_line = line_p();line_distance_queue.pop();computeAngle(principal_component, this_line);descriptor.line_info_vec_.push_back(this_line);}for(auto info:descriptor.line_info_vec_){cout<<info.distance<<","<<info.angle<<endl;}return descriptor;
}float AutoLocation::computeAngle(LineInfo& ori, LineInfo& curr)
{//以主成分方向的方向为参照，求其它向量与主成分向量的夹角float muliple = (ori.vec_x*curr.vec_x+ori.vec_y*curr.vec_y);float disMultiple = sqrt(ori.vec_x*ori.vec_x+ori.vec_y*ori.vec_y)*sqrt(curr.vec_y*curr.vec_y+curr.vec_x*curr.vec_x);curr.angle = 180.0/M_PI*acos(muliple/disMultiple);//通过叉乘计算顺逆时针旋转角度（正负值）float cross = ori.vec_x*curr.vec_y-ori.vec_y*curr.vec_x;if(cross < 0){curr.angle = 360.0-curr.angle;}return curr.angle;
}LineInfo AutoLocation::computeLine(pcl::PointIndices::Ptr& indice)
{LineInfo info;auto sqDis = [&](int i, int j) -> float{auto& p1 = laser_cloud_ptr_->points[indice->indices[i]];auto& p2 = laser_cloud_ptr_->points[indice->indices[j]];return ((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));};int ind1, ind2;if(indice->indices.size()==2){ind1 = 0;ind2 = 1;}else{//找到一条拟合线段最远的两端点float max_dis; ind1 = 0;ind2 = 1;max_dis = sqDis(ind1, ind2);for(size_t i = 2; i < indice->indices.size(); ++i){float dis1 = sqDis(i, ind1);float dis2 = sqDis(i, ind2);if(dis1>dis2 && dis1>max_dis){ind2 = i;}else if(dis1<dis2 && dis2>max_dis){ind1 = i;}}}auto& p1 = laser_cloud_ptr_->points[indice->indices[ind1]];auto& p2 = laser_cloud_ptr_->points[indice->indices[ind2]];float x1 = p1.x;float x2 = p2.x;float y1 = p1.y;float y2 = p2.y;info.B = (1.0-x1/x2)/(x1*y2/x2-y1);info.A = -(1.0+info.B*y1)/x1;info.distance = 1.0/sqrt(info.A*info.A+info.B*info.B);info.vec_x = -info.A/(info.A*info.A+info.B*info.B);info.vec_y = -info.B/(info.A*info.A+info.B*info.B);return info;
}void AutoLocation::getFakeLaser(geometry_msgs::Pose pose)
{float center_x = pose.position.x;float center_y = pose.position.y;int pixel_x = center_x/scale_;int pixel_y = center_y/scale_;float dx, dy;fake_laser_cloud_.points.clear();laser_cloud_ptr_->clear();PointType p;//回溯法求虚假点云for(int i = 0; i < 720; ++i){float theta = i*0.5;if(theta == 90.0){dx = 0.0;dy = 1.0;}else if(theta == 270.0){dx = 0.0;dy = -1.0;}else{dx = cos(M_PI*theta/180.0);dy = dx*tan(M_PI*theta/180.0);}float x = pixel_x;float y = pixel_y;bool no_conflict = true;while(no_conflict){x += dx;y += dy;if(x<0 || y<0 || x>=map_img_.cols || y>=map_img_.rows)no_conflict = false;if((int)map_img_.at<unsigned char>((int)y,(int)x)==0){no_conflict = false;p.x = x*scale_;p.y = y*scale_;p.z = 0.0;laser_cloud_ptr_->points.push_back(p);}}}//将其转换至初始位姿（原点）auto r_q = ientation;auto r_p = pose.position;Eigen::Quaternionf q(r_q.w, r_q.x, r_q.y, r_q.z);Eigen::Matrix4f pose_m = Eigen::Matrix4f::Identity();pose_m.block<3,3>(0,0) = q.matrix();pose_m(0,3) = r_p.x;pose_m(1,3) = r_p.y;pose_m(2,3) = r_p.z;Eigen::Matrix4f inv_m = pose_m.inverse();PointCloud cloud;pcl::transformPointCloud(*laser_cloud_ptr_, cloud, inv_m);*laser_cloud_ptr_ = cloud;geometry_msgs::Point32 g_p;for(size_t i = 0; i < cloud.points.size(); ++i){g_p.x = cloud.points[i].x;g_p.y = cloud.points[i].y;g_p.z = cloud.points[i].z;fake_laser_cloud_.points.push_back(g_p);}fake_laser_pub_.publish(fake_laser_cloud_);
}}/* end of namespace */int main(int argc, char** argv)
{ros::init(argc, argv, "auto_location");ros::NodeHandle nh("~");auto_location::AutoLocation location(nh);ros::spin();return 0;
}

在这里我将每个位置采样的点云，分解成直线的组合，同时将最远的直线的法向量作为主成分方向。那么，原点到其它直线的法向量就根据主成分方向作相应转换。对主成分的提取规则可能有更好的选择，例如最长的直线，点最多的直线等。
另外，描述子的匹配规则仍然需要改善。我的测试一直是在完全结构化的环境下进行，get到的雷达点云也十分理想，在真实传感器数据下必然会打折扣，因此权且抛砖引玉，等待下一波再优化~