几个重要文件的启动顺序：

1. 通过/boot/vm进行启动 vmlinuz

2. init /etc/inittab

3. 启动相应的脚本，并且打开终端

   1 2 3 4 5

   rc.sysinit rc.d(里面的脚本) rc.local

4. 启动login登录界面

5. 登录，此时执行sh脚本的顺序，每次登录的时候都会完全执行的

   1 2 3 4 5 6 7 8 9

   /etc/profile.d/file /etc/profile /etc/bashrc /root/.bashrc /root/.bash_profile

这里就清楚了，如果不登录还要加载环境变量，就只能把环境变量放到rc.local里

添加开机启动项： sudo vim /etc/profile.d/apps-bin-path.sh， 在其中放入可执行文件

函数在stack上分配的空间在此函数执行结束时会释放掉，这样就产生了一个问题: 如果想将函数中此变量的值保存至下一次调用时，如何实现？ 最容易想到的方法是定义为全局的变量，但这样最明显的缺点是 破坏了此变量的访问范围 (使得在此函数中定义的变量，不仅仅只受此函数控制). 想要使用全局变量的之前应该先考虑使用 static

全局变量和静态变量的存储都放在内存的全局区

全局变量和全局静态变量的区别

• 全局变量默认是有外部链接性的，作用域是整个工程，在一个文件内定义的全局变量，可以在另一个文件中使用。比较规范的方法是：在A.h中声明，比如extern int a;，但不能赋值，否则报错。在A.cpp中定义，int a=1;。然后在B.cpp中使用，cout << a <<endl;

• 全局静态变量是显式用 static 修饰的全局变量，作用域仅在声明此变量的文件，其他的文件即使用 extern 声明也不能使用。这样即使两个不同的源文件都定义了相同名字的static全局变量，它们也是不同的变量。

1
2
3
4
5
6
7
8
9

void test_static()
{
    static int n=0;
    n++ ;
    cout << n <<endl;
}
    test_static();
    test_static();
    test_static();

运行结果是

1
2
3

1
2
3

静态局部变量有以下特点：

1. 该变量在全局数据区分配内存；
2. 静态局部变量在程序执行到该对象的声明处时，被首次初始化，即以后的函数调用不再进行初始化。即上面的static int n=0;
3. 静态局部变量一般在声明处初始化，如果没有显式初始化，会被程序自动初始化为 0；比如上面的n可以不初始化为0
4. 始终驻留在全局数据区，直到程序运行结束。但其作用域为局部作用域，当定义它的函数或语句块结束时，其作用域随之结束。
5. 它和全局变量的区别:全局变量对所有的函数都是可见的，而static局部变量只对定义自己的函数体可见。

把局部变量改变为static变量后是改变了它的生存期和内存中的存储区域，作用域其实不变。 把全局变量改变为static变量是改变了它的作用域，限制了它的使用范围。

在一个package中新建文件夹，第一个文件夹为urdf，用来放置模型文件。第二个文件夹叫做meshes，用来放置外观纹理，一般是通过三维软件建模完成后，导出并放置在meshes中，第三个为launch文件夹。第四个是config文件夹，功能包的配置文件以及rviz的显示配置文件。

我们要准备这样的launch文件：

1
2
3
4
5
6
7
8
9

<launch>                                               
    <param name="robot_description" textfile="$(find robot_description)/urdf/robot.urdf" />
    <!-- 设置GUI参数，显示关节控制插件，配合下面的joint_state_publisher使用 -->
    <param name="use_gui" value="true" />
    <!-- 根据关节状态，创建tf关系，并发布到tf tree中  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <!-- 发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</launch>

一个简单的模型，就是一个底盘带一个轮子：

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

<?xml version="1.0" ?>
<robot name="robot">
	<material name="Black">
		<color rgba="0.15 0.15 0.15 1"/>
	</material>
	<material name="Blue">
		<color rgba="0 0 0.8 1"/>
	</material>

	<link name="base_link">
		<visual>
			<origin xyz="0 0 0" rpy="0 0 0" />
			<geometry>
				<box size="0.5 0.3 0.01" />
			</geometry>
			<material name="Black"/>
		</visual>
	</link>

	<link name="left_front_wheel_link">
		<visual>
			<origin xyz="0 0 0" rpy="1.5707 0 0" />
			<geometry>
				<cylinder radius="0.1125" length="0.064" />
			</geometry>
			<material name="Blue"/>
		</visual>
	</link>

	<joint name="left_front_joint" type="continuous">
		<origin xyz="0.1375 0.182 0" rpy="0 0 0" />
		<parent link="base_link" />
		<child link="left_front_wheel_link" />
		<axis xyz="0 1 0" />
	</joint>
