S-MSCKF代码阅读

阅读学习代码

第一部分：相关库函数介绍

1. pluginlib理解与示例

• pluginlib是一个C++库，可以实现为一个ROS包动态的加载和卸载插件。这里的插件通常是一些功能类，且以运行时可动态加载的库（如共享对象，动态链接库）的形式存在。借助pluginlib的帮助，用户不必关心自己的应用程序该如何链接包含自己想使用的的class的库（如定义class的头文件在哪里，如何定义的），因为pluginlib会在你调用时自动打开你需要的插件库（Note：需要提前将插件库注册到pluginlib）。使用插件来扩展或者修改应用程序的功能非常方便，不用改动源码重新编译应用程序，通过插件的动态加载即可完成功能的扩展和修改。

• pluginlib利用了C++多态的特性，不同的插件只要使用统一的接口，便可以替换使用。这样用户通过调用在插件中实现的统一的接口函数，不需要更改程序，也不需要重新编译，更换插件即可实现功能修正。

  利用pluginlib编写插件的方法大致包括如下四步：

  1. 创建插件基类，定义统一接口（如果为现有接口编写插件，则跳过该步）
  2. 编写插件类，继承插件基类，实现统一接口
  3. 导出插件，并编译为动态库
  4. 将插件加入ROS系统，使其可识别和管理

原文链接：pluginlib
参考博客： pluginlib示例

2. nodelet

1 ROS的数据通信在graph结构中以topic,service和param的方式传输数据，天生的数据交互存在一定的延时和阻塞。Nodelet 包就是改善这一状况设计的， 使得多个算法运行在同一个过程中，并且算法间数据传输无需拷贝就可实现。 详见http://wiki.ros.org/nodelet。 简单的讲就是可以将以前启动的多个node捆绑在一起manager，使得同一个manager里面的topic的数据传输更快。即同一个manager过程的nodelet数据传输zero copy 。
2 nodelet的manager可以加载多个的nodelets，nodelets间数据传输zero copy，有效避免数据copy和网络传输代价
3 支持pulgin的方式动态加载，基类nodelet::Nodelet， 任何nodelet继承自它可以使用plugin的方式动态加载。
4 Nodelets旨在提供一种在单机器单进程运行多个算法而不会在进程中传递消息时产生复制成本的方法。roscpp具有在同一节点内的发布和订阅调用之间进行零拷贝指针传递的优化,因此实际相当于不同nodelet在统一node运行,但写起来仍是多个node的写法.

参考博客：nodelet使用、nodelet原理

nodelet和node很大程度上保持一致, 对外来说, nodelet几乎可以视为node。
1、创建package：代码中package名字为msckf_vio
2、在include, src中创建一个类, 该类继承nodelet::Nodelet, 最好在类的外面加一个namespace, 可以以package名字做namespace. 类中有一个onInit()函数, 相当于main()函数.

#include 
#include 
#include 

namespace msckf_vio {
class MsckfVioNodelet : public nodelet::Nodelet {
public:
  MsckfVioNodelet() { return; }
  ~MsckfVioNodelet() { return; }

private:
  virtual void onInit();
  MsckfVioPtr msckf_vio_ptr;
};
} // end namespace msckf_vio

3、在cpp中实现onInit()函数, 编写自己的代码. nodelet允许将类动态加载到同一个节点，进而实现零拷贝通信，并且nodelet提供了简单的单独命名空间，使得尽管nodelet在同一个进程中，它使用时仍然像一个独立的节点，因此，它在运行时使用pluginlib是动态可加载的。因此，cpp中还需要加一行，导出为插件。

PLUGINLIB_EXPORT_CLASS(example_pkg::SampleNodelet, nodelet::Nodelet)

#include 

namespace msckf_vio {
void MsckfVioNodelet::onInit() {
  msckf_vio_ptr.reset(new MsckfVio(getPrivateNodeHandle()));
  if (!msckf_vio_ptr->initialize()) {
    ROS_ERROR("Cannot initialize MSCKF VIO...");
    return;
  }
  return;
}

PLUGINLIB_EXPORT_CLASS(msckf_vio::MsckfVioNodelet,
    nodelet::Nodelet);

} // end namespace msckf_vio

