IMU-camera联合标定

概览.jpg

Kalibr

kalibr官方教程

github.com

ROS :: Message-filter

wiki.ros.org

setup

安装

1
sudo apt-get install ros-noetic-message-filters

设置环境变量

1
source /opt/ros/noetic/setup.bash

或者添加到.bashrc中

1
2
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc

喜闻乐见的报错环节🙃

CMake Error at /opt/ros/noetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package): Could not find a package configuration file provided by “message_filter” with any of the following names:

​ message_filterConfig.cmake
​ message_filter-config.cmake

Add the installation prefix of “message_filter” to CMAKE_PREFIX_PATH or set “message_filter_DIR” to a directory containing one of the above files. If “message_filter” provides a separate development package or SDK, be sure it has been installed. Call Stack (most recent call first): msg_filter/CMakeLists.txt:10 (find_package)

安装完包之后可以尝试刷新环境

1
source /opt/ros/noetic/setup.bash

检查是否正确安装了包

1
2
3
4
rospack find mseeage_filter
# [rospack] Error: package 'message_filter' not found
rospack find mseeage_filters
# /opt/ros/noetic/share/message_filters

发现是因为我把filters写成了filter 😆

example

msg_filter(subscriber)

1
message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "image", 1)；

从创建订阅实体image_sub，话题为”image”，消息类型为<sensor_msgs::Image>

ApproximateTime policy

1
typedef sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::WrenchStamped> MySyncPolicy;

​ 这一行是定义了一个同步策略的类型，叫做MySyncPolicy，它使用了message_filters中的 ApproximateTime policy ，这个策略可以根据时间戳来匹配不同话题的消息，即使它们有不同的时间戳。这个策略需要指定两个参数，分别是要同步的两个话题的消息类型，这里是sensor_msgs::Image和geometry_msgs::WrenchStamped。

1
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, wrench_sub);

​ 这一行是创建了一个Synchronizer对象，它是一个消息过滤器，可以根据指定的同步策略来同步不同话题的消息。这个对象需要三个参数，第一个是同步策略的实例，这里是MySyncPolicy(10)，其中 10 表示消息队列大小。第二个和第三个参数是要同步的两个话题的订阅器，这里是image_sub和wrench_sub。

1
sync.registerCallback(boost::bind(&callback, _1, _2));

​ 这一行是注册了一个回调函数，当Synchronizer对象同步了两个话题的消息后，就会调用这个回调函数，并把同步后的消息作为参数传递给它1。这里使用了boost::bind来绑定回调函数的地址和占位符，表示当Synchronizer对象有两个参数时，就调用callback函数，并把第一个参数传递给_1，第二个参数传递给_2。

1
void callback(const sensor_msgs::ImageConstPtr& image, const geometry_msgs::WrenchStampedConstPtr& wrench)

​ 这一行是定义了一个回调函数callback，它有两个参数，分别是sensor_msgs::ImageConstPtr&和geometry_msgs::WrenchStampedConstPtr&。这两个参数的类型都是智能指针（smart pointer），它们可以自动管理内存，避免内存泄漏或无效指针1。智能指针的类型由消息类型加上ConstPtr后缀组成，例如sensor_msgs::Image消息对应的智能指针类型是sensor_msgs::ImageConstPtr。智能指针的好处是，它们可以在不同的节点间共享同一块内存，提高效率。在回调函数中，我们使用const修饰符和引用符号（&）来表示这些参数是常量引用，也就是说我们不会修改它们的值，也不会拷贝它们的值。

2023-04-22 10-04-46 的屏幕截图.png

可以看到这个策略只能筛选出时间相近的信息，并不能对齐。🤔

TimeSynchronizer

1
TimeSynchronizer<sensor_msgs::Image, geometry_msgs::WrenchStamped> sync2(image_sub, wrench_sub, 10);

这个同步器对时间精度的要求会比较高/

mypub(publisher)

