STL 标准库提供了 4 种关联式容器，分别为 map、set、multimap、multiset

set

set的元素有序不重复，而且能根据元素的值自动进行排序。set中的键值不能直接修改，只能先删除再插入。底层采用红黑树。

set不支持随机访问，只能使用迭代器去访问。由于set放入一个元素就会调整这个元素的位置，把它放到合适的位置，所以set中只有一个insert插入操作。

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

set<int> s;
s.insert(5);
s.insert(2);
s.insert(3);
s.insert(1);
s.insert(2);
s.insert(4);
cout<<"set 的 size："<<  s.size() <<endl;  // 5
cout<<"set 中的第一个元素是 ："<<  *s.begin()<<endl;  // 1
cout<<"set 中的最后一个元素是:"<<  *s.end()<<endl;   // 5
set<int>::iterator it;
for(it=s.begin(); it!=s.end(); it++)
{
    cout<< *it <<endl;
}    // 1  2  3  4  5
//    s.clear();
s.erase(++s.begin());
cout << "       after erase begin"<<endl;
for(it=s.begin(); it!=s.end(); it++)
{
    cout<< *it <<endl;
}	 // 1  3  4  5
cout<<"lower bound: "<<*s.lower_bound(5)<<endl;  // 5
cout<<"upper bound: "<<*s.upper_bound(5)<<endl;  // 4
if(s.empty())
    cout << "set is empty !" <<endl;

multiset底层也是红黑树，但允许有重复数据

map 和 unordered_map

map适合存储一个数据字典，并要求方便地根据key找value。Map节点有一个Key和Value两个元素，Key不重复，Value可以重复。map可以通过key改变value的值

底层也是红黑树，所有元素都是有序的，红黑树的每一个节点都代表着map的一个元素，因此它的插入删除查找的时间复杂度为O(logN)

map支持随机访问(at函数和[])，这是set没有的。

1
2
3
4
5
6
7
8
9

map<int, string> m;
m.insert(pair<int, string>(1, "a"));
m.insert(pair<int, string>(2, "b"));
m.insert(pair<int, string>(3, "c"));
m.insert(pair<int, string>(4, "d"));
m.insert(pair<int, string>(5, "e"));

cout << m.upper_bound(3)->second <<endl;   // 大于
cout << m.lower_bound(3)->second <<endl;   // 不小于

1
2

d
c

缺点： 空间占用率高，因为map内部实现了红黑树，虽然提高了运行效率，但是因为每一个节点都需要额外保存父节点，孩子节点以及红/黑性质，使得每一个节点都占用大量的空间

unordered_map内部实现了一个哈希表，因此其元素的排列顺序是杂乱的，无序的，但查找速度非常的快。 缺点：哈希表的建立比较耗费时间。

Cartographer是基于图优化的SLAM，采用误差累计少，计算成本低的Scan to Map匹配方式，而不是造成误差快速累积和计算成本高的scan to scan匹配。整个算法分为局部SLAM和全局SLAM，两部分都对雷达观测的位姿进行了优化。

虽然cartographer适用于2D和3D两种情况，但3D情况下，cartographer用的不如LOAM多

订阅的话题

节点cartographer_node订阅的话题是：scan, tf, tf_static。 cartographer_node的launch需要增加remap，因为它接受的话题名称和我们现有的话题可能不一致：

1
2
3

<remap from="scan"  to="/scan" />
<remap from="odom"  to="/xqserial_server/Odom" />
<remap from="imu"   to="/xqserial_server/IMU" />

cartographer_node 发布的话题

• scan_matched_points2, 类型sensor_msgs/PointCloud2, scan-to-submap匹配的2D点云数据，这个云可以根据配置进行滤波和投影。是scan话题的大部分，不会包括行走的人的轮廓

• submap_list, 类型cartographer_ros_msgs/SubmapList，所有轨迹的所有submaps的列表，成员：

  1 2 3 4 5 6 7 8

  std_msgs/Header header cartographer_ros_msgs/SubmapEntry[] submap # 其中submap的类型如下 int32 trajectory_id int32 submap_index int32 submap_version geometry_msgs/Pose pose

  话题包括一个子图的ID list(包括轨迹ID和子图index)，以及子图的全局位姿。

发布话题的源码流程是 Node::PublishSubmapList —— MapBuilderBridge::GetSubmapList() —— PoseGraph2D::GetAllSubmapPoses() —— PoseGraph2D::GetSubmapDataUnderLock —— PoseGraph2D::ComputeLocalToGlobalTransform

• trajectory_node_list，类型为visualization_msgs/MarkerArray，即轨迹路径node列表，代码是

1
2
3
4
5
6
7
8
9
10

void Node::PublishTrajectoryNodeList(
    const ::ros::WallTimerEvent& unused_timer_event)
{
  if (trajectory_node_list_publisher_.getNumSubscribers() > 0)
  {
    carto::common::MutexLocker lock(&mutex_);
    trajectory_node_list_publisher_.publish(
        map_builder_bridge_.GetTrajectoryNodeList()  );
  }
}

• constraint_list: 类型为visualization_msgs/MarkerArray，约束列表

• landmark_poses_list: 类型为visualization_msgs/MarkerArray，路标点位姿列表

节点 cartographer_occupancy_grid_node 发布话题 map(栅格地图)， 订阅submap_list [cartographer_ros_msgs/SubmapList]。 cartographer_occupancy_grid_node 节点将子图转换为ROS的地图格式

服务

• submap_query (cartographer_ros_msgs/SubmapQuery)： 查询submap的服务，输入的是trajectory_id 和 submap_index
  
  

start_trajectory