</robot>

使用check_urdf robot.urdf可以检查语法,但是偶尔也有漏掉错误的时候.

第一行的xml版本声明,开头不能有空格, 也不能加注释. 在urdf文件中不能加中文注释

joint_state_publisher 和 robot_state_publisher

joint_state_publisher节点读取robot_description参数,这个就是我们的urdf文件.

use_gui参数,布尔类型,默认false,决定是否显示GUI界面. 这个小工具把所有joint做成slider形式,供用户测试,对continuous的joint,slider范围是-π到π

发布话题/joint_states, 消息类型sensor_msgs/JointState,可以echo查看

robot_state_publisher订阅话题/joint_states, tf变换也是它发布的,它有一个参数use_tf_static,用于决定是否使用tf_static latched 静态变换的广播,而这个参数默认true.

一般写成launch形式:

1
2
3
4
5
6

<launch>
    <param name="robot_description" textfile="$(find robot_description)/urdf/robot.urdf" />
    <param name="use_gui" value="false" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</launch>

现在可以启动完整的机器人程序了，有了map坐标系，也就有了map->base_link转换，现在移动机器人，rviz里的模型就会跟着移动了。

问题

rviz里没有显示机器人的模型，发现终端有这样的错误：

在urdf文件中对某个约束改名为camera，对相应的STL文件也改名后，在本地机的rviz打开RobotModel，结果终端报错：

但是rviz里没报错，tf树是正常的。最后发现还是URDF文件里的STL路径没有改过来

urdf_reader.cc

cartographer中的urdf_reader.cc是从urdf文件读取所有的坐标系变换，返回std::vector<geometry_msgs::TransformStamped>类型，然后使用下面的代码发送TF变换

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

#include "urdf_reader.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "urdf/model.h"
#include "absl/strings/str_split.h"

  tf2_ros::Buffer tf_buffer;
  std::vector<geometry_msgs::TransformStamped> urdf_transforms;
  const std::vector<std::string> urdf_filenames =
      absl::StrSplit(FLAGS_urdf_filenames, ',', absl::SkipEmpty());
  for (const auto& urdf_filename : urdf_filenames) {
    const auto current_urdf_transforms =
        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
    urdf_transforms.insert(urdf_transforms.end(),
                           current_urdf_transforms.begin(),
                           current_urdf_transforms.end());
  }
  ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;
  if (urdf_transforms.size() > 0)
      static_tf_broadcaster.sendTransform(urdf_transforms);
  else
  	  ROS_WARN("no transform in urdf file !");

参考：
古月居的urdf教程
urdf文件编码导致的缺少tf变换

pcd_viewer

查看pcd文件 pcd_viewer <filename>

• 设置点的大小 pcl_viewer file.pcd -ps 3

• 设置点的颜色: pcl_viewer -fc 173,255,47 file.pcd

• 查看点云的坐标: pcl_viewer test.pcd -use_point_picking，然后按住shift选择点，在界面显示界面选择点云之后，会在终端输出点云坐标。

• 同一窗口里打开多个文件，分开显示: pcl_viewer -multiview 1 pig1.pcd pig2.pcd test.pcd

• 同一窗口打开多个文件，在一起显示: pcl_viewer pig1.pcd pig2.pcd test.pcd

快捷键:

• 1键: 改变点云颜色
• r键: 重现视角。如果读入文件没有在主窗口显示，不妨按下键盘的r键一试。
• j键: 截图功能。
• g键: 显示/隐藏 坐标轴。

如何判断点云的数据类型

先转成pcd文件，然后用文本形式打开，看FIELDS一行，可能的结果有

1
2

FIELDS x y z
FIELDS x y z intensity timestamp ring

pcl_ros 包

这个包的工具如下：