生成随机图像数据和力矢量数据
安装新的包

1
2
sudo apt install ros-noetic-opencv-apps ros-noetic-cv-bridg
source /opt/ros/noetic/setup.bash

创建图像

1
2
3
// create a dummy image with random pixels
cv::Mat image(480, 640, CV_8UC3);
cv::randu(image, cv::Scalar(0, 0, 0), cv::Scalar(255, 255, 255));

​ 使用OpenCV库创建了一个480x640的三通道无符号字符型矩阵，也就是一个彩色图像。然后使用cv::randu函数给图像的每个像素赋予了一个随机的颜色值，范围是从黑色（0,0,0）到白色（255,255,255）。

cv_8uc3是一个表示图像类型的枚举值，就是用来选择图像类型，它意味着每个像素由三个无符号字符组成，分别代表蓝色、绿色和红色通道。这样的图像占用了8位的空间，也就是一个字节。

转换为ROS的图像消息

​ 首先创建了一个cv_bridge::CvImage对象，它包含了一个std_msgs::Header对象（用于存储消息的元数据），一个字符串（用于指定图像的编码方式，这里是bgr8，表示每个像素由三个无符号字符组成，分别代表蓝色、绿色和红色通道），和一个cv::Mat对象（就是前面创建的图像）。

​ 然后调用了toImageMsg() ，将cv_bridge::CvImage对象转换为sensor_msgs::ImagePtr对象，也就是一个指向sensor_msgs::Image消息的智能指针。

1
2
// convert the image to a sensor_msgs::Image message
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();

在发布的时候发布指针或者直接发送对象都可以，不过指针会快一点

1
2
pub.publish(msg); // 发布指针
pub.publish(*msg); // 发布指针指向的对象

添加时间戳信息

这个其实就是这份代码最主要的部分了，虽然简单，但是我一开始确实漏了。🤫

1
msg->header.stamp = ros::Time::now();

如果消息缺少时间戳信息的话是不会触发回调函数的。

Recoder

Cv-bridge保存/image

保存sensor_msgs::Image为PNG，并且以时间戳命名

1
2
3
4
5
6
7
8
9
10
cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

​ 使用cv_bridge::toCvCopy函数，将ROS的图像消息msg转换为OpenCV的图像，并赋值给cv_ptr。如果转换过程中发生异常，就会抛出cv_bridge::Exception类型的错误，并使用ROS_ERROR宏打印错误信息，并返回。

1
2
3
4
5
6
7
8
9
10
11
// Get the timestamp of the image message
std::stringstream ss;
ss << msg->header.stamp;
std::string timestamp = ss.str();

// Construct the filename with timestamp
char filename[256];
sprintf(filename, filename_format.c_str(), timestamp.c_str());

// Save the image as a png file
cv::imwrite(save_path + filename, cv_ptr->image);

​ 将msg的时间戳msg->header.stamp转换为字符串，并赋值给timestamp变量。具体有关 stringstream 的内容可以参考文章后面的补充。

​ 使用cv::imwrite函数，将OpenCV图像cv_ptr->image保存为png格式的文件。这个函数需要两个参数，第一个是要保存的文件路径和文件名，这里使用了save_path和filename拼接而成的字符串，save_path是一个预定义的变量，表示文件保存的路径，例如"/home/user/images/"。第二个参数是要保存的OpenCV图像。

message_filter::cache

环形缓存器

1
2
3
4
5
//set up subscriber & cache
message_filters::Subscriber<sensor_msgs::Image> sub(nh, image_topic, 1);
sub.registerCallback(boost::bind(&image_callback, _1));
cache.connectInput(sub);//输入
cache.setCacheSize(cache_size);//大小

保存/imu & /wrench

