SLAM算法与工程实践系列文章
下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此
SLAM算法与工程实践系列文章链接
下面是专栏地址：
SLAM算法与工程实践系列专栏
前言
这个系列的文章是分享SLAM相关技术算法的学习和工程实践
SLAM算法与工程实践——相机篇：RealSense T265相机使用（2）
校正畸变
参考：
一文讲透鱼眼相机畸变矫正，及目标检测项目应用
一文讲透鱼眼相机畸变矫正，及目标检测项目应用
AVM环视系统——鱼眼相机去畸变算法
无论是单目相机还是双目相机，拍摄的图像都会存在畸变。
它们和鱼眼相机的畸变矫正原理也是一样的：核心是求解一个“好”的重映射矩阵（remap matrix）。
从而将原图中的部分像素点（或插值点）进行重新排列，“拼”成一张矩形图。
“好”是跟最终需求挂钩的，不同任务往往采用不同的矫正/变形方案。
比如：
（1）单目相机的畸变矫正
对于单目相机，为了得到相机像素坐标系和三维世界坐标系的对应关系，我们需要对相机的桶形畸变和枕形畸变进行矫正。
（2）双目相机的畸变矫正
而对于双目相机，为了做极线对齐，实现深度估计。
我们需要将两个相机，输出变换到同一个坐标系下。
张正友的棋盘标定法，通过标志物的位置坐标，估计出相机的内外参数和畸变系数，从而计算出remap matrix。该方法是目前上述两类相机，矫正效果最好的方法。
（3）鱼眼相机的矫正变形
对于鱼眼相机，主要有三种方法：棋盘标定法、横向展开法、经纬度法。
下图是某款鱼眼相机的采集图像，而真正有效的监控区域，是内部的圆形区域。
棋盘标定法
棋盘矫正法的目的，是将鱼眼图“天生”的桶形畸变进行矫正。
具体效果类似于“用手对着圆形中心做挤压，把它压平”，使得真实世界中的直线，在矫正后依然是直线。
采用棋盘标定法进行矫正后：
我们发现：
① 现实世界中的直线，在鱼眼图中发生了扭曲（如鱼眼图中的蓝色和绿色曲线），矫正后变成了直线（如正方形图中的蓝色和绿色直线）;
② 矫正图只占据了鱼眼图中间的一部分（如鱼眼图中的红色曲线）。
从这个矫正效果中，可以看出：棋盘标定法的缺点，是靠近圆周（外围区域）的区域，会被拉伸的很严重，视觉效果变差。
所以一般会进行切除，导致矫正后的图片只保留了原图的中间区域。
基于以上特点，在实际使用中，我会把棋盘标定法，作为简单测量的前置任务（矫正图中的两点距离和真实世界中的两点距离，存在一一对应的关系）。
也可以作为鱼眼图像拼接的前置任务（真实世界中的三点共线，在拼接图中依然共线）。
横向展开法
横向展开法，主要是利用鱼眼相机的大FOV和俯视拍摄的特点，来进行变形。
比如我们把上图中的红点，想象成一个观察者，当他身体旋转360度，看到的什么样的画面呢？
上图是经过横向展开法，变形后的画面。
可以看到，从原先的俯视视角变为了正视视角。
因此可以根据区域功能，进行切片，再用普通视角的检测模型，做后续任务。
但是缺点也一目了然，比如展开图的左右两侧，在真实世界中应该是连通的。
所以当有目标在鱼眼图中穿过分界线时，在展开图中该目标会从左侧消失，右侧出现（或者倒过来），看起来不是很自然。
基于以上特点，在实际使用中，我会利用鱼眼相机，覆盖面积大的特点（比如3米层高的情况下，至少覆盖100平米），在“某些场景”中取代枪机或半球机，画面展开后用正常的检测器去完成后续任务。
这里还要补充两点：
① COCO数据集上训练的人体检测器，在鱼眼图中直接使用是不会work的;
② 与棋盘标定法不同，横向展开不会损失像素，所以展开图也可以再remap回鱼眼原图
经纬度法
经纬度法主要分为两个方面：
① 经度
下图是鱼眼图沿着经度对齐矫正后的画面。
该方法与棋盘矫正法相比，没有像素损失，也不需要标定（人为设计规则求解remap matrix）。
但是缺点也很明显，它只对竖直方向（图中的蓝色线和绿色线）进行了矫正，而水平方向（红色线）依然是扭曲的。
② 纬度
下图是鱼眼图沿着纬度对齐矫正后的画面。
可以看到，只对水平方向（图中的蓝色线和绿色线）进行了矫正，而竖直方向（红色线）依然是扭曲的。
基于以上特点，在实际落地中，我没有采用经纬度矫正法。
更多的是在学习和研究阶段，把它当作设计和计算remap matrix的一个作业。
接收和发布图像
参考：
ROS中C++ boost编程，类内回调函数
关于ROS在一个回调函数中处理两个订阅话题消息（多话题回调、多参数回调问题）
ROS之订阅多个话题并对其进行同步处理(多传感器融合)
同时订阅双目图像
订阅多个话题并对其进行同步处理，我这里订阅两个图像后再对图像做处理，使用同一个回调函数
在主函数中实现
利用全局变量 TimeSynchronizer
opencv官方方法：http://wiki.ros.org/action/fullsearch/message_filters?action=fullsearch&context=180&value=linkto%3A%22message_filters%22#Subscriber
#include <message_filters/subscriber.h>
2 #include <message_filters/synchronizer.h>
3 #include <message_filters/sync_policies/exact_time.h>
4 #include <sensor_msgs/Image.h>
5 #include <sensor_msgs/CameraInfo.h>
6
7 using namespace sensor_msgs;
8 using namespace message_filters;
9
10 void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
11 {
12 // Solve all of perception here...
13 }
14
15 int main(int argc, char** argv)
16 {
17 ros::init(argc, argv, "vision_node");
18
19 ros::NodeHandle nh;
20 message_filters::Subscriber<Image> image_sub(nh, "image", 1);
21 message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
22
23 typedef sync_policies::ExactTime<Image, CameraInfo> MySyncPolicy;
24 // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
25 Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
26 sync.registerCallback(boost::bind(&callback, _1, _2));
27
28 ros::spin();
29
30 return 0;
31 }
我的实现
#include "UseT265Cam.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include <pcl/point_cloud.h>
#include <ros/ros.h>
#include <boost/bind.hpp>
#include <cv_bridge/cv_bridge.h>
void StereoImageCallback(const sensor_msgs::ImageConstPtr &img1_msg, const sensor_msgs::ImageConstPtr &img2_msg) {
// 将ROS图像消息转换为OpenCV图像
cv_bridge::CvImagePtr cv_image_left = cv_bridge::toCvCopy(img1_msg, sensor_msgs::image_encodings::MONO8);
cv_bridge::CvImagePtr cv_image_right = cv_bridge::toCvCopy(img2_msg, sensor_msgs::image_encodings::MONO8);
cv::Mat image_left = cv_image_left->image.clone();
cv::Mat image_right = cv_image_right->image.clone();
std::cout << "xxx" << std::endl;
// 创建窗口
cv::namedWindow("Left Raw View", cv::WINDOW_AUTOSIZE);
cv::namedWindow("Right Raw View", cv::WINDOW_AUTOSIZE);
// 显示图像
cv::imshow("Left Raw View", image_left);
cv::imshow("Right Raw View", image_right);
cv::waitKey(1);
}
int main(int argc, char **argv) {
// 初始化ROS节点
ros::init(argc, argv, "t265_viewer");
// 创建RealsenseViewer T265对象
UseT265Cam *t265_cam = new UseT265Cam();
ros::NodeHandle nh;
message_filters::Subscriber<sensor_msgs::Image> image_sub1(nh, "/camera/fisheye1/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> image_sub2(nh, "/camera/fisheye2/image_raw", 1);
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> sync(image_sub1, image_sub2, 10);
sync.registerCallback(boost::bind(&StereoImageCallback, _1, _2));
}
在类的成员函数中实现
利用类成员 message_filters::Synchronizer
在类中写函数实现一个回调函数处理多个话题信息时，需要用指针变量，如下所示，
不用指针变量我在写代码时不会进入回调函数，不知道是因为我代码写的有问题还是其他的原因，
在类的成员函数中实现接收多个话题，一定要用指针来实现
//定义变量
public:
message_filters::Subscriber<sensor_msgs::Image> *sub_1;
message_filters::Subscriber<sensor_msgs::Image> *sub_2;
message_filters::Synchronizer<stereoSyncPolicy> *sync;
//类中的函数，将两个图像收到后一起处理
void UseT265Cam::subscribeStereoImage() {
sub_1 = new message_filters::Subscriber<sensor_msgs::Image>(_nh, "/camera/fisheye1/image_raw", 10);
sub_2 = new message_filters::Subscriber<sensor_msgs::Image>(_nh, "/camera/fisheye2/image_raw", 10);
sync = new message_filters::Synchronizer<stereoSyncPolicy>(stereoSyncPolicy(10), *sub_1, *sub_2);
sync->registerCallback(boost::bind(&UseT265Cam::StereoImageCallback, this, _1, _2));
}
//或者写为
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> *sync1=new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10),*sub_1, *sub_2);
sync1->registerCallback(boost::bind(&UseT265Cam::StereoImageCallback, this, _1, _2));
使用Opencv库订阅T265图像
T265启动后的话题为文章来源：https://www.toymoban.com/news/detail-800338.html
/camera/accel/imu_info
/camera/accel/metadata
/camera/accel/sample
/camera/fisheye1/camera_info
/camera/fisheye1/image_raw
/camera/fisheye1/image_raw/compressed
/camera/fisheye1/image_raw/compressed/parameter_descriptions
/camera/fisheye1/image_raw/compressed/parameter_updates
/camera/fisheye1/image_raw/compressedDepth
/camera/fisheye1/image_raw/compressedDepth/parameter_descriptions
/camera/fisheye1/image_raw/compressedDepth/parameter_updates
/camera/fisheye1/image_raw/theora
/camera/fisheye1/image_raw/theora/parameter_descriptions
/camera/fisheye1/image_raw/theora/parameter_updates
/camera/fisheye1/metadata
/camera/fisheye2/camera_info
/camera/fisheye2/image_raw
/camera/fisheye2/image_raw/compressed
/camera/fisheye2/image_raw/compressed/parameter_descriptions
/camera/fisheye2/image_raw/compressed/parameter_updates
/camera/fisheye2/image_raw/compressedDepth
/camera/fisheye2/image_raw/compressedDepth/parameter_descriptions
/camera/fisheye2/image_raw/compressedDepth/parameter_updates
/camera/fisheye2/image_raw/theora
/camera/fisheye2/image_raw/theora/parameter_descriptions
/camera/fisheye2/image_raw/theora/parameter_updates
/camera/fisheye2/metadata
/camera/gyro/imu_info
/camera/gyro/metadata
/camera/gyro/sample
/camera/odom/metadata
/camera/odom/sample
/camera/realsense2_camera_manager/bond
/camera/tracking_module/parameter_descriptions
/camera/tracking_module/parameter_updates
常用的有文章来源地址https://www.toymoban.com/news/detail-800338.html
/camera/fisheye1/image_raw
/camera/fisheye2/image_raw
/camera/accel/imu_info
/camera/accel/metadata
/camera/accel/sample
/camera/gyro/imu_info
/camera/gyro/metadata
/camera/gyro/sample
#include<iostream>
#include<string>
#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;
int main(int argc,char** argv)
{
rs2::config cfg;
// 使能 左右目图像数据
cfg.enable_stream(RS2_STREAM_FISHEYE,1, RS2_FORMAT_Y8);
cfg.enable_stream(RS2_STREAM_FISHEYE,2, RS2_FORMAT_Y8);
// 使能 传感器的POSE和6DOF IMU数据
cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
rs2::pipeline pipe;
pipe.start(cfg);
rs2::frameset data;
while (1)
{
data = pipe.wait_for_frames();
// Get a frame from the pose stream
auto f = data.first_or_default(RS2_STREAM_POSE);
auto pose = f.as<rs2::pose_frame>().get_pose_data();
cout<<"px: "<<pose.translation.x<<" py: "<<pose.translation.y<<" pz: "<<pose.translation.z<<
"vx: "<<pose.velocity.x<<" vy: "<<pose.velocity.y<<" vz: "<<pose.velocity.z<<endl;
cout<<"ax: "<<pose.acceleration.x<<" ay: "<<pose.acceleration.y<<" az: "<<pose.acceleration.z<<
"gx: "<<pose.angular_velocity.x<<" gy: "<<pose.angular_velocity.y<<" gz: "<<pose.angular_velocity.z<<endl;
rs2::frame image_left = data.get_fisheye_frame(1);
rs2::frame image_right = data.get_fisheye_frame(2);
if (!image_left || !image_right)
break;
cv::Mat cv_image_left(cv::Size(848, 800), CV_8U, (void*)image_left.get_data(), cv::Mat::AUTO_STEP);
cv::Mat cv_image_right(cv::Size(848, 800), CV_8U, (void*)image_right.get_data(), cv::Mat::AUTO_STEP);
cv::imshow("left", cv_image_left);
cv::imshow("right", cv_image_right);
cv::waitKey(1);
}
return 0;
}
到了这里，关于SLAM算法与工程实践——相机篇：RealSense T265相机使用（2）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