如果想获得雷达的点云数据，可以发布PointCloud2类型的话题，同时用rosbag record录制得到bag文件，然后使用pcl_ros包中的bag_to_pcd得到pcd点云文件，用法：rosrun pcl_ros bag_to_pcd input.bag topic output_directory，过程：

注意输出是个目录，因为bag文件的消息数量就是生成的pcd文件数量，bag文件的消息数量可以用rosbag info查看

可以用pcl_viewer工具查看pcd，但是这样转换出的点云pcd文件在用Cloud Compare打开时报错

考虑换一种生成方式，在当前路径将点云ROS数据转为pcd文件，也就是一个pcd对应一帧点云，但是会不停地转换
rosrun pcl_ros pointcloud_to_pcd input:=/hesai/pandar _prefix:=./pcd

pcd转ply程序： pcl_pcd2ply，使用格式：pcl_pcd2ply demo.pcd demo.ply。生成的pcd转成ply之后可以导入Cloud Compare

pointcloud_to_laserscan包

将3D点云转换为2D的雷达scan, 最适用于把Kinect相机用作雷达，再使用2D算法。详细使用

我们可以直接利用kinect的点云数据，因为costmap2D的接口是直接调用点云。问题是计算量很大，点云规模太大，可以把点云数量降下去，但这样效果又不好了，可能无法识别障碍。点云覆盖信息大，对障碍物信息敏感。计算量大，实时性差。从障碍物进入相机视野出现到加入代价地图有1.5秒时间

depthimage_to_laserscan

如果想从RGBD相机创建虚拟的雷达scan，这个包的效果更好，它直接处理图像数据而不是点云。
详细使用

使用Kinect的一个方案是把点云或深度图降维生成线激光，本质上是当成2D激光雷达，在平坦地面环境用。还是用gmapping、hector SLAM、cartographer等手段建图

参考：
ROS中解析bag包中的点云文件到pcd格式
激光雷达bag文件播放和转PCD文件

ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。

IterativeClosestPoint类提供了标准ICP算法的实现（The transformation is estimated based on SVD），算法迭代结束条件有如下几个:

• 最大迭代次数： 最大迭代次数。 setMaximumIterations (100)
• 两次变化矩阵之间的差值：前一个变换矩阵和当前变换矩阵的差异小于阈值时，就认为已经收敛了。 setTransformationEpsilon(1e-10)
• 均方误差（MSE）：均方误差和小于给定阈值， 停止迭代。 setEuclideanFitnessEpsilon(0.01)

align函数是配准，在使用之前至少给定上面三个条件，还有setMaxCorrespondenceDistance等其他函数。PCL的ICP里的transformation estimation就是基于SVD分解实现的。

如果从一个好的初始猜想变换矩阵开始迭代，那么算法将会在比较少的迭代之后就收敛，配准结果也较好，当像我们这里没有指定初始guess时，就默认使用单位阵Matrix4::Identity()

计时

1
2
3
4

pcl::console::TicToc time;
time.tic ();
//需要记录执行多长时间的代码
cout <<time.toc () << " ms :<< endl;

日志输出

同ROS类似，就是把ROS换成了PCL: PCL_DEBUG, PCL_INFO, PCL_WARN, PCL_ERROR

参考：

matlab的操作步骤：

1. 产生独立变量，为带有两个变量 x 和 y 的集合，meshgrid是一个可以建立独立变量的函数，产生矩阵元素，元素x和y按照指定的范围和增量来产生。
2. 输入要使用的函数
3. 调用contour(x,y,w)命令，contour函数是画一个多维函数的等高线

1
2
3
4

[x,y] = meshgrid(-5:0.05:5,-5:0.05:5)
w = x.^2+y.^2
contour(x,y,w, 'showText', 'on')
% surf(x,y,w), title('等高线')

surf函数用于画三维的等高线

高维高斯分布的概率密度函数和等高线图

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

u=[0;0];%均值
v=[4,3;3,9];%协方差阵
x=-7:0.05:7;
y=-7:0.05:7;