一般选择保存为csv文件。

  • 使用rostopic echo -p命令，将订阅到的话题数据以csv格式输出到文件，例如：rostopic echo -p /imu/data > imu_data.csv
  • 使用rosbag record命令，将订阅到的话题数据保存为rosbag文件，然后使用rosbag_to_csv包或者自己编写的脚本，将rosbag文件中的数据转换为csv文件。
  • 使用自己编写的节点，订阅到话题数据后，使用标准库或者第三方库（如pandas）来创建和保存csv文件。

前两种方法可能简单一些，但是显然即时性不太好，所以还是选择第三种好了。😋

csv-parser第三方库

GitHub - vincentlaucsb/csv-parser: A modern C++ library for reading, writing, and analyzing CSV (and similar) files.

我是在功能包目录下克隆仓库

在CmakeLists.txt中添加

1
2
3
4
add_subdirectory(csv-parser)

add_executable(<your program> ...)
target_link_libraries(<your program> csv)

编译后在节点文件里面引用，我不能直接引用，要加上相对路径

1
#include "../csv-parser/single_include/csv.hpp"

我的文件目录

msg_filter(功能包)

include

src

writeCSV_demo.cpp

csv-parse

single_include

csv.hpp

code_example

​ 定义一个CSVWriter对象的指针，使用std::ofstream作为输出流类型

​ csv::CSVWriter是一个模板类，它需要一个模板参数，来指定输出流的类型。需要在声明CSVWriter对象的时候，提供一个模板参数

1
2
CSVWriter<std::ofstream>* writer;
writer = new CSVWriter<std::ofstream>("/home/stonewu/Data/test.csv");

或者可以使用auto关键字，让编译器自动推断模板参数的类型

1
auto writer = new CSVWriter("/home/stonewu/Data/test.csv");

写入数据

需要注意的是writer是一个指针，需要先解引用指针

1
2
auto header = std::vector<std::string>{"topic", "time", "fx", "fy", "fz", "tx","ty","tz","\n"};
*writer << header;

这个是将订阅到的数据写入CSV文件中。虽然只有短短几行代码，却花了不少时间。

​ 实际上要注意的就是这个库传给writer的数据必须是std::vector<std::string>，所以要将这些数据转换为字符串形式，这里使用了ostringstream oss来存储。但是也不能直接通过ostringstream来构造一个vector，所以又构造了一个istringstream来作为中介。

​ 也不能通过<<endl来输入换行，这里直接用“\n”即可。 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
void wrench_callback(const geometry_msgs::WrenchStampedConstPtr& msg)
{
    // 将消息中的数据写入到CSV文件中
    ostringstream oss;
    oss << msg->header.stamp << " " << msg->wrench.force.x << " " 
        << msg->wrench.force.y << " " << msg->wrench.force.z << " " 
        << msg->wrench.torque.x << " " << msg->wrench.torque.y << " " 
        << msg->wrench.torque.z<<" "<< "\n";
        // 创建一个vector<string>对象
    vector<string> data;
    // 创建一个istringstream对象，用来读取oss中的字符串
    istringstream iss(oss.str());
    // 创建一个string对象，用来存储每个子字符串
    string word;
    // 使用空格作为分隔符，循环读取每个子字符串
    while (iss >> word) {
      // 将子字符串放入向量中
      data.push_back(word);
    }
    *writer << data ;
    return;
}

rosbag工具

使用rosbag工具录制topic数据

1
rosbag record -O output.bag /imu /wrench

这样可以将/imu和/wrench主题的数据保存到output.bag文件中。

回放数据

1
rosbag play output.bag

使用rostopic完成数据转存：

1
2
rostopic echo -b output.bag -p /imu > imu.csv
rostopic echo -b output.bag -p /wrench > wrench.csv

这样就可以将/imu和/wrench主题的数据保存到imu.csv和wrench.csv文件中，它们是以逗号分隔的文本格式。

Keyboard Input

