Matlab判断矩阵的正定性，实际是判断特征值是否全为正

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

% 判断矩阵m是正定、半正定还是负定
m = [2 -1; -1 2]; 

if issymmetric(m) % 检查矩阵是否对称
    % disp('矩阵对称');
    d = eig(m); % 计算矩阵特征值
    if all(d > 0)
        disp('矩阵正定');
    elseif all(d >= 0)
        disp('矩阵半正定');
    else
        disp('矩阵负定');
    end
else
    disp('矩阵不对称');
end

二次规划的一般形式可以表示如下

其中$x$是n维的输入变量，$f$是n维向量，$H$是$n \times n$的对称矩阵，$A$是$m \times n$的矩阵，$b$是m维向量。

$A_{eq}$是矩阵，$b_{eq}, l_b, u_b$是向量。

二次规划的要求就是在约束条件下找到一个n维的向量X，使得$f(X)$的值最小。

二次规划问题的求解速度和怎么构造优化问题有一定的联系, 要想提升整体的求解速度和求解成功率, 应该尽量少的使用等式约束。

另一种形式如下，是OsqpEigen所用的形式，以后主要用这种

$H$ 就是Hessian矩阵，为方阵，大小和变量的个数相同。 $f$ 就是Gradient，如果没有线性部分，就设置为零向量。$A$ 就是约束矩阵，矩阵尺寸为约束个数 x 变量个数。 OSQP把等式和不等式约束放在一起了， 这里有时就需要自己推导一下约束矩阵了。如果是等式约束，就把上下限写成一个数就行

上下限向量的大小是约束的大小

• 如果H是半正定矩阵，那么f(X)是一个凸函数。

• 如果H是正定矩阵，那么全局最小值就是唯一的。

• 如果H=0，那么f(x)只剩线性部分，二次规划问题就变成线性规划问题。

• 如果至少有一个向量x满足约束而且f(x)在可行域有下限，二次规划问题就有一个全局最小值X。

对于维数较低情况，把二次型表达式转为矩阵形式比较简单，矩阵H的元素是二次型中的矩阵元素大小的两倍。 设矩阵H第i行第j列的元素大小为H(i,j)，二次项 $x_i x_j$ 的系数为a(i,j)，则

也就是平方项的系数对应矩阵H的对角元素，可以直接写出。

如果表达式太复杂，无法直接手写，使用Matlab求海森矩阵获得

1
2
3
4
5
6
7

syms x1 x2;
f = 0.5*x1.^2+x2.^2-x1*x2;
H = hessian(f, [x1, x2]);
% 转换为double类型
H = double(H);
% 输出矩阵
disp(h) 

c++

C++的求解使用osqp-eigen库，使用ospq感觉有点繁琐

安装 osqp(必需) 和 osqp-eigen

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

git clone --recursive https://github.com/osqp/osqp
cd osqp
mkdir build
cd build
cmake -G "Unix Makefiles" ..
cmake --build .
cmake --build . --target install

cd ../..
git clone https://github.com/robotology/osqp-eigen.git
cd osqp-eigen
mkdir build
cd build
安装到 /usr/local/lib/
cmake -DCMAKE_INSTALL_PREFIX:PATH=/usr/local ../
make
sudo make install

CMake配置

1
2
3
4
5

cmake_minimum_required(VERSION 3.0)
project(myproject)
find_package(OsqpEigen REQUIRED)
add_executable(example example.cpp)
target_link_libraries(example OsqpEigen::OsqpEigen)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

#include "OsqpEigen/OsqpEigen.h"
#include <Eigen/Dense>
#include <iostream>