[X,Y]=meshgrid(x,y);
s2x=v(1,1)      %x的方差
s2y=v(2,2)
sx=sqrt(s2x)    %标准差多个
sy=sqrt(s2y)
Cov=v(1,2)
r=Cov/(sx*sy)
a=1/(2*pi*sx*sy*sqrt(1-r^2));
b1=-1/(2*(1-r^2));
b2=((X-u(1))./sx).^2;
b3=((Y-u(2))./sy).^2;
b4=2*r.*(X-u(1)).*(Y-u(2))./(sx*sy)
Z=a*exp(b1*(b2+b3-b4));     %也就是f(x1,x2)的表达式

mesh(X,Y,Z),title('密度函数图')
figure
contour(X,Y,Z,'showText','on'),title('等高线图')

参考： 使用surface 和 contour 画图

安装

ceres是google库，首先安装相关依赖

1
2
3

sudo apt-get install -y libatlas-base-dev
sudo apt-get install -y liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev 
sudo apt-get install -y libgoogle-glog-dev libgtest-dev

如果安装时找不到 cxsparse 或者其他的lib，需要添加下面的源

1

sudo vim /etc/apt/sources.list

从github上下载，这里要注意ceres的版本和Eigen是搭配的，ceres版本越新，对Eigen的版本要求也越新，它的CMakeLists里有提示，所以不要安装最新的。 安装2.0.0 即可

下载解压后执行老一套命令：

1
2
3
4
5

mkdir build
cd build
cmake ..
make 
sudo make install

配置 CMake

安装官方的说明配置是错误的，应该是这样：

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

find_package(Ceres REQUIRED)

INCLUDE_DIRECTORIES(/usr/include/eigen3)

include_directories(
  ${catkin_INCLUDE_DIRS}
  # 这行可以没有
  ${CERES_INCLUDE_DIRS} 
)

add_executable(program src/program.cpp) 
target_link_libraries(program
   ${catkin_LIBRARIES}
   ${CERES_LIBRARIES}
)

ceres使用LM或狗腿算法，求解参数的数据类型只支持 double*。Ceres相对于g2o的缺点仅仅是依赖的库多一些（g2o仅依赖Eigen）．但是可以直接对数据进行操作

ceres是求解给定函数的最小值

Solver::Options

• Solver::Options::trust_region_strategy_type 可以取LEVENBERG_MARQUARDT或DOGLEG

• Solver::Options::use_inner_iterations 设为True，可以启用 Ruhe & Wedin的非线性推广的算法II。这个版本的Ceres具有更高的迭代复杂度，但是每次迭代都显示更好的收敛行为。把 Solver::Options::num_threads设为最大值是非常值得推荐的

• minimizer_progress_to_stdout

• num_threads: 设置使用的线程，但有时线程少反而用时更少，不明白为什么

linear_solver_type

对于非线性最小二乘问题，最终转化为求解方程 $H\Delta x = g$

Ceres提供了很多计算的方法，这就涉及Solver::Options::linear_solver_type的取值问题

• DENSE_QR

默认值。 适合使用稠密雅可比矩阵的小规模问题（几百个参数和几千个残差），也就是使用Eigen库的稠密矩阵QR分解。如果ceres优化问题不是SLAM的大型后端，不是稀疏问题，使用DENSE_QR

• DENSE_NORMAL_CHOLESKY & SPARSE_NORMAL_CHOLESKY

大规模的非线性最小二乘问题通常是稀疏的。对于这种情况使用稠密QR分解是低效率的，改用Cholesky因式分解，它有两种变体 - 稀疏和密集。

DENSE_NORMAL_CHOLESKY是执行正规方程的稠密Cholesky分解。 Ceres使用Eigen稠密的LDLT因式分解算法。

SPARSE_NORMAL_CHOLESKY是执行正规方程的稀疏Cholesky分解，这为大量稀疏问题节省了大量的时间和内存消耗。Ceres使用SuiteSparse 或 CXSparse库中的稀疏Cholesky分解算法 或 Eigen中的稀疏Cholesky分解算法。

如果Ceres编译时支持了这三个库，那么linear_solver_type默认值是SPARSE_NORMAL_CHOLESKY，否则就是DENSE_QR。使用最多的是DENSE_QR，cartographer前端的ceres scan mather用的也是DENSE_QR

• CGNR : 使用共轭梯度法求解稀疏方程

• DENSE_SCHUR & SPARSE_SCHUR : 适用于BA问题