原来的keyboard扫描函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
int scanKeyboard()
{
	int in;
	struct termios new_settings;
	struct termios stored_settings;
    //设置终端参数
	tcgetattr(0,&stored_settings);
	new_settings = stored_settings;
	new_settings.c_lflag &= (~ICANON);
	new_settings.c_cc[VTIME] = 0;
	tcgetattr(0,&stored_settings);
	new_settings.c_cc[VMIN] = 1;
	tcsetattr(0,TCSANOW,&new_settings);
	in = getchar();
	tcsetattr(0,TCSANOW,&stored_settings);
 
	return in;//返回的是字符对应的ascii码
 
}

​ 这个函数会一直等待输入，直到有键盘按键被按下。它使用了tcsetattr函数来修改终端的属性，将其设置为非规范模式（non-canonical mode），这意味着输入不会被缓冲，而是立即被读取。同时，它将VMIN属性设置为1，表示至少需要一个字符才能返回。因此，这个函数会阻塞其他函数的进行，除非有键盘输入。

重新设置终端参数

1
2
3
4
5
new_settings.c_lflag &= ~(ICANON | ECHO); 
//当关闭规范模式时，输入不会被缓冲，而是立即被读取
//如果开启规范模式，输入会被缓冲，直到遇到换行符或文件结束符才会被读取（真的吗）
new_settings.c_cc[VTIME] = 0; //设置超时时间为0，即不等待输入
new_settings.c_cc[VMIN] = 0; //设置最小字符数为0，即不需要输入任何字符就可以读取

指令集

  • q: save latest data
  • w: save all data
  • b: begin to save data
  • stop saving data

serial

串口及比较简单了，直接对照下位机的协议写即可

下位机协议

c++与ros库 补充

boost::bind

假设我们有一个函数f，它接受三个整数参数，并返回它们的和：

1
2
3
4
int f(int a, int b, int c)
{
  return a + b + c;
}

我们可以使用boost::bind来创建一个新的函数对象，它只接受一个整数参数，并把它作为f的第二个参数，而把第一和第三个参数固定为1和2。例如：

1
boost::function<int (int)> g = boost::bind(f, 1, _1, 2);

这里，_1是一个占位符，表示g的第一个参数。我们可以这样调用g：

1
2
int x = g(3); // x = f(1, 3, 2) = 6
int y = g(4); // y = f(1, 4, 2) = 7

我们也可以使用boost::bind来绑定类的成员函数。假设我们有一个类myclass，它有一个成员函数fun，它接受一个整数参数，并打印它：

1
2
3
4
5
6
7
8
class myclass
{
public:
  void fun(int x)
  {
    std::cout << x << std::endl;
  }
};

我们可以使用boost::bind来创建一个函数对象，它接受一个myclass的对象和一个整数参数，并调用该对象的fun方法。例如：

1
boost::function<void (myclass&, int)> h = boost::bind(&myclass::fun, _1, _2);

这里，_1和_2分别表示h的第一和第二个参数。我们可以这样调用h：

1
2
3
myclass mc;
h(mc, 5); // mc.fun(5)
h(mc, 6); // mc.fun(6)

Q: 定义的h有三个参数，但是我们只给他两个参数也是可以的对吗?

A: 定义的h只有两个参数，因为我们使用了boost::bind来绑定myclass的成员函数fun，它本身就是一个参数。所以h的第一个参数是一个myclass的对象，第二个参数是一个整数。我们给h两个参数就可以了，不需要再给fun传递参数。

sensor_msgs::Image

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
# 这个消息包含了一个未压缩的图像
# (0, 0) 是图像的左上角
#
# Header header        # Header timestamp 应该是图像获取的时间
#                      # Header frame_id 应该是相机的光学帧
#                      # 帧的原点应该是相机的光学中心
#                      # +x 指向图像的右边
#                      # +y 指向图像的下边
#                      # +z 指向图像的平面
#                      # 如果这里的 frame_id 和与图像相关联的 CameraInfo 消息的 frame_id 冲突
#                      # 行为是未定义的
uint32 height         # 图像高度，也就是行数
uint32 width          # 图像宽度，也就是列数
# 合法的编码值在文件 src/image_encodings.cpp 中
# 如果你想标准化一个新的字符串格式，加入
# ros-users@lists.sourceforge.net 并发送一封邮件提出一个新的编码。
string encoding       # 像素的编码 -- 通道的含义，顺序，大小
                      # 从 include/sensor_msgs/image_encodings.h 中的字符串列表中取得
