SLAM算法與工程實(shí)踐系列文章

下面是SLAM算法與工程實(shí)踐系列文章的總鏈接，本人發(fā)表這個(gè)系列的文章鏈接均收錄于此

SLAM算法與工程實(shí)踐系列文章鏈接

下面是專(zhuān)欄地址：

SLAM算法與工程實(shí)踐系列專(zhuān)欄

前言

這個(gè)系列的文章是分享SLAM相關(guān)技術(shù)算法的學(xué)習(xí)和工程實(shí)踐

SLAM算法與工程實(shí)踐——相機(jī)篇：RealSense T265相機(jī)使用（2）

校正畸變

參考：

一文講透魚(yú)眼相機(jī)畸變矯正，及目標(biāo)檢測(cè)項(xiàng)目應(yīng)用

一文講透魚(yú)眼相機(jī)畸變矯正，及目標(biāo)檢測(cè)項(xiàng)目應(yīng)用

AVM環(huán)視系統(tǒng)——魚(yú)眼相機(jī)去畸變算法

無(wú)論是單目相機(jī)還是雙目相機(jī)，拍攝的圖像都會(huì)存在畸變。

它們和魚(yú)眼相機(jī)的畸變矯正原理也是一樣的：核心是求解一個(gè)“好”的重映射矩陣（remap matrix）。

從而將原圖中的部分像素點(diǎn)（或插值點(diǎn)）進(jìn)行重新排列，“拼”成一張矩形圖。

“好”是跟最終需求掛鉤的，不同任務(wù)往往采用不同的矯正/變形方案。

比如：

（1）單目相機(jī)的畸變矯正

對(duì)于單目相機(jī)，為了得到相機(jī)像素坐標(biāo)系和三維世界坐標(biāo)系的對(duì)應(yīng)關(guān)系，我們需要對(duì)相機(jī)的桶形畸變和枕形畸變進(jìn)行矯正。

（2）雙目相機(jī)的畸變矯正

而對(duì)于雙目相機(jī)，為了做極線對(duì)齊，實(shí)現(xiàn)深度估計(jì)。

我們需要將兩個(gè)相機(jī)，輸出變換到同一個(gè)坐標(biāo)系下。

張正友的棋盤(pán)標(biāo)定法，通過(guò)標(biāo)志物的位置坐標(biāo)，估計(jì)出相機(jī)的內(nèi)外參數(shù)和畸變系數(shù)，從而計(jì)算出remap matrix。該方法是目前上述兩類(lèi)相機(jī)，矯正效果最好的方法。

（3）魚(yú)眼相機(jī)的矯正變形

對(duì)于魚(yú)眼相機(jī)，主要有三種方法：棋盤(pán)標(biāo)定法、橫向展開(kāi)法、經(jīng)緯度法。

下圖是某款魚(yú)眼相機(jī)的采集圖像，而真正有效的監(jiān)控區(qū)域，是內(nèi)部的圓形區(qū)域。

棋盤(pán)標(biāo)定法

棋盤(pán)矯正法的目的，是將魚(yú)眼圖“天生”的桶形畸變進(jìn)行矯正。

具體效果類(lèi)似于“用手對(duì)著圓形中心做擠壓，把它壓平”，使得真實(shí)世界中的直線，在矯正后依然是直線。

采用棋盤(pán)標(biāo)定法進(jìn)行矯正后：

我們發(fā)現(xiàn)：

① 現(xiàn)實(shí)世界中的直線，在魚(yú)眼圖中發(fā)生了扭曲（如魚(yú)眼圖中的藍(lán)色和綠色曲線），矯正后變成了直線（如正方形圖中的藍(lán)色和綠色直線）;

② 矯正圖只占據(jù)了魚(yú)眼圖中間的一部分（如魚(yú)眼圖中的紅色曲線）。

從這個(gè)矯正效果中，可以看出：棋盤(pán)標(biāo)定法的缺點(diǎn)，是靠近圓周（外圍區(qū)域）的區(qū)域，會(huì)被拉伸的很?chē)?yán)重，視覺(jué)效果變差。

所以一般會(huì)進(jìn)行切除，導(dǎo)致矯正后的圖片只保留了原圖的中間區(qū)域。

基于以上特點(diǎn)，在實(shí)際使用中，我會(huì)把棋盤(pán)標(biāo)定法，作為簡(jiǎn)單測(cè)量的前置任務(wù)（矯正圖中的兩點(diǎn)距離和真實(shí)世界中的兩點(diǎn)距離，存在一一對(duì)應(yīng)的關(guān)系）。

也可以作為魚(yú)眼圖像拼接的前置任務(wù)（真實(shí)世界中的三點(diǎn)共線，在拼接圖中依然共線）。

橫向展開(kāi)法

橫向展開(kāi)法，主要是利用魚(yú)眼相機(jī)的大FOV和俯視拍攝的特點(diǎn)，來(lái)進(jìn)行變形。

比如我們把上圖中的紅點(diǎn)，想象成一個(gè)觀察者，當(dāng)他身體旋轉(zhuǎn)360度，看到的什么樣的畫(huà)面呢？

上圖是經(jīng)過(guò)橫向展開(kāi)法，變形后的畫(huà)面。

可以看到，從原先的俯視視角變?yōu)榱苏曇暯恰?/p>

因此可以根據(jù)區(qū)域功能，進(jìn)行切片，再用普通視角的檢測(cè)模型，做后續(xù)任務(wù)。