int main()
{
    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian(2, 2);      //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd gradient(2);                    //Q: n*1向量
    Eigen::SparseMatrix<double> linearMatrix(2, 2); //A: m*n 矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd lowerBound(2);                  //L: m*1下限向量
    Eigen::VectorXd upperBound(2);                  //U: m*1上限向量
    // 注意稀疏矩阵的初始化方式,无法使用<<初始化
    hessian.insert(0, 0) = 2.0; 
    hessian.insert(1, 1) = 2.0;
    std::cout << "hessian:" << std::endl << hessian << std::endl;
    gradient << -2, -2;
    // 注意稀疏矩阵的初始化方式,无法使用<<初始化
    linearMatrix.insert(0, 0) = 1.0; 
    linearMatrix.insert(1, 1) = 1.0;
    std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;
    lowerBound << 1, 1;
    upperBound << 1.5, 1.5;
    // instantiate the solver
    OsqpEigen::Solver solver;
    // settings
    solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);
    // set the initial data of the QP solver
    // NumberOfVariables 与 NumberOfConstraints 跟 Hessian 矩阵的维数对应上
    solver.data()->setNumberOfVariables(2);   //变量数n
    solver.data()->setNumberOfConstraints(2); //约束数m
    if (!solver.data()->setHessianMatrix(hessian) )
        return 1;
    if (!solver.data()->setGradient(gradient))   // 二次规划的线性表达式，有时为空，此时用零向量
        return 2;
    if (!solver.data()->setLinearConstraintsMatrix(A))  // 约束中的矩阵A
        return 3;
    if (!solver.data()->setLowerBound(lowerBound))
        return 4;
    if (!solver.data()->setUpperBound(upperBound))
        return 5;
    // instantiate the solver
    if (!solver.initSolver())
        return 6;

    Eigen::VectorXd QPSolution;
    // solve the QP problem
    if (!solver.solve())
    {
        return 7;
    }
    QPSolution = solver.getSolution();
    std::cout << "QPSolution" << std::endl
              << QPSolution << std::endl; //输出为m*1的向量
    return 0;
}

结果为 1.0003, 1.0003

Matlab

x = quadprog(H,f,A,b,Aeq,beq,lb,ub) 在满足 lb ≤ x ≤ ub 的限制条件下求解上述问题。输入 lb 和 ub 是由双精度值组成的向量，这些限制适用于每个 x 分量。如果不存在等式约束，请设置 Aeq = [] 和 beq = []

1
2
3
4
5
6
7
8

H = [2 0; 0 2];
g = [-2; -2]

A = [1 0; 0 1];
b = [1.5; 1.5];
lb = [1; 1];

[x,fval,exitflag,output,lambda] = quadprog(H,g,A,b,[],[],lb);

结果直接为 1.0000, 1.0000. 比osqp-eigen更精确。

x是解。 fval是解处的目标函数值

python

OSQP的python的使用很简单，如果只是求解OSQP，不用C++，可以优先用python。 安装步骤

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

import osqp
import numpy as np
from scipy import sparse

# Define problem data
P = sparse.csc_matrix([[4, 1], [1, 2]])
q = np.array([1, 1])
A = sparse.csc_matrix([[1, 1], [1, 0], [0, 1]])
l = np.array([1, 0, 0])
u = np.array([1, 0.7, 0.7])
# Create an OSQP object
prob = osqp.OSQP()
# Setup workspace and change alpha parameter
prob.setup(P, q, A, l, u, alpha=1.0)
# Solve problem
res = prob.solve()

参考:
Matlab中的quadprog函数
OSQP库计算标准二次规划
Matlab计算二次规划
利用OSQP库计算标准二次规划(QP)问题的实例
如何使用OSQP-Eigen

同一个数据集，最简单的比较不同算法结果的方式是计算所建地图和真值地图的差别。但真值地图通常难以获得，需要有高精度传感器，比如 FARO Vantage Laser Tracker

找出最好的地图就是看哪个地图的噪音少，墙最准确，artifacts(假障碍)最少。

最常见和准确的评价地图的方法就是计算地图和真值地图的差别。常见的计算尺度是K近邻的归一化距离。真值地图难以获得时，可以比较估计的轨迹和真值轨迹的差距，衡量尺度是均方根差(RMSE)。其他的衡量参数还有轨迹的全局和局部误差，收敛速度，对噪音的健壮性。

评价SLAM算法质量的标准

1. 占据和空闲栅格的比例，检查是否有模糊的墙。模糊的障碍越多，地图质量越差。

2. 地图中角落的数量，低质量的地图的角落更多。真实的角落当然不能缺少，多余的角落必是错误的：artifacts, 轨迹匹配错误导致的双墙现象, 不准确的圆弧区域。

3. 封闭区域的数量。

对比的算法有gmapping, cartographer, tinySLAM, vinySLAM. 所用数据集是MIT stata center. 这个数据集提供了真值轨迹，所以可以用RMSE评价算法质量。

测试方案包括SLAM算法、数据集、iteration次数，测试是在远程server上运行的。

看完感觉论文讲的不太实用，而且定量的分析比较费时，实际中恐怕用不上，暂时用于定性判断地图质量的标准：

1. 模糊化的部分要尽量少
2. 假墙, artifact应当不存在
3. 不得有重影，区域形状不能有明显偏差

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

#include "ros/ros.h"
#include <tf/transform_listener.h>
#include <tf/tf.h>

