从程序中学习UKF-SLAM（一）

然间从GitHub上发现一个小项目，做的是一个基于UKF的状态估计，使用C++版本的mathplot做显示，编译之后却发现显示一直有问题，于是萌生了一个想法：能不能把它移植到ROS中？然后就有了接下来的学习过程，以此记录。如有问题，欢迎提出。

这是需要复写的代码：在GitHub上（project的核心代码是来自源码的，如有侵权，请联系我更改，谢谢。）
源码最终实现的效果如下：

同样也可参考UKF最初代码：GitHub_Self-Driving Car
同样也有相关博客对该代码进行了解析，仅供参考：UKF算法解析

复写完成的代码请看最后一篇博客。
在ROS中实现的效果如下：百度网盘视频或者YouTube视频

一、前提说明

首先解释一下，路标数据，里程计数据，激光数据各自的组成部分，以及project涉及的运动模型。

1.landmark路标

在程序中（在ukf_console.h的51行），用一个vector表示：

std::vector<Eigen::Vector3f> Landmarks;

各表示点的id号，点的x坐标以及y坐标。此处说明路标具有id标识的，也就决定了后续过程中数据关联的方式。
在param/world_data.txt中，写明了各个landmark的详细信息：

在程序中直接用C++的文件读取方法，将landmark信息读入vector数组中：

bool ukf_console::getLandmarks(const std::string& path)
{std::ifstream in_file(path, std::ifstream::in);if (!in_file.is_open()) {std::cerr << "Cannot open input file: " << (path) << std::endl;exit(EXIT_FAILURE);}std::string line;while(std::getline(in_file, line)) {std::istringstream ss(line);Eigen::Vector3f mp;ss>>mp(0);ss>>mp(1);ss>>mp(2);Landmarks.push_back(mp);if (Debug)std::cout << (Landmarks.back())(0) << ": " << (Landmarks.back())(1)  << ": " << (Landmarks.back())(2)  << std::endl;}if (in_file.is_open()) in_file.close();}

2.观测信息

这里的观测信息包括里程计信息和激光信息，在tool.h中定义了一组观测信息的格式：