ceres::Solver::Summary 的常用函数

ceres::Solver::Summary summary;

• double Solver::Summary::total_time_in_seconds. Time (in seconds) spent in the solver

直接使用summary.total_time_in_seconds

• int Solver::Summary::num_threads_given: Number of threads specified by the user for Jacobian and residual evaluation.

• int Solver::Summary::num_threads_used

Number of threads actually used by the solver for Jacobian and residual evaluation. This number is not equal to Solver::Summary::num_threads_given if none of OpenMP or CXX_THREADS is available.

以上两个线程的参数是由Options::num_threads决定的，如果设置太大，这两个参数就会不同，只会用最大线程数。

• min_linear_solver_iteration 和 max_linear_solver_iteration：线性求解器的最小/最大迭代次数，默认为0/500，一般不需要更改

• max_num_iterations ： 默认是50。求解器的最大迭代次数，并不是越大越好。对于SLAM前端的实时性有要求，所以max_num_iterations不能太大，ALOAM里设置为4

• bool Solver::Summary::IsSolutionUsable() const

算法返回的结果是否数值可靠。 也就是Solver::Summary:termination_type是否是CONVERGENCE, USER_SUCCESS 或者 NO_CONVERGENCE，也就是说求解器满足以下条件之一：

1. 达到了收敛误差
2. 达到最大迭代次数和时间
3. user indicated that it had converged

通过实验发现除了多线程以及 linear_solver_type，别的对优化性能和结果影响不是很大

BriefReport

FullReport

例子

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

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.259266e+05    0.00e+00    5.00e+04   0.00e+00   0.00e+00  1.00e+04        0    1.91e-05    1.26e-04
   1  2.468045e+04    1.01e+05    2.69e+02   1.31e+01   1.00e+00  3.00e+04        1    5.60e-05    2.02e-04
   2  2.467825e+04    2.20e+00    1.17e+00   5.00e-02   1.00e+00  9.00e+04        1    2.29e-05    2.37e-04

Solver Summary (v 2.0.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                            4                        3
Parameters                                 12                        9
Residual blocks                             5                        5
Residuals                                  15                       15

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        3

Cost:
Initial                          1.259266e+05
Final                            2.467825e+04
Change                           1.012484e+05

Minimizer iterations                        3
Successful steps                            3
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.000106

  Residual only evaluation           0.000009 (3)
  Jacobian & residual evaluation     0.000026 (3)
  Linear solver                      0.000053 (3)
Minimizer                            0.000166

Postprocessor                        0.000004
Total                                0.000276

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 6.530805e-10 <= 1.000000e-06)

参考:
官方教程学习笔记（十三）

欧氏变换

欧氏变换(Isometry Transform)可以看作是维持任意两点距离不变的变换，在实际场景中使用比较多。在Eigen中已经内置好了一些常用的欧氏变换:

1
2
3
4

typedef Transform<float,2,Isometry> Isometry2f;
typedef Transform<float,3,Isometry> Isometry3f;
typedef Transform<double,2,Isometry> Isometry2d;
typedef Transform<double,3,Isometry> Isometry3d;

1
2
3
4
5
6

AngleAxisd rotation(3.1415926 / 4, Vector3d(1, 0, 1).normalized());
Vector3d translation(1, 3, 4);
Isometry3d T= Isometry3d::Identity();
// 先平移后旋转
T.translate(translation);
T.rotate(rotation);

A.translate(B)等价于A×B，而A.pretranslate(B)等价于B×A，对应于左乘和右乘的区别。凡是前面带pre的函数，其变化都是相对于上一步变化之前的状态进行的。举例说我要新建一个按固定轴先平移后旋转的变换。但我首先设置了旋转，然后再设置平移。这个时候设置平移就不能用translate()了，而应该用pretranslate()。因为第一步已经对坐标系进行了旋转，后面的平移是在旋转后的坐标系中进行的，所以最好不要用pre开头的函数

针对求解Ax = b这种线性问题，Eigen提供了下面几种分解方法，每一种方法都提供了一个solve()函数以便求解得到 x，Eigen对每一种分解方法的速度和精度做了如下对比。当然对于小矩阵，各个方法没什么区别