• nodelet相当于插件基类
• MsckfVioNodelet为插件类
• 然后导出插件类，要实现class可动态加载，必须要将其标记为可导出的class。通过特定的宏PLUGINLIB_EXPORT_CLASS，最后一行就是为了实现这个。
• 编译为动态链接库，下面第四步
  4、CMakelist中添加library. 这里我们发现, nodelet的pkg中没有main()没有可执行程序, 而是生成一个lib, 让ros调用.

# Msckf Vio
add_library(msckf_vio
  src/msckf_vio.cpp
  src/utils.cpp
)
add_dependencies(msckf_vio
  ${${PROJECT_NAME}_EXPORTED_TARGETS}
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(msckf_vio
  ${catkin_LIBRARIES}
  ${SUITESPARSE_LIBRARIES}
)

# Msckf Vio nodelet
add_library(msckf_vio_nodelet
  src/msckf_vio_nodelet.cpp
)
add_dependencies(msckf_vio_nodelet
  ${${PROJECT_NAME}_EXPORTED_TARGETS}
  ${catkin_EXPORTED_TARGETS}
)
target_link_libraries(msckf_vio_nodelet
  msckf_vio
  ${catkin_LIBRARIES}
)

• 将插件加入ROS系统，使其可识别和管理
  5、创建插件描述文件：nodelet_plugins.xml是nodelet特有的, 文件名可以自己定, 最好用这个名字. xml文件里面指定nodelet的lib的路径, 以及nodelet类的名字.

<library path="lib/libmsckf_vio_nodelet">
  <class name="msckf_vio/MsckfVioNodelet"
         type="msckf_vio::MsckfVioNodelet"
         base_class_type="nodelet::Nodelet">
    <description>
      Multi-State contraint Kalman filter for vision-
      aided inertial navigation with observability constain.
    </description>
  </class>
</library>

<library path="lib/libimage_processor_nodelet">
  <class name="msckf_vio/ImageProcessorNodelet"
         type="msckf_vio::ImageProcessorNodelet"
         base_class_type="nodelet::Nodelet">
    <description>
      Detect and track features in image sequence.
    </description>
  </class>
</library>

6、注册插件到ROS系统：package.xml中增加export项, 把上一个xml文件的位置配置进去.

<export>
    <nodelet plugin="${prefix}/nodelets.xml"/>
</export>

注意：如果插件类与基类不在同一package，为了使插件的export生效，还必须添加对插件基类所在package的依赖。

<depend>nodelet</depend>
有的是：
 <build_depend>my_plugin_test</build_depend>
 <run_depend>my_plugin_test</run_depend>

7、在catkin_make执行成功之后，source develop/setup.bash，然后运行如下命令如果能正确看到输出polygon_plugin.xml则ok。

rospack plugins --attrib=plugin nodelet

正常的插件就可以使用了，在节点cpp里面包含头文件创建指针就好了，我们这里使用nodelet，用起来很方便
8、以launch文件运行，编写launch文件, 如下. 该launch打开两个nodelet, 更改第二个nodelet的name和args.
虽然说nodelet是为了方便消息传输，但其实这个代码中只包含了一个nodelet，没发挥这个快速传输的效果，调用就相当与node的调用，没什么区别，如果是多个nodelet就可以快速通信了。类似这样

<launch>
  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="SampleNodelet" args="load example_pkg/SampleNodelet standalone_nodelet" output="screen">
  <node pkg="nodelet" type="nodelet" name="SampleNodelet2" args="load example_pkg/SampleNodelet2 standalone_nodelet" output="screen">
  </node>
</launch>

解释一下：
第一行：创建mananger ，名字为standalone_nodelet
第二行：将example_pkg/SampleNodelet加入到manager standalone_nodelet
第三行：将另一个nodelet加入manager，他们就是一个进程了，共享内存，这两个nodelet消息传输很快

• nodelet load pkg/Type manager //向manager中loader nodelet
• nodelet standalone pkg/Type //程序复用，相当启动一个普通node
• nodelet unload name manager //从manager移除nodelet
• nodelet manager //创建mananger

命令行调用:

#启动nodelet管理器
roscore
rosrun nodelet nodelet manager __name:=nodelet_manager
#启动自己的nodelet
rosrun nodelet nodelet load nodelet_tutorial_math/Plus nodelet_manager __name:=nodelet1 nodelet1/in:=foo _value:=1.1

<!-- Msckf Vio Nodelet  -->
  <group ns="$(arg robot)">
    <node pkg="nodelet" type="nodelet" name="vio"
      args='standalone msckf_vio/MsckfVioNodelet'
      output="screen">

      <!-- Calibration parameters -->
      <rosparam command="load" file="$(arg calibration_file)"/>

      <param name="publish_tf" value="true"/>
      ........
      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>

      <remap from="~imu" to="/imu0"/>
      <remap from="~features" to="image_processor/features"/>

    </node>
  </group>

3. launch文件

launch常用配置

<launch>                <!--根标签-->
<node>                  <!--需要启动的node及其参数-->
<include>               <!--包含其他launch-->
<machine>               <!--指定运行的机器-->
<env-loader>            <!--设置环境变量-->
<param>                 <!--定义参数到参数服务器-->
<rosparam>              <!--加载yaml文件中的参数到参数服务器-->
<arg>                   <!--定义变量-->
<remap>                 <!--设定 topic 映射-->
<group>                 <!--设定分组-->
</launch>               <!--根标签-->

参考: launch使用

启动文件的核心是启动ROS节点，采用标签定义，语法如下：

<node name="node-name" pkg="package-name" type="executable-name"/>

从上边的定义规则可以看出，在启动文件中启动一个节点需要三个属性：name、pkg和type。其中name属性用来定义节点运行的名称，将覆盖节点中ros::init()定义的节点名称；pkg属性定义节点所在的功能包名称，type属性定义节点的可执行文件名称，这两个属性等同于在终端中使用rosrun命令执行节点时的输入参数。这是三个最常用的属性，在某些情况下，我们还有可能用到以下属性：

属性属性作用output="screen"终端输出转储在当前的控制台上，而不是在日志文件中respawn="true"当roslaunch启动完所有该启动的节点之后，会监测每一个节点，保证它们正常的运行状态。对于任意节点，当它终止时，roslaunch 会将该节点重启required="true"当被此属性标记的节点终止时，roslaunch会将其他的节点一并终止。注意此属性不可以与respawn="true"一起描述同一个节点ns = "NAME_SPACE"这个属性可以让你在自定义的命名空间里运行节点args = "arguments"节点需要的输入参数

二、参数设置

为了方便设置和修改，launch文件支持参数设置的功能，类似于编程语言中的变量声明。关于参数设置的标签元素有两个：、，一个代表parameter，另一个代表argument。这两个标签元素翻译成中文都是“参数”的意思，但是这两个“参数”的意义是完全不同的。
2.1 param
parameter是ROS系统运行中的参数，存储在参数服务器中。在launch文件中可以通过param元素加载parameter。launch文件执行后，parameter就加载到ROS的参数服务器上了。
比如现在在参数服务器中添加一个名为demo_param，值为666的参数

<param name="demo_param" type="int" value="666"/>

运行launch文件后，demo_param这个parameter的值就设置为666，并且加载到ROS参数服务器上了。但是在很多复杂的系统中，参数的数量很多，如果这样一个一个的设置会非常麻烦，ROS也为我们提供了另外一种类似的参数加载方式——rosparam：

<rosparam file="$(find 2dnav_pr2)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />

rosparam可以帮助我们将一个yaml格式文件中的参数全部加载到ROS参数服务器中，需要设置command属性为“load”，还可以选择设置命名空间“ns”。
2.2 arg

arg标签用来在launch文件中定义参数，arg和param在ROS里有根本性的区别，就像局部变量和全局变量的区别一样。arg不储存在参数服务器中，不能提供给节点使用，只能在launch文件中使用。param则是储存在参数服务器中，可以被节点使用。

<arg name="demo"/>

像上面这样，就简单地声明了一个参数，名叫demo，但是声明不等于定义，我们需要给他赋值，在赋值之后参数才能够发挥作用。

<arg name="demo1" value="666"/>
<arg name="demo2" default="666"/>

以上是两种简单的赋值方法，两者的区别是使用后者赋值的参数可以在命令行中像下面这样被修改，前者则不行。

roslaunch demo demo.launch demo:2=6666

launch文件中需要使用到argarg-name时，可以使用如下方式调用：

<arg name="arg-name" value="666"/>
<param name="foo" value="$(arg arg-name)" />
<node name="node" pkg="package" type="type "args="$(arg arg-name)" />

当$(arg arg_name)出现在launch文件任意位置时，将会自动替代为所给参数的值。

三、重映射机制

简单来说就是取别名,比如turtlebot的键盘控制节点，发布的速度控制指令话题可能是/turtlebot/cmd_vel，但是我们自己的机器人订阅的速度控制话题是/cmd_vel，这个时候使用remap就可以轻松解决问题，将/turtlebot /cmd_vel重映射为/cmd_vel，我们的机器人就可以接收到速度控制指令了：

<remap from="/turtlebot/cmd_vel" to="/cmd_vel"/>

四、嵌套复用
在复杂的系统当中，launch文件往往有很多，这些launch文件之间也会存在依赖关系。如果需要直接复用一个已有launch文件中的内容，可以使用标签包含其他launch文件，这和C语言中的include几乎是一样的。

<include file="$(find demo)/launch/demo.launch" ns="demo_namespace"/>

属性作用file ="$(find pkg-name)/path/filename.xml"指明我们想要包含进来的文件ns="NAME_SPACE"相对NAME_SPACE命名空间导入文件

五、拓展说明

使用 roslaunch 命令 和 使用 rosrun 命令 单独运行每个节点之间的重要区别

默认情况下，roslaunch 命令 从启动节点开始，标准输出信息会重定向到一个日志文件中，而不会像 rosrun 命令那样，将 log 信息显示在终端(console)上。日志文件所在路径： ∼/.ros/log/run_id/node_name-number-stdout.log

Q： 如何将标准输出信息显示在终端(console)上？

A： 在 node 元素中使用 output 属性：output=”screen”。

扩展： node 元素的 output 属性只能影响这个节点自己。除了 output 属性，我们可以使用 roslaunch命令行工具的 –screen 命令行选项强制性的在终端的窗口中显示所有节点的输出信息。

$ roslaunch --screen package-name launch-file-name

4. Eigen内存分配器

对eigen中的固定大小的类使用STL容器的时候，如果直接使用会出错，所谓固定大小（fixed-size）的类是指在编译过程中就已经分配好内存空间的类，为了提高运算速度，对于SSE或者AltiVec指令集，向量化必须要求向量是以16字节即128bit对齐的方式分配内存空间，所以针对这个问题，容器需要使用eigen自己定义的内存分配器，即aligned_allocator。

#include 

vector<Eigen::Matrix4d>;
std::map<int, Eigen::Vector4f>

std::vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d>>
std::map<int, Eigen::Vector4f, Eigen::aligned_allocator<std::pair<const int, Eigen::Vector4f>>
或者

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix2d)
std::vector<Eigen::Vector2d>