int main( int argc, char* argv[] )
{
    ros::init(argc, argv, "test_tf");
    ros::NodeHandle nh;

    tf::TransformListener listener;
    geometry_msgs::PoseStamped in_pose;
    in_pose.header.frame_id = "parent";
    in_pose.header.stamp = ros::Time(0);
    in_pose.pose.position.x = 1;
    in_pose.pose.position.y = 2;
    in_pose.pose.position.z = 0;

    in_pose.pose.orientation.x = 0;
    in_pose.pose.orientation.y = 0;
    in_pose.pose.orientation.z = 0.344215153;
    in_pose.pose.orientation.w = 0.93889079678;
    ros::Rate r(10);
    while(nh.ok() )
    {
        out_pose.header.frame_id = "child";
        // listener->waitForTransform("child", "parent", ros::Time(0),  ros::Duration(1) );
        listener->transformPose("child", in_pose, out_pose);
        r.sleep();
    }
}

如果没有while循环，只是一个transformPose，运行就会报错，找不到child，即使tf关系是正常的。如果还是报错，需要加上waitForTransform一行

问题

tf使用transformPose报错: Exception in transformPose: Quaternion malformed, magnitude: 0 should be 1.0

源码在此，原因是四元数平方和偏离1太多

1
2
3
4
5
6
7
8
9

inline void assertQuaternionValid(const geometry_msgs::Quaternion & q)
{
   if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
   {
     std::stringstream ss;
     ss << "Quaternion malformed, magnitude: " << q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w << " should be 1.0" <<std::endl;
     throw tf::InvalidArgument(ss.str());
   }
};

python中使用transformPose

1
2
3
4
5
6
7
8
9
10
11
12
13
14

import rospy
import tf
from tf import TransformListener

quaternion = tf.transformations.quaternion_from_euler(0, 0, self.ang, axes='sxyz')
self.temp_pose.pose = Pose(Point(self.goal_x, self.goal_y, 0.000), Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3]))
self.temp_pose.header.frame_id = 'base_link'
# 解决时间戳的问题
self.temp_pose.header.stamp = rospy.Time(0)
try:
    self.tf_listener_.waitForTransform("odom", "base_link", rospy.Time(0), rospy.Duration(2.0) )
    self.goal = self.tf_listener_.transformPose('odom', self.temp_pose)
except (tf.LookupException, tf.ConnectivityException):
    rospywarn('tranform frame failed !')

开始总是出现target_frame does not exist的错误，但是用tf_echo明明可以看到两个坐标系，于是加上了waitForTransform，没有问题了。但是又出现了那个时间戳范围不足的问题，还是Time(0)解决

参考：
tf listener cant look up transform frame

单一多项式插值会有两个问题：

1. 随着多项式的阶数越来越高，计算量也越来越大。
2. 随着多项式的阶数越来越高，插值精度并不会越来越高，恰恰相反，函数曲线会出现剧烈的振荡，即龙格现象。

既能穿过所有已知点又能避免龙格现象（剧烈的震荡），就要用分段函数插值。就是把所有的已知数据分割成若干段，每段都对应一个插值函数，最终得到一个插值函数序列。

为了保证分段函数之间彼此衔接，每个分段函数都采用高次函数形式来构造（三次样条差值 就是用x的三次方形式构造）这就保证了多个函数之间的衔接光滑。不能用过高阶的函数，否则抖动太剧烈。

三次样条插值就是把已知数据分割成若干段，每段构造一个三次函数，并且保证分段函数的衔接处具有0阶连续，一阶导数连续，二阶导数连续的性质（也就是光滑衔接）。

一阶连续意味着曲线y=S(x)没有急转弯，没有特别剧烈的跳变。二阶连续意味着每个点的曲率半径有定义。

给定n+1个数据点，共有n个区间，推导如下

自由边界：首尾两端没有受到任何让它们弯曲的力。让端点的斜率自由的在某一位置保持平衡，使得曲线的摇摆最小。
固定边界：首尾两端点的微分值是被指定的。用外力使柔软而有弹性的曲线经过数据点，并在端点处使其具有固定斜率。
非节点边界：指定样条曲线的三次微分匹配

文章QP方法解基于五次多项式形式的cost function minimization问题不是分段的多项式函数，问题相当简单。

参考：
原理
如何推导

安装依赖库

1
2

sudo apt-get install gnuplot
sudo apt-get install gnuplot-x11  

这样还没完，还需下载gnuplot，解压之后，输入 make 进行编译。如果在自己的项目需要使用gnuplot，将gnuplot_i.hpp复制到对应项目，在程序中加入 #include "gnuplot_i.hpp"， 无需链接动态库， 这也是我使用它的主要原因，懒得改CMake

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