但是缺點(diǎn)也一目了然，比如展開(kāi)圖的左右兩側(cè)，在真實(shí)世界中應(yīng)該是連通的。

所以當(dāng)有目標(biāo)在魚(yú)眼圖中穿過(guò)分界線時(shí)，在展開(kāi)圖中該目標(biāo)會(huì)從左側(cè)消失，右側(cè)出現(xiàn)（或者倒過(guò)來(lái)），看起來(lái)不是很自然。

基于以上特點(diǎn)，在實(shí)際使用中，我會(huì)利用魚(yú)眼相機(jī)，覆蓋面積大的特點(diǎn)（比如3米層高的情況下，至少覆蓋100平米），在“某些場(chǎng)景”中取代槍機(jī)或半球機(jī)，畫(huà)面展開(kāi)后用正常的檢測(cè)器去完成后續(xù)任務(wù)。

這里還要補(bǔ)充兩點(diǎn)：

① COCO數(shù)據(jù)集上訓(xùn)練的人體檢測(cè)器，在魚(yú)眼圖中直接使用是不會(huì)work的;

② 與棋盤(pán)標(biāo)定法不同，橫向展開(kāi)不會(huì)損失像素，所以展開(kāi)圖也可以再remap回魚(yú)眼原圖

經(jīng)緯度法

經(jīng)緯度法主要分為兩個(gè)方面：

① 經(jīng)度

下圖是魚(yú)眼圖沿著經(jīng)度對(duì)齊矯正后的畫(huà)面。

該方法與棋盤(pán)矯正法相比，沒(méi)有像素?fù)p失，也不需要標(biāo)定（人為設(shè)計(jì)規(guī)則求解remap matrix）。

但是缺點(diǎn)也很明顯，它只對(duì)豎直方向（圖中的藍(lán)色線和綠色線）進(jìn)行了矯正，而水平方向（紅色線）依然是扭曲的。

② 緯度

下圖是魚(yú)眼圖沿著緯度對(duì)齊矯正后的畫(huà)面。

可以看到，只對(duì)水平方向（圖中的藍(lán)色線和綠色線）進(jìn)行了矯正，而豎直方向（紅色線）依然是扭曲的。

基于以上特點(diǎn)，在實(shí)際落地中，我沒(méi)有采用經(jīng)緯度矯正法。

更多的是在學(xué)習(xí)和研究階段，把它當(dāng)作設(shè)計(jì)和計(jì)算remap matrix的一個(gè)作業(yè)。

接收和發(fā)布圖像

參考：

ROS中C++ boost編程，類(lèi)內(nèi)回調(diào)函數(shù)

關(guān)于ROS在一個(gè)回調(diào)函數(shù)中處理兩個(gè)訂閱話題消息（多話題回調(diào)、多參數(shù)回調(diào)問(wèn)題）

ROS之訂閱多個(gè)話題并對(duì)其進(jìn)行同步處理(多傳感器融合)

同時(shí)訂閱雙目圖像

訂閱多個(gè)話題并對(duì)其進(jìn)行同步處理，我這里訂閱兩個(gè)圖像后再對(duì)圖像做處理，使用同一個(gè)回調(diào)函數(shù)

在主函數(shù)中實(shí)現(xiàn)

利用全局變量 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 }

我的實(shí)現(xiàn)

#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圖像消息轉(zhuǎn)換為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;
    // 創(chuàng)建窗口
    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節(jié)點(diǎn)
    ros::init(argc, argv, "t265_viewer");

    // 創(chuàng)建RealsenseViewer T265對(duì)象
    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));
}

在類(lèi)的成員函數(shù)中實(shí)現(xiàn)

利用類(lèi)成員 message_filters::Synchronizer

在類(lèi)中寫(xiě)函數(shù)實(shí)現(xiàn)一個(gè)回調(diào)函數(shù)處理多個(gè)話題信息時(shí)，需要用指針變量，如下所示，

不用指針變量我在寫(xiě)代碼時(shí)不會(huì)進(jìn)入回調(diào)函數(shù)，不知道是因?yàn)槲掖a寫(xiě)的有問(wèn)題還是其他的原因，

在類(lèi)的成員函數(shù)中實(shí)現(xiàn)接收多個(gè)話題，一定要用指針來(lái)實(shí)現(xiàn)

//定義變量
public:
    message_filters::Subscriber<sensor_msgs::Image> *sub_1;
    message_filters::Subscriber<sensor_msgs::Image> *sub_2;
    message_filters::Synchronizer<stereoSyncPolicy> *sync;

//類(lèi)中的函數(shù)，將兩個(gè)圖像收到后一起處理
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));
}

//或者寫(xiě)為
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庫(kù)訂閱T265圖像

T265啟動(dòng)后的話題為

/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

常用的有文章來(lái)源地址http://www.zghlxwxcb.cn/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;

    // 使能 左右目圖像數(shù)據(jù)
    cfg.enable_stream(RS2_STREAM_FISHEYE,1, RS2_FORMAT_Y8);
    cfg.enable_stream(RS2_STREAM_FISHEYE,2, RS2_FORMAT_Y8);

    // 使能 傳感器的POSE和6DOF IMU數(shù)據(jù)
    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;
}

到了這里，關(guān)于SLAM算法與工程實(shí)踐——相機(jī)篇：RealSense T265相機(jī)使用（2）的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！