EIGEN_MAKE_ALIGNED_OPERATOR_NEW:
Eigen内存对齐问题

5. std::map的第三个参数

map的其中一个构造函数有第三个参数，可以直接定义map的key值得排序规则

默认为std::less，即按“<”运算符进行排序
map<string, int> mapWord = { { "father", 1 },{ "mother", 4 },{ "daughter", 5 } };
等价于：
map<string, int, std::less<string>> mapWord2 = { { "father", 1 },{ "mother", 4 },{ "daughter", 5 } };

map<string, int, std::greater<string>> mapWord2 ;
bool compFunc(const string& a, const string& b)
{
　　return a.compare(b) > 0;
}
map <string, int, decltype(compFunc)*> mapWord3;  //注意*号的存在。比较操作类型必须为函数指针类型

auto fc = [](const string& str1, const string& str2) {return str1.compare(str2) > 0; };
map <string, int, decltype(fc)*> mapWord4;  //同样要使用使用函数指针

6. image_transport

ROS通过image_transport传递通用图像:
参考: image_transport翻译

// Use the image_transport classes instead.
#include 
#include 

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  // ...
}

ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("in_image_base_topic", 1, imageCallback);
image_transport::Publisher pub = it.advertise("out_image_base_topic", 1);
/*******
image_transport 默认的传输，通过ROS传输sensor_msgs/Image
compressed_image_transport JPEG or PNG 压缩
theora_image_transport 视频流传输
*****/