LLT(cholesky)是最快的求解器，但是精度也是最差的，并且只能对正定矩阵进行分解，而LDLT则可以应对正半定和负半定问题，精度较LLT更高，所以尽量使用LDLT，但是LDLT在求解大矩阵问题时，耗时较QR增加更多，所以究竟选择那种分解方式求解问题，需要根据速度和精度综合考量

使用info()判断是否收敛

1
2
3

SelfAdjointEigenSolver<Matrix2f> eigensolver(A);
if (eigensolver.info() != Success)
    abort();

1
2
3
4

Eigen::SelfAdjointEigenSolver< Eigen::Matrix3d > eigen_solver( matrix_33 );
// 特征值   特征向量
cout << "Eigen values = " << eigen_solver.eigenvalues( ) << endl;
cout << "Eigen vectors = " << eigen_solver.eigenvectors( ) << endl;

cholesky 分解

Cholesky分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。Eigen的LLT分解实现了Cholesky分解。代码如下：

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

#include<Eigen/Cholesky>
int main(int argc, char** argv)
{
   Eigen::Matrix2d down;
   down<<1,0,
         2,1;
   // A <<1,2
   //   2,5
   Eigen::Matrix2d A = down*down.transpose();
   std::cout<<"A: "<< A <<std::endl;
   // 直接用 llt() 函数
   Eigen::Matrix2d ml = A.llt().matrixL();
   Eigen::Matrix2d testA = ml*ml.transpose();

   std::cout<<"mllt:   "<< ml<<std::endl;
   std::cout<<"testA:  "<<testA<<std::endl;
  return 0;
}

LDLT分解

LDLT分解法实际上是Cholesky分解法的改进，优先使用LDLT而不是LLT方法。 Cholesky分解法虽然不需要选主元，但其运算过程中涉及到开方问题，而LDLT分解法则避免了这一问题。仍然要求A矩阵正定。 其中L为下三角形 单位 矩阵(即主对角线元素皆为1，下三角其他元素不为0)，D为对角矩阵， $L^T$ 为L的转置矩阵。

1
2
3
4
5
6
7

Matrix2f A, b;
A << 2, -1, -1, 3;
b << 1, 2, 3, 1;
cout << "Here is the matrix A:\n" << A << endl;
cout << "Here is the right hand side b:\n" << b << endl;
Matrix2f x = A.ldlt().solve(b);
cout << "The solution is: \n" << x << endl;

QR分解

1
2
3
4
5
6
7

Matrix3f A;
Vector3f b;
A << 1,2,3,  4,5,6,  7,8,10;
b << 3, 3, 4;
// 返回一个类ColPivHouseholderQR的对象
Vector3f x = A.colPivHouseholderQr().solve(b);
cout << "The solution is:\n" << x << endl;

SVD分解 - 奇异值分解

特征值分解仅针对方阵，而不是方阵的矩阵就有了SVD分解： $A=U\Sigma V^T$
其中A为m x n的矩阵， 正交矩阵 U(m x m阶) 和 V(n x n阶)。

矩阵U的列称为左奇异向量, 是正交的。矩阵V的列向量(也称为右奇异向量)也是正交的.

此时的A如果是方阵，那么逆矩阵也很容易求出: $A^{-1} = VD^{-1}U^T$
奇异值分解同时包含了旋转、缩放($\Sigma$)和投影三种作用。特征值分解只有缩放的效果。

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

Eigen::Matrix3f A;
A << 3,4,2,
     1,2,4,
     8,0,1;
// SVD分解
Eigen::JacobiSVD<Eigen::Matrix3f> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
// 求出三个矩阵
Eigen::Matrix3f U = svd.matrixU();
Eigen::Matrix3f V = svd.matrixV();
// 推荐方法，从对角线元素组成的向量直接构造对角矩阵
Eigen::Matrix3f diag = svd.singularValues().asDiagonal();

cout << "U: "<<endl<< U <<endl<<endl;
cout << "V transpose: "<< endl << V.transpose() <<endl<<endl;
cout << "Sigma from SVD: "<< endl << diag<<endl<<endl;

// 判断U和V是否正交矩阵（酉矩阵），和转置的乘积为单位矩阵
cout << U.isUnitary() <<endl<<endl;
cout << V.isUnitary() <<endl<<endl;
cout <<"***********************"<<endl;