struct Record {Eigen::Vector3f odom;                 //里程计数据： 起点夹角, 行走距离, 终点夹角std::vector<Eigen::Vector3f> radar;       //激光数据：路标id号, 射线距离, 射线角度};

在实际project中，会有多组观测信息，所以这里用一个vector提前保存了所有数据：

std::vector<Record> Measurements;

首先解释一下里程计数据的格式，其中的起点夹角和终点夹角是指dθt+1′d\theta _{t+1}^{'}dθt+1′和dθt+1d\theta _{t+1}dθt+1，行走距离应该是直线距离RRR。（可能这里的角度关系还是有点理解不对，，，）

激光数据主要是关于landmark的观测信息，这里出了观测距离ranges和观测角度φ\varphiφ，还有路标的特殊id代号。激光观测原理如下

实际的观测数据由param/sensor_data.txt文件读入，某一项数据格式如下：

ODOMETRY 0.100692392654 0.100072845247 0.000171392857486
SENSOR 1 1.89645381418 0.374031885671
SENSOR 2 3.85367751107 1.51951017943

同样，用C++的文件读取方法将信息读入vector中：

bool ukf_console::getMeasurements(const std::string& path)
{std::ifstream in_file(path, std::ifstream::in);if (!in_file.is_open()) {std::cerr << "Cannot open input file: " << path << std::endl;exit(EXIT_FAILURE);}std::string line;Record record;int index = 0;while(getline(in_file, line)) {std::string sensor_type;std::istringstream ss(line);ss >> sensor_type;//measurement type r1 t r2if (sensor_type.compare("ODOMETRY") == 0) {//end the first record;if (record.radar.size() != 0) {Measurements.push_back(record);record.radar.clear();if (Debug && index < 50)std::cout << index << "-----------" << std::endl;index++;}auto& odo = record.odom;ss >> odo(0);ss >> odo(1);ss >> odo(2);if (Debug && index < 50)std::cout << (record.odom)(0) << ": " << (record.odom)(1) << ": " << (record.odom)(2) << std::endl;} else if (sensor_type.compare("SENSOR") == 0) {auto& radars = record.radar;Eigen::Vector3f radarR;ss >> radarR(0);ss >> radarR(1);ss >> radarR(2);radars.push_back(radarR);if (Debug && index < 50)std::cout << (radars.back())(0) << ": " << (radars.back())(1) << ": " << (radars.back())(2) << std::endl;}}if (record.radar.size() != 0) Measurements.push_back(record);if (in_file.is_open()) in_file.close();
}

3.运动模型

项目中的小车可以认为是两轮差动运动模型，如上图所示，即很容易就能得出下面的运行模型：

{xt+1=xt+R⋅cos⁡(θt+Δθ)yt+1=yt+R⋅sin⁡(θt+Δθ)θt+1=θt+Δθ+Δθˊ\left\{ \begin{array}{c} x_{t+1}=x_t+R\cdot \cos \left( \theta _t+\varDelta \theta \right)\\ y_{t+1}=y_t+R\cdot \sin \left( \theta _t+\varDelta \theta \right)\\ {\theta _{t+1}=\theta _t+\varDelta \theta +\varDelta \acute{\theta}}\\ \end{array} \right. ⎩⎨⎧xt+1=xt+R⋅cos(θt+Δθ)yt+1=yt+R⋅sin(θt+Δθ)θt+1=θt+Δθ+Δθˊ

在程序（unscented_kf.cpp的90-93）中，这样实现：

double angle = Xsig(2,i);
Xsig(0, i) = Xsig(0, i) + t * cos(angle + r1);
Xsig(1, i) = Xsig(1, i) + t * sin(angle + r1);
Xsig(2, i) = Xsig(2, i) + r1 + r2;

二、相关Markers的显示

喜欢使用ROS的一个很重要的原因就是它的可视化界面了，RVIZ里的各种marker可以显示很多丰富的信息，令程序调试十分的方便。

1.landmark以及pose的显示

这里采用圆柱体进行显示，直接看landmark的代码，estimatePose的不看了：

void ukf_console::LandmarksPublish()
{visualization_msgs::MarkerArray ma;visualization_msgs::Marker marker;// 设置该标记的命名空间和ID，ID应该是独一无二的// 具有相同命名空间和ID的标记将会覆盖前一个marker.header.frame_id = "map";marker.ns = "2Dlandmarks";// 设置标记行为：ADD（添 加），DELETE（删 除）marker.action = visualization_msgs::Marker::ADD;// 设置标记类型，初始值为球体。在立方体、球体、箭头和 圆柱体之间循环marker.type = visualization_msgs::Marker::CUBE;// 设置初始形状为圆柱体marker.id = 0;marker.pose.position.z = 0;marker.pose.orientation.x = 0.0;marker.pose.orientation.y = 0.0;marker.pose.orientation.z = 0.0;marker.pose.orientation.w = 1.0;// 设置标记的比例，所有方向上尺度1表示1米marker.scale.x = marker_scale;marker.scale.y = marker_scale;marker.scale.z = marker_scale;//设置标记颜色，确保alpha（不透明度）值不为0, 紫色marker.color.r = 160.0/255.0;marker.color.g = 32.0/255.0;marker.color.b = 240.0/255.0;marker.color.a = 1.0;marker.lifetime = ros::Duration(0);int markers_size = Landmarks.size();for(int i=0 ; i < markers_size ; i++){// 设置帧 ID和时间戳marker.header.stamp = ros::Time::now();marker.id++;//设置标记位姿。 marker.pose.position.x = (Landmarks[i])(1);marker.pose.position.y = (Landmarks[i])(2);ma.markers.push_back(marker);}Landmarks_pub.publish(ma);
}

2.观测信息显示

这里采用的是线段类型LINE_STRIP进行显示，看代码：

void ukf_console::AssociationPublish(const std::vector< Eigen::Vector3f >& radars)
{// visualization of the data associationvisualization_msgs::MarkerArray ma;visualization_msgs::Marker line_strip;line_strip.header.frame_id = "map";line_strip.header.stamp = ros::Time::now();line_strip.id = 0;line_strip.ns = "data_association";line_strip.type = visualization_msgs::Marker::LINE_STRIP;line_strip.action = visualization_msgs::Marker::ADD;line_strip.lifetime = ros::Duration(0.1);line_strip.pose.position.x = 0;line_strip.pose.position.y = 0;line_strip.pose.position.z = 0;line_strip.pose.orientation.x = 0.0;line_strip.pose.orientation.y = 0.0;line_strip.pose.orientation.z = 0.0;line_strip.pose.orientation.w = 1.0;line_strip.scale.x = marker_scale*0.3;  // line uses only x componentline_strip.scale.y = marker_scale*0.3;line_strip.scale.z = marker_scale*0.3;line_strip.color.a = 1.0;line_strip.color.r = 255/255;line_strip.color.g = 0;line_strip.color.b = 0;// robot pose     geometry_msgs::Point pointRobotPose;pointRobotPose.x = (ukf_tool->Mu_x)(0);pointRobotPose.y = (ukf_tool->Mu_x)(1);pointRobotPose.z = 0;//draw observation linesint visible_num = radars.size();for(int i = 0; i < visible_num; i++) {int index = 0;for ( ; index < ukf_tool->LandmarkINmap.size() ; index++) {if(ukf_tool->LandmarkINmap[index] == (int)(radars[i])(0)) break;}line_strip.points.clear();line_strip.points.push_back(pointRobotPose);geometry_msgs::Point pointLm;pointLm.x = ukf_tool->Mu_x(2 * index + 3);pointLm.y = ukf_tool->Mu_x(2 * index + 4);pointLm.z = 0.0;line_strip.points.push_back(pointLm);ma.markers.push_back(line_strip);line_strip.id++;}   Assoc_pub.publish(ma);
}

3.误差的显示

这里对位姿估计的误差，landmark估计的误差都进行了显示，通常都是采用不确定椭圆进行显示，这里也不例外。首先需要对相应的协方差矩阵求解特征向量和特征值，在tool.cpp文件用下面这个函数实现了这样的功能：

bool tool::computeEllipseOrientationScale2D(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues, const Eigen::Matrix2d& covariance)
{eigenvectors.setZero(2, 2);eigenvalues.setIdentity(2, 1);Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(covariance);if (eigensolver.info() == Eigen::Success){eigenvalues = eigensolver.eigenvalues();eigenvectors = eigensolver.eigenvectors();}else{eigenvalues = Eigen::Vector2d::Zero();  // Setting the scale to zero will hide it on the screeneigenvectors = Eigen::Matrix2d::Identity();return false;}// Be sure we have a right-handed orientation systemmakeRightHanded(eigenvectors, eigenvalues);return true;
}

另外，为了确保是右手坐标系（因为ROS系统中的tf坐标系是右手坐标系），这里需要进行一次检查：

void tool::makeRightHanded(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues)
{Eigen::Vector3d c0;c0.setZero();c0.head(2) = eigenvectors.col(0);c0.normalize();Eigen::Vector3d c1;c1.setZero();c1.head(2) = eigenvectors.col(1);c1.normalize();Eigen::Vector3d cc = c0.cross(c1);if (cc(2) < 0){eigenvectors << c1.head(2), c0.head(2);double e = eigenvalues(0);eigenvalues(0) = eigenvalues(1);eigenvalues(1) = e;}elseeigenvectors << c0.head(2), c1.head(2);
}

然后，就可以用协方差矩阵进行不确定性椭圆的绘制了，看代码：

void ukf_console::UncertaintyEllipseShow()
{visualization_msgs::MarkerArray ma;visualization_msgs::Marker marker;marker.header.frame_id = "map";marker.id = 0;marker.ns = "ekf_predict";marker.type = visualization_msgs::Marker::SPHERE;marker.action = visualization_msgs::Marker::ADD;marker.lifetime = ros::Duration(0.5);marker.pose.position.z = 0.1;marker.color.r = 135/255.0;marker.color.g = 206/255.0;marker.color.b = 235/255.0;marker.color.a = 0.8;//设置标记颜色，确保alpha（不透明度）值为0marker.scale.x = marker_scale;marker.scale.y = marker_scale;marker.scale.z = marker_scale;// covariance ellipsestf::Quaternion orientation;tf::Matrix3x3 tf3d;Eigen::Matrix2d eigenvectors;Eigen::Vector2d eigenvalues;Eigen::Matrix2d covariance;// get the covariance matrix 2x2 for each ellipsoid including robot pose// Count the number of landmarks + robot poseunsigned int objs_counter = ((ukf_tool->Mu_x.rows())-3 )/2 + 1;unsigned int pose_id=0;double quantiles=1 ;double ellipse_scale_=5;for (size_t i = 0; i < objs_counter; i++){marker.id++;if( i != 0)pose_id = i * 2 + 1;covariance=ukf_tool->Sigma_y.block(pose_id, pose_id, 2, 2);if(tool::computeEllipseOrientationScale2D(eigenvectors, eigenvalues, covariance) == false)continue;// Rotation matrix around  z axistf3d.setValue(eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), eigenvectors(1, 1), 0, 0, 0, 1);// get orientation from rotation matrixtf3d.getRotation(orientation);marker.pose.position.x = (ukf_tool->Mu_x)(pose_id);marker.pose.position.y = (ukf_tool->Mu_x)(pose_id+1);marker.pose.position.z = 0;marker.pose.orientation.x = orientation.x();marker.pose.orientation.y = orientation.y();marker.pose.orientation.z = orientation.z();marker.pose.orientation.w = orientation.w();marker.scale.x = ellipse_scale_ * quantiles * sqrt(eigenvalues(0));marker.scale.y = ellipse_scale_ * quantiles * sqrt(eigenvalues(1));marker.scale.z = 0.00001;  // Z can't be 0, limitation of ROSma.markers.push_back(marker);}Uncertainty_pub.publish(ma);
}

三、主循环

在结束各项准备工作之后，就可以进入主循环了，主循环其实也就是两个工作，预测和更新，非常简单：

void ukf_console::run()
{//read the map data for all landmarksgetLandmarks(Worlddata_filepath);//read the measurements with odometry and radar datagetMeasurements(Sensordata_filepath);LandmarksPublish();ukf_tool = new unscented_kf(Landmarks.size());ros::Rate rat(10);for (const auto& record : Measurements) { if (!ros::ok())   break;ukf_tool->ProcessMeasurement(record);EstimatePosePublish((ukf_tool->Mu_x).head(3));AssociationPublish(record.radar);UncertaintyEllipseShow();rat.sleep();}}

大致思路就是，将观测数据逐条送入ukf系统，进行ukf估计，观测结束，也就预示着程序结束了。

下一篇将会重点说说ukf的估计过程，这里就告一段落了。