Gnuplot g1;

void plot(const vector<double>& Xs, const vector<double>& Ys, const string& comment, const string& style)
{
    g1.set_grid();
    g1.set_pointsize(3);
    g1.set_style(style).plot_xy(Xs, Ys, comment);
}

void wait_for_key()
{
    cout << "Press Enter to continue ..." <<endl;
    cin.clear();
    cin.ignore(cin.rdbuf()->in_avail() );
    cin.get();
    return;
}

            /*以下为main函数部分*/
std::vector<double> X_t, Y_t;
/*对所有 vector 插入数据*/

g1.set_grid();
g1.set_xlabel("X");  //设置x轴说明
g1.set_ylabel("Y");  //设置y轴说明
// 图例文字
g1.set_style("lines").plot_xy(X_t, Y_t, "left");
plot(X_t, Y_t, "left", "points lc rgb 'red' pt 7");

wait_for_key();

图样(style): gnuplot 描绘数据数据图形是以读入档案中的坐标值后，以图样绘上。gnuplot可提供9种图样，我常用的是:

1. lines: 将相邻的点以线条连接
2. points: 将每一点以一符号绘上
3. linespoints: 同时具有lines 及 points 的功能。

其他还包括

• linestyle 连线风格（包括linetype，linewidth等）
• linetype 连线种类
• linewidth 连线粗细
• linecolor 连线颜色
• pointtype 点的种类
• pointsize 点的大小

参考:C++ 调用Gnuplot实现图形绘制的过程

论文

论文A comparison of line extraction algorithms using 2D range data for indoor mobile robotics对比了6种提取雷达数据线条算法的效果。结论如下：

Split-and-Merge 和 Incremental由于处理速度和准确性而表现最好。 对于实时的应用，Split-and-Merge是最佳选择，因为处理速度很快。它也是地图定位问题的第一选择，此时FalsePos不太重要。但是，算法实现的细节对应用是很重要的。

对于laser_line_extraction对应的03年论文，这篇论文在介绍Split-and-Merge时没有提到，只在Related Work里提了一下。但是laser_line_extraction的介绍里声称是使用了Split-and-Merge算法，还有待观察。

以及2017年的论文 A line segment extraction algorithm using laser data based on seeded region growing，效果在Line Segment Extraction

原理

对雷达扫描的结果，如果形状接近直线，就进行连线。

先对离群点进行过滤，再执行split-and-merge算法决定哪些点属于线，然后使用weighted line fitting算法找到最合适的线和相应的协方差矩阵。

使用roslaunch laser_line_extraction example.launch，修改参数以符合自己的需求。

参数

• bearing_std_dev (default: 0.001): The standard deviation of bearing uncertainty in the laser scans (rad).

• min_split_dist (default: 0.05): When performing “split” step of split and merge, a split between two points results when the two points are at least this far apart (m) 线段split的阈值，过大时很多线段被合并成一条，过小时，出现很多碎短的线段

• outlier_dist (default: 0.05): Points who are at least this distance from all their neighbours are considered outliers (m).

• range_std_dev (default: 0.02): The standard deviation of range uncertainty in the laser scans (m).

1
2
3
4
5

float32 radius
float32 angle
float32[4] covariance
float32[2] start
float32[2] end

• start和end分别是这个线段的起点坐标和终点坐标。
• angle: 原点到直线的垂线的角度，角度以机器人为坐标系计算，范围是-PI~PI
• radius: 原点到直线的距离

这两个参数完全能够根据start和end点计算出来。不理解这两个参数存在的意义。

covariance是2X2的矩阵，是radius和angle的协方差

line_extractor

line_extractor发布:

1
2
3
4
5
6
7
8

camera_line_markers     [visualization_msgs/Marker]
camera_line_segments    [laser_line_extraction/LineSegmentList]
line_markers            [visualization_msgs/Marker]
line_segments           [laser_line_extraction/LineSegmentList]

taimi/line_center_point       [nav_msgs/Odometry]
taimi/line_wall_position      [geometry_msgs/PoseStamped]
taimi/waste_aruco_position    [geometry_msgs/PoseStamped]

订阅:

1
2
3
4
5

camera1/scan [sensor_msgs/LaserScan]
shelf_lifter/cmd [std_msgs/String]
sick_tim551_scan [sensor_msgs/LaserScan]
tf [tf2_msgs/TFMessage]
tf_static 

参考：室内移动机器人二维激光数据线特征提取算法的总结与开源算法分享