// 从公式推导出的对角矩阵，有误差
Eigen::Matrix3f Sigma = U.inverse() * A * V.transpose().inverse();
cout << "Sigma from formula: "<< endl << Sigma <<endl<<endl;

// 大致等于原矩阵A
cout<< "A from formula: "<< endl <<U *Sigma *V.transpose()<<endl;

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

U: 
   0.4853 -0.514629 -0.706853
 0.299358 -0.661777   0.68734
 0.821504  0.545168  0.167102

V transpose: 
  0.904647   0.275928   0.324773
  0.423663  -0.664689  -0.615385
-0.0460712    -0.6943    0.71821

S from SVD: 
9.20501       0       0
      0  5.0882       0
      0       0 2.09237

1

1
***********************
S from formula: 
     9.20501 -9.53674e-07 -2.38419e-07
 -1.3113e-06       5.0882  1.05798e-06
-7.15256e-07  2.90573e-07      2.09237

A from formula: 
3 4 2
1 2 4
8 0 1

• A转为正交矩阵

把上面的ComputeFullU换成ComputeThinU，那么$U \times V^T$ 就是一个正交矩阵B

1
2
3
4
5
6
7
8
9

// SVD分解, ComputeThin参数时，A必须是 Eigen::MatrixXf
Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV );
// 求出三个矩阵，这样B就是个正交矩阵
Eigen::Matrix3f U = svd.matrixU();
Eigen::Matrix3f V = svd.matrixV();
Eigen::Matrix3f B = U * V.transpose();

cout << B.transpose() << endl <<endl;
cout << B.inverse() << endl<<endl;

线性方程组的最小二乘解

如果一个线性方程组是超定的(overdeterminated，未知数个数>方程数)，这时候常规方法无解，就需要用最小二乘拟合最优结果。最精确的解法是SVD分解。SVD也有多种解法，官方推荐的是BDCSVD方法。

如下超定方程组：

最小二乘求解代码如下：

1
2
3
4
5
6
7
8
9
10

MatrixXf A(3, 2);
Vector3f b;
Vector2f x;
A << 1,1, 1,2, 1,3 ;
b << 0,4,10;
x = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);
cout << x << endl;

Vector3f error = (A*x - b).cwiseAbs();
double mean_error = error.mean();

将得到的解带回方程会发现其并不是严格成立的，有时可能还会相差较大。这是因为对于超定方程，采用最小二乘法得出的解并不一定对每一个方程都严格成立，其确保的是当前解在所有方程上的总误差最小。得到解以后我们可以反算出其解的整体精度

对于小矩阵，逆矩阵和行列式随便算，如果是大矩阵，计算量会很庞大。确定你是否真的需要逆矩阵，因为很多时候求逆矩阵都是为了求解Ax = b问题，所以最好使用上面介绍的分解方法代替.

参考：
Eigen学习与使用笔记
Eigen学习与使用笔记2
Eigen官方文档-AngleAxis
矩阵的特征分解与奇异值分解

shared_ptr 内部包含两个指针，一个指向对象，另一个指向控制块，控制块中包含一个引用计数和其它一些数据。由于这个控制块需要在多个shared_ptr之间共享，即多个shared_ptr可以共享同一块内存，所以它也是存在于 heap 中的。

如果没有共享所有权的必要，就不必使用shared_ptr，而非unique_ptr

由于支持共享所有权,shared_ptr支持拷贝和赋值运算, 也支持移动。如果对象在创建的时候没有使用共享指针存储的话，之后也不能用共享指针管理这个对象了。

避免使用原生指针来创建shared_ptr指针

shared_ptr销毁对象的情况:

1. 最后一个智能指针离开作用域
2. 用其他的shared_ptr给一个shared_ptr初始化
3. 最后一个智能指针调用 reset

shared_ptr的缺点:

• 内存占用是原生指针的两倍，因为除了要管理一个原生指针外，还要维护一个引用计数

• 使用了性能低的原子操作：考虑到线程安全问题，引用计数的增减必须是原子操作。 而原子操作一般情况下都比非原子操作慢

• 两个shared_ptr可能出现循环引用，永远不能释放指向的对象

线程安全性