7. message_filter

消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。
举个例子，比如时间同步器，它接收来自多个源的不同类型的消息，并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们，也就是起到了一个消息同步输出的效果。

message_filters::Subscriber<sensor_msgs::Image> cam0_img_sub;
message_filters::Subscriber<sensor_msgs::Image> cam1_img_sub;
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> stereo_sub;
cam0_img_sub.subscribe(nh, "cam0_image", 10);
cam1_img_sub.subscribe(nh, "cam1_image", 10);
stereo_sub.connectInput(cam0_img_sub, cam1_img_sub);
stereo_sub.registerCallback(&ImageProcessor::stereoCallback, this);

参考博客: message_filter用法

第二部分:MSCKF代码-image_processor

状态

struct StateServer {
      IMUState imu_state;
      CamStateServer cam_states;

      // State covariance matrix
      Eigen::MatrixXd state_cov;
      Eigen::Matrix<double, 12, 12> continuous_noise_cov;
    };

每帧特征:
std::map<int, std::vector<FeatureMetaData> > GridFeatures;   //key为第几个grid,vector为每个格里的特征集合
struct FeatureMetaData {
    FeatureIDType id;
    float response;
    int lifetime;
    cv::Point2f cam0_point;
    cv::Point2f cam1_point;
};

算法