uint8 is_bigendian    # 这个数据是否为大端序
uint32 step           # 每行的字节数
uint8[] data          # 实际的矩阵数据，大小是 (step * rows)

外部参数获取

1
2
3
std::string image_topic;
ros::NodeHandle pnh("~");
pnh.param<std::string>("image_topic", image_topic, "/camera/image_raw");

param方法有三个参数

  • 参数的名字
  • 参数的值
  • 参数的默认值

param方法会尝试从参数服务器或者命令行参数中获取指定名字的参数，如果成功，就将其赋值给第二个参数，如果失败，就使用第三个参数作为默认值。

rosparam工具

将image_topic参数的值设置为/camera/image

1
rosparam set image_topic /camera/image

使用rosparam get命令来获取参数的值

1
rosparam get image_topic

运行ROS节点的时候，通过命令行参数来设置参数的值

1
rosrun image_saver_node image_saver_node _image_topic:=/camera/image

注意，私有参数的名字前面要加一个下划线 _

node命名空间

话题/camera/image中的camera和image分别是什么意思呢

话题/camera/image中的camera和image分别是话题的命名空间和基本名。话题的命名空间是用来组织和区分不同的话题的，它可以有多个层级，用斜杠/分隔。话题的基本名是用来描述话题的内容或者功能的，它不能包含斜杠/或者波浪线~。话题的命名空间和基本名都应该遵循一些命名规范1，例如：

  • 话题的命名空间和基本名都应该使用小写字母，下划线_和数字。
  • 话题的基本名应该尽量具有描述性，例如laser_scan而不是scan。
  • 话题的命名空间应该尽量避免使用全局或者私有的前缀，例如/ros或者~。

在这个例子中，camera是一个命名空间，表示这个话题是关于相机的。image是一个基本名，表示这个话题传输的是图像数据。你可以使用rosnode list命令来查看当前系统中所有的话题列表。

那么我在订阅的时候这两周写法效果是一样的吗

1
2
3
ros::Subscriber sub = nh.subscribe("image", 1, imageCallback);

ros::Subscriber sub = nh.subscribe("/camera/image", 1, imageCallback);

不一定。这两种写法的效果取决于你的节点的命名空间。如果你的节点的命名空间是/，也就是全局命名空间，那么这两种写法的效果是一样的，都会订阅到/camera/image这个话题。但是如果你的节点的命名空间不是/，例如/wg，那么这两种写法的效果就不一样了。第一种写法会订阅到/wg/image这个话题，而第二种写法仍然会订阅到/camera/image这个话题。这是因为第一种写法使用了相对名称，而第二种写法使用了全局名称。相对名称会根据节点的命名空间进行解析，而全局名称不会。

请告诉我如何获取或者修改我的节点的命名空间。

你可以使用rosnode命令来获取或者修改你的节点的命名空间。rosnode命令是一个用于显示和管理节点信息的命令行工具1。你可以使用rosnode info命令来获取节点的详细信息，包括节点的命名空间，例如：

rosnode info /wg/node1

这样就会打印出/wg/node1节点的信息，其中第一行就是节点的命名空间，例如：

Node [/wg/node1] Publications:

  • /wg/image [sensor_msgs/Image]
  • /rosout [rosgraph_msgs/Log]

Subscriptions:

  • /camera/image [sensor_msgs/Image]

Services:

  • /wg/node1/get_loggers
  • /wg/node1/set_logger_level

你也可以在运行节点的时候，使用__ns参数来修改节点的命名空间，例如：