shared_ptr有两个成员，指向对象的指针ptr和管理引用计数的指针ref_count。引用计数本身是原子操作，是线程安全的，但 shared_ptr的赋值操作由复制对象指针和修改使用计数两个操作复合而成, 因此仍不是线程安全的。如果要从多个线程读写同一个shared_ptr 对象，还是需要加锁。

陈硕专门写了这篇文章分析这个问题，
也可以看我自己的这篇文章，子线程里能写shared_ptr指向的对象，回到主线程就变了。

尽量使用 make_shared()

为了节省一次内存分配，原来shared_ptr<Foo> x(new Foo); 需要为 Foo 和 ref_count 各分配一次内存，现在用make_shared()的话，可以一次分配一块足够大的内存，供 Foo 和 ref_count 对象容身。

常见用法

1
2
3
4
5
6
7
8

    int num = 12;
    int* p = new int(num);
//    shared_ptr<int> p1 = &num;      // error
    shared_ptr<int> p2 = boost::make_shared<int>(num);
    shared_ptr<Foo> p3(p);

//    cout << *p2 <<endl;
//    cout << *p3 <<endl;

p1的用法是错的，p2和p3正确，但是不要同时使用，改用p3(p2)即可

1
2
3

boost::shared_ptr<Foo> a;
cout<< a.use_count()<<endl;
a->out();

执行a->out()会报错，原因是a没有指向对象，应该这样定义：boost::shared_ptr<Foo> a(new Foo());

再看这样的代码:

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

boost::shared_ptr<Foo> a(new Foo());            // 让a指向对象
cout<< a.use_count()<<endl;                        // 1

boost::shared_ptr<Foo> b = a;                   // 另一个指针也指向同一个对象
cout<< a.use_count()<<endl;                     // 2
cout<< b.use_count()<<endl;                     // 2

a.reset();                      // 不执行析构函数，实际执行 delete a; a = NULL;
a->out();                    // 报错
b->out();                   // 正常
cout<< a.use_count()<<endl;                     // 0
cout<< b.use_count()<<endl;                     // 1
b.reset();                  // 执行析构函数
cout<<"end"<<endl;

只有对象的引用计数为0的时候，才执行类的析构和free其内存:

1
2
3
4
5
6
7
8
9

boost::shared_ptr<Foo> a(new Foo());
boost::shared_ptr<Foo> b = a;

cout<<a<<endl;
cout<<b<<endl;

a.reset(new Foo());         // a重新初始化，指向另一个地址
cout<<a<<endl;
cout<<b<<endl;

运行结果：

1
2
3
4
5

0x99fc20
0x99fc20

0x9a0c70
0x99fc20

不要把一个原生指针给多个shared_ptr管理

1
2
3

int* ptr = new int;
boost::shared_ptr<int> p1(ptr);
boost::shared_ptr<int> p2(ptr); 

可以在标准容器里存储boost::shared_ptr，但不能存储std::auto_ptr和boost::scoped_ptr，后两者不能共享对象所有权．

1
2
3

std::vector<boost::shared_ptr<int> > v; 
v.push_back(boost::shared_ptr<int>(new int(1))); 
v.push_back(boost::shared_ptr<int>(new int(2)));

自定义删除器

默认情况下，shared_ptr调用delete()函数进行资源释放，即delete p;。但是如果shared_ptr指向一个数组而不是一个简单的指针，应该调用delete[] p，此时可以将一个回调传递给shared_ptr的构造函数来定制删除器。

主要是利用了构造函数template<class Y, class D> shared_ptr(Y * p, D d);，第一个参数是要被管理的指针, 与其它形式的构造函数一致; 第二个参数称为删除器, 他是一个接受Y*的可调用物, d(p)的行为应类似与delete p, 而且不应该抛出异常。有了删除器, 我们就可以管理一些更复杂的资源, 比如数据库连接, socket连接。

其实有很多种用法，只列举常用的普通函数法

1
2
3
4
5
6
7
8
9

Derived *d = new Derived[5];
boost::shared_ptr<Derived> p1(d, deleter);

// deleter函数定义
void deleter(Derived* d)
{
    delete[] d;
    cout<<"delete"<<endl;
}

参考：官方说明
boost智能指针之shared_ptr