类型cartographer_ros_msgs/StartTrajectory，通过将其传感器主题和轨迹选项指定为二进制编码的原型来启动另一个轨迹。返回指定的轨迹ID。

StartTrajectory.srv:

1
2
3
4
5
6
7
8

string   configuration_directory
string   configuration_basename
bool   use_initial_pose
geometry_msgs/Pose   initial_pose
int32   relative_to_trajectory_id
---
cartographer_ros_msgs/StatusResponse   status
int32   trajectory_id

命令稍微长了点:

1
2
3
4
5
6
7

rosservice call /start_trajectory "configuration_directory: '/home/user/carto_ws/src/cartographer_ros/cartographer_ros/configuration_files'
configuration_basename: 'localization.lua'
use_initial_pose: true
initial_pose:
  position: {x: 5.211, y: 5.617, z: 0.0}
  orientation: {x: 0.0, y: 0.0, z: 0.689, w: 0.725}
relative_to_trajectory_id: 0"

出错的地方可能会在relative_to_trajectory_id：

1
2
3
4
5
6
7

status:
  code: 5
  message: "Trajectory 6 doesn't exist."

status:
  code: 3
  message: "Trajectory 5 is in 'DELETED' state."

加载 frozen state 意味着不插入no pose graph constraints. 一条frozen trajectory和它的子图不受 optimization影响

finish_trajectory

类型cartographer_ros_msgs/FinishTrajectory: Finish 一个给定ID的轨迹，方式是运行最终的图优化。

1
2
3

int32 trajectory_id
---
cartographer_ros_msgs/StatusResponse status

命令rosservice call finish_trajectory "trajectory_id: 5"，得到这样的结果:

1
2
3
4

status:
  code: 3
  message: "Topics are already used by another trajectory."
trajectory_id: 0

trajectory_query

• get_trajectory_states (cartographer_ros_msgs/GetTrajectoryStates): 返回所有轨迹的ID和状态， 可以用于从单独的节点观察Cartographer的状态。

  1 2 3 4

  --- cartographer_ros_msgs/StatusResponse status cartographer_ros_msgs/TrajectoryStates trajectory_states

命令rosservice call /get_trajectory_states "{}"，比如得到这样的结果：

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

status:
  code: 0
  message: ''
trajectory_states:
  header:
    seq: 0
    stamp:
      secs: 1612232712
      nsecs:  41338148
    frame_id: ''
  trajectory_id: [0, 1, 2, 3, 4, 5]
  trajectory_state: [2, 2, 2, 2, 2, 0]

trajectory_state的枚举值也就是 轨迹的四种状态对应: ACTIVE, FINISHED, FROZEN, DELETED

• write_state (cartographer_ros_msgs/WriteState): 将当前内部状态写入磁盘到文件名。用于保存pbstream文件，此文件可用作assets_writer_main的输入，以生成概率网格，X-Rays或PLY文件等资源

read_metrics

类型(cartographer_ros_msgs/ReadMetrics)， 返回Cartographer的所有内部指标的最新值。运行时度量标准的集合(collection of runtime metrics)是可选的，必须使用节点中的-collect_metrics命令行标志激活:

1
2
3
4
5
6
7
8
9

<node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename map.lua
          -collect_metrics true"
      output="screen">
  <remap from="scan" to="/scan_rectified" />
  <remap from="odom" to="/odom" />
</node>

如果没有加-collect_metrics标志，rosservice call /read_metrics "{}"的结果:

1
2
3
4
5
6
7

status:
  code: 14
  message: "Collection of runtime metrics is not activated."
metric_families: []
timestamp:
  secs: 1612333336
  nsecs: 359502516

需要的tf变换

必须提供从所有传入传感器数据帧到配置的tracking_frame和published_frame的转换。通常，这些是由robot_state_publisher或static_transform_publisher定期发布的。

发布的tf变换

提供map_frame和 published_frame之间的转换。如果在Lua中启用了provide_odom_frame，则将提供配置的odom_frame和published_frame之间的连续（不受循环闭包影响）转换。

其他节点

• cartographer_rosbag_validate

可以检测bag中的错误，使用：

1

rosrun cartographer_ros cartographer_rosbag_validate -bag_filename <bag filename>

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

E0722 22:11:01.138546  3442 rosbag_validate_main.cc:389] IMU data (frame_id: "imu") has a large gap, largest is 0.064731 s, recommended is [0.0005, 0.005]s with no jitter.
I0722 22:11:01.139314  3442 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/IMU_data" (frame_id: "imu"):
Count: 27679  Min: 0.000144  Max: 0.064731  Mean: 0.005027
[0.000144, 0.006602)    					    Count: 27445 (99.154594%)    Total: 27445 (99.154594%)
[0.006602, 0.013061)                            Count: 181 (0.653925%)  Total: 27626 (99.808517%)
[0.013061, 0.019520)                            Count: 42 (0.151740%)   Total: 27668 (99.960258%)
[0.019520, 0.025979)                            Count: 7 (0.025290%)    Total: 27675 (99.985550%)
[0.025979, 0.032437)                            Count: 3 (0.010839%)    Total: 27678 (99.996384%)
[0.032437, 0.038896)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.038896, 0.045355)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.045355, 0.051814)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.051814, 0.058272)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.058272, 0.064731]                            Count: 1 (0.003613%)    Total: 27679 (100.000000%)

• cartographer_pbstream_map_publisher

一个简单的节点，它从序列化的Cartographer状态（pbstream格式）创建静态占用网格。如果实时更新不重要，它是占用网格节点的有效替代方案。 发布话题：map (nav_msgs/OccupancyGrid)，只发布一次

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

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)

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