rosrun image_saver_node image_saver_node __ns:=/camera

这样就会将image_saver_node节点的命名空间设置为/camera。你也可以在launch文件中使用ns属性来设置节点的命名空间2，例如：

这样也会将image_saver_node节点的命名空间设置为/camera。

那么在默认情况下的节点都是在全局命名空间对吗，我是否能在代码的程序里面修改呢

不一定。在默认情况下，节点的命名空间是由ros::init函数的第二个参数决定的。这个参数是一个ros::NodeHandle类型的引用，它可以指定节点的命名空间和名称。如果你不提供这个参数，或者提供一个空的ros::NodeHandle，那么节点的命名空间就是/，也就是全局命名空间。但是如果你提供了一个非空的ros::NodeHandle，那么节点的命名空间就是由这个ros::NodeHandle指定的。例如：

ros::init(argc, argv, “image_saver_node”);

这样就会将节点的名称设置为image_saver_node，而节点的命名空间设置为/。

ros::NodeHandle nh(“/camera”); ros::init(argc, argv, “image_saver_node”, nh);

这样就会将节点的名称设置为image_saver_node，而节点的命名空间设置为/camera。

std::stringstream

stringstream是一个C++中的类，它可以用来操作字符串。stringstream可以将字符串转换为其他类型，例如数值，也可以将其他类型转换为字符串。stringstream还可以对字符串进行格式化，例如添加空格或者0填充。stringstream的用法类似于输入输出流，可以使用<<和>>运算符来向其中插入或者提取数据。stringstream的内部有一个字符串缓冲区，可以使用str()方法来获取或者设置。

例如：

1
2
3
4
5
6
// 声明一个stringstream对象
std::stringstream ss;  
// 向其中插入字符串和数值 
ss << “Hello” << " " << 2021; 
// 输出缓冲区的内容，结果是Hello 2021 int n; 
std::cout << ss.str() << std::endl;

从缓冲区提取数值的时候，stringstream 会按照空格或者换行符来分隔数据，然后尝试将第一个分隔的数据转换为目标类型。如果转换成功，就返回这个数值，如果转换失败，就返回 0，并且设置错误状态标志。提取之后，ss内的缓冲区内容不会改变，::但是会有一个内部指针指向下一个要提取的数据的位置::。你可以使用tellg()方法来获取这个指针的位置，也可以使用seekg()方法来移动这个指针。

例如：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// 声明一个stringstream对象 
std::stringstream ss;
// 向其中插入字符串和数值 
ss << “Hello” << " " << 2021; 
int n; 
// 从缓冲区中提取一个数值 
ss >> n; 
// 输出提取的数值，结果是0，因为Hello不能转换为int
std::cout << n << std::endl;  
// 输出内部指针的位置，结果是5，表示下一个要提取的数据是空格 
std::cout << ss.tellg() << std::endl; 
// 移动内部指针到位置6，表示下一个要提取的数据是2021 
ss.seekg(6); 
// 从缓冲区中提取一个数值 
ss >> n; 
// 输出提取的数值，结果是2021
std::cout << n << std::endl;

提取的时候，::stringstream会自动跳过空格或者换行符，直到遇到非空白字符为止。提取的顺序是从左往右的，也就是从缓冲区的开头到结尾的顺序。

例如：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
// 声明一个stringstream对象
std::stringstream ss;  
// 向其中插入一些数值，中间用空格分隔
ss << “1 2 3 4 5”;  
// 从缓冲区中提取一个数值输出，结果是1 
int n; 
ss >> n;  
std::cout << n << std::endl; 
// 从缓冲区中提取一个数值，输出,结果是2
ss >> n;
std::cout << n << std::endl; 
// 从缓冲区中提取一个数值，输出,结果是3
ss >> n; 
std::cout << n << std::endl;

ostringstream & istringstream

从前缀可以看出来，一个用作输出，一个只用作输入。