image_processor算法介绍
这部分代码也比较简单, Fast特征点加光流,双目相机

第三部分:基础知识

1. 四元数的表示

kinematics.pdf
常用的四元数类型: Hamilton和JPL

1)Hamilton和JPL的差异

• 元素顺序不同:
  
  左侧是H,右侧是JPL
  H是更习惯俄一种表示方式,JPL类似单应向量的表示,当处理3D空间的几何问题时,使在单应向量上的操作和四元数的操作更一致.
  两种方法计算得到的左乘矩阵不同
  Hamiltion:
  $$q=q_w+{\text{q}}_v$$
  
  JPL:
  $$q={\text{q}}_v+q_w=q_1\text{i}+q_2\text{j}+q_3\text{k}+q_4$$
  

• 其次,四元数定义的乘法规则不同, Hamilton是右手系, JPL是左手系, 也就是说, 给定一个旋转轴$u$, 一个按右手系规则旋转$\theta$,一个按左手系规则旋转$\theta$.所以:
  $$q_{right}^{\star }=q_{left}$$
  所以两种方法对应的旋转矩阵是一个转置的关系:
  
  q4=q0=qw

• active表示坐标系不动,目标旋转; passive表示目标不动,坐标系旋转
• 在passive中,两种方法表示的旋转方向相反
  
  passive表示更类似于一个旋转变换,可以对应一个Direct cosine matrix
  $c_{ij}$表示源坐标轴与目标坐标周旋转角的cos值.

2)扰动,积分和微分

• SO(3)上的加减操作
  
  
  其中:
  
  

• 四元数与旋转矩阵的对应关系
  JPL表示:
  
  Halmtion对应的旋转矩阵是上述矩阵的转置
  当旋转的角度很小时,JPL:
  
  如果是Halmition,就把$\delta \theta$变成$-\delta \theta$
  

• 雅可比计算(Halmition)

1. 关于向量的Jacobian
   
   2)关于四元数的雅可比
   q = [w v], v是向量
   

2. SO3的右雅可比
   
   
   右雅可比有如下性质
   
   而
   

3. 关于旋转向量的雅可比

• 扰动
  若绕静坐标系（世界坐标系）旋转，则左乘，也是变换矩阵坐标矩阵；若是绕动坐标系旋转（自身建立一个坐标系），则右乘，也就是坐标矩阵变换矩阵。即，左乘是相对于坐标值所在的坐标系（世界坐标系）下的三个坐标轴进行旋转变换。而右乘则是以当前点为旋转中心，进行旋转变换。

1. 局部扰动:右乘扰动
   
   2)全局扰动:左乘扰动
   

• 四元数的微分
  全局扰动
  
  
  上面使用的JPL表示,也可使用Halmition,$q=q_{\star }$, $\delta \theta =-\delta \theta$
  局部扰动:
  
  推导使用Halmition,也可使用JPL

• 四元数积分
  零阶四元数积分:
  $\omega (t)=\omega$在$\delta t$内是常值
  即
  
  泰勒展开推到得到:
  
  根据:
  
  得到:
  
  一阶积分:
  
  推导结果:
  
  MSCKF统一使用JPL左手系的定义方式, 求解关于扰动的雅可比时,使用左乘的定义方式.