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

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

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

下面是專欄地址：

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

前言

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

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

相機(jī)標(biāo)定

參考：

realsense相機(jī)兩種獲取相機(jī)內(nèi)外參的方式

使用Realsense D435i運(yùn)行VINS-Fusion

kalibr標(biāo)定realsenseD435i --多相機(jī)標(biāo)定

kalibr標(biāo)定realsenseD435i（二）–多相機(jī)標(biāo)定

realsense相機(jī)內(nèi)參如何獲得+python pipeline+如何通過python script獲取realsense相機(jī)內(nèi)參（windows下可用）

將.bag包解析為圖片

創(chuàng)建extract.py，修改相應(yīng)信息，然后：python extract.py

#coding:utf-8

import roslib;  
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

path_left='/home/lk/Pictures/left/' #存放圖片的位置
path_right='/home/lk/Pictures/right/' #存放圖片的位置
class ImageCreator():

    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('_2023-05-29-19-42-07.bag', 'r') as bag:   #要讀取的bag文件；
            for topic,msg,t in bag.read_messages():
                if topic == "/camera/infra1/image_rect_raw":  #圖像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print (e)
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小數(shù)點(diǎn)后帶有6位，可根據(jù)精確度需要修改；
                        image_name = timestr+ ".jpg" #圖像命名：時(shí)間戳.jpg
                        cv2.imwrite(path_left+image_name, cv_image)  #保存；
                if topic == "/camera/infra2/image_rect_raw":  #圖像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print (e)
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小數(shù)點(diǎn)后帶有6位，可根據(jù)精確度需要修改；
                        image_name = timestr+ ".jpg" #圖像命名：時(shí)間戳.jpg
                        cv2.imwrite(path_right+image_name, cv_image)  #保存；


if __name__ == '__main__':

    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

在解析的left、right目錄里選取相互對(duì)應(yīng)的圖片

注：圖片要清晰，棋盤格要在視野內(nèi)。附棋盤格生成網(wǎng)站：Camera Calibration Pattern Generator

打開標(biāo)定軟件，輸入圖片、內(nèi)角點(diǎn)、尺寸

直接獲取相機(jī)參數(shù)

將相機(jī)的roslaunch停止，然后使用命令讀取參數(shù)

rs-enumerate-devices -c

黑白相機(jī)參數(shù)

左相機(jī)內(nèi)參

Intrinsic of "Infrared 1" / 424x240 / {Y8}
  Width:        424
  Height:       240
  PPX:          216.466430664062
  PPY:          118.884384155273
  Fx:           214.962127685547
  Fy:           214.962127685547
  Distortion:   Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    89.19 x 58.34

右相機(jī)內(nèi)參

 Intrinsic of "Infrared 2" / 424x240 / {Y8}
  Width:        424
  Height:       240
  PPX:          216.466430664062
  PPY:          118.884384155273
  Fx:           214.962127685547
  Fy:           214.962127685547
  Distortion:   Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    89.19 x 58.34

1. 相機(jī)內(nèi)參（Intrinsic Parameters）：
   • 焦距（Focal Length）：?jiǎn)挝粸橄袼兀╬ixel）。
   • 光學(xué)中心（Optical Center）：?jiǎn)挝粸橄袼兀╬ixel）。
2. 相機(jī)畸變參數(shù)（Distortion Parameters）：
   • 畸變系數(shù)（Distortion Coefficients）：無(wú)單位。

相機(jī)內(nèi)參和畸變參數(shù)可以通過Realsense相機(jī)的SDK或工具獲取。請(qǐng)注意，Realsense D435相機(jī)的深度圖像默認(rèn)以毫米（mm）作為單位。如果您要將深度圖像轉(zhuǎn)換為米（m）作為單位，可以將深度值除以1000。

關(guān)于 fx，fy, ppx, ppy的詳細(xì)說(shuō)明見：https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0#intrinsic-camera-parameters

外參，from infrared 1 to infrared 2 指的就是T

平移向量的單位是米

相機(jī)的內(nèi)參矩陣是
[ f x s c x 0 f y c y 0 0 1 ] \left[\begin{matrix} f_x&s&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{matrix}\right] ?fx?00?sfy?0?cx?cy?1? ?
$f_x,f_y$ 為焦距，一般情況下，二者相等。

$x_0,y_0$ 為主坐標(biāo)（相對(duì)于成像平面）。

$s$ 為坐標(biāo)軸傾斜參數(shù)，理想情況下為0

彩色相機(jī)參數(shù)

 Intrinsic of "Color" / 640x480 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        640
  Height:       480
  PPX:          326.332061767578
  PPY:          241.928192138672
  Fx:           610.158020019531
  Fy:           610.118896484375
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    55.35 x 42.95

 Intrinsic of "Color" / 848x480 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        848
  Height:       480
  PPX:          430.33203125
  PPY:          241.928192138672
  Fx:           610.157958984375
  Fy:           610.118896484375
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    69.59 x 42.95

 Intrinsic of "Color" / 960x540 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        960
  Height:       540
  PPX:          487.12353515625
  PPY:          272.169219970703
  Fx:           686.427734375
  Fy:           686.3837890625
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    69.92 x 42.95

 Intrinsic of "Color" / 1280x720 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        1280
  Height:       720
  PPX:          649.498046875
  PPY:          362.892272949219
  Fx:           915.236938476562
  Fy:           915.178405761719
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    69.92 x 42.95

 Intrinsic of "Color" / 1920x1080 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y8/Y16}
  Width:        1920
  Height:       1080
  PPX:          974.2470703125
  PPY:          544.338439941406
  Fx:           1372.85546875
  Fy:           1372.767578125
  Distortion:   Inverse Brown Conrady
  Coeffs:       0       0       0       0       0
  FOV (deg):    69.92 x 42.95

訂閱話題

參考：

Robotics - Publish and Subscribe to RGB Image from Intel Realsense

使用ros從realsence相機(jī)中獲取圖像

realsense在ros中的訂閱處理

[ROS調(diào)用cv_bridge]undefined reference to `cv_bridge::toCvCopy(boost::shared_ptr＜sensor_msgs::Image_

訂閱RGB相機(jī)

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/CameraInfo.h"
#include "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
#include "opencv2/highgui/highgui.hpp"
using namespace std;


void imgRGBCallback(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    cv::Mat color_mat;
    cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
    color_mat = cv_ptr->image;
    cv::imshow("RGB", color_mat);
    cv::waitKey(1);
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "realsense_sub");
    ros::NodeHandle n;
    ros::Subscriber subRGBImg;
    subRGBImg = n.subscribe("/camera/color/image_raw", 1,  imgRGBCallback);
    ros::spin();
    return 0;
}

CMakeLists.txt如下

cmake_minimum_required(VERSION 3.0.2)
project(depth_rs)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  cv_bridge	# 記得包含cv_bridge，不然可能會(huì)報(bào)錯(cuò)
)

add_executable(depth_rs_node src/depth_rs_node.cpp)

target_link_libraries(depth_rs_node
  ${catkin_LIBRARIES}
)

訂閱雙目

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

class RealsenseViewer
{
public:
  RealsenseViewer()
  {
    // 初始化ROS節(jié)點(diǎn)
    nh_ = ros::NodeHandle("~");

    // 創(chuàng)建左右視圖的圖像訂閱器
    left_image_sub_ = nh_.subscribe("/camera/infra1/image_rect_raw", 1, &RealsenseViewer::leftImageCallback, this);
    right_image_sub_ = nh_.subscribe("/camera/infra2/image_rect_raw", 1, &RealsenseViewer::rightImageCallback, this);

    // 創(chuàng)建窗口
    cv::namedWindow("Left View", cv::WINDOW_AUTOSIZE);
    cv::namedWindow("Right View", cv::WINDOW_AUTOSIZE);
  }

  void leftImageCallback(const sensor_msgs::ImageConstPtr& msg)
  {
    // 將ROS圖像消息轉(zhuǎn)換為OpenCV圖像
    cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);

    // 顯示左視圖圖像
    cv::imshow("Left View", cv_image->image);
    cv::waitKey(1);
  }

  void rightImageCallback(const sensor_msgs::ImageConstPtr& msg)
  {
    // 將ROS圖像消息轉(zhuǎn)換為OpenCV圖像
    cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);

    // 顯示右視圖圖像
    cv::imshow("Right View", cv_image->image);
    cv::waitKey(1);
  }

private:
  ros::NodeHandle nh_;
  ros::Subscriber left_image_sub_;
  ros::Subscriber right_image_sub_;
};

int main(int argc, char** argv)
{
  // 初始化ROS節(jié)點(diǎn)
  ros::init(argc, argv, "realsense_viewer");

  // 創(chuàng)建RealsenseViewer對(duì)象
  RealsenseViewer viewer;

  // 循環(huán)運(yùn)行ROS節(jié)點(diǎn)
  ros::spin();

  return 0;
}

立體匹配

參考：

awesome-sgbm

https://blog.csdn.net/wwp2016/article/details/86080722

SGBM算法使用

在SGBM（Semi-Global Block Matching）算法中，有多個(gè)參數(shù)可以調(diào)整以獲得最佳的立體匹配效果。以下是SGBM算法中常用參數(shù)的說(shuō)明：

1. minDisparity：最小視差值。它表示視差圖中最小的視差值，通常為0，默認(rèn)為0。。
2. numDisparities：視差范圍的數(shù)量。它表示圖像中可能的最大視差值與最小視差值之間的差異范圍。通常，它是16的倍數(shù)。
3. blockSize：匹配塊的大小。它定義了在計(jì)算代價(jià)體積時(shí)要使用的像素塊的大小。較大的塊大小可以提供更好的噪聲抑制，但可能會(huì)導(dǎo)致更大的計(jì)算開銷。它必須是奇數(shù)，并且通常在3到11之間，默認(rèn)為3。
4. P1：控制視差平滑度的第一個(gè)參數(shù)，是一個(gè)整數(shù)值，默認(rèn)為0。它是一個(gè)正整數(shù)，表示相鄰像素之間的視差差異所引起的懲罰。較大的P1值會(huì)導(dǎo)致更平滑的視差圖像，但可能會(huì)丟失一些細(xì)節(jié)。
5. P2：控制視差平滑度的第二個(gè)參數(shù)，是一個(gè)整數(shù)值，默認(rèn)為0。進(jìn)一步控制視差平滑性的參數(shù)。它通常比P1大，可以用來(lái)控制邊緣處的視差平滑程度。
6. disp12MaxDiff：左右視圖之間一致性檢查的最大差異。它是一個(gè)正整數(shù)，用于限制左右視差圖之間的最大差異。較大的值可以提高一致性，但可能會(huì)導(dǎo)致視差圖的不連續(xù)性。表示左右視差圖像之間的最大差異，默認(rèn)為0。較大的值可以用于去除不可靠的匹配。
7. preFilterCap：預(yù)處理濾波器的截?cái)嘀怠Ｋ糜诳刂祁A(yù)處理階段中代價(jià)體積的截?cái)喾秶?。預(yù)處理濾波器的容量，用于去除噪聲，默認(rèn)為0。較大的值可以提高匹配的魯棒性，但也可能導(dǎo)致某些細(xì)節(jié)丟失。
8. uniquenessRatio：視差唯一性比率，表示最佳匹配和次佳匹配之間的差異閾值，默認(rèn)為0。較大的值會(huì)增加匹配的準(zhǔn)確性，但也可能導(dǎo)致視差圖像的稀疏性增加（可能會(huì)導(dǎo)致視差圖的不連續(xù)性）。它定義了最佳匹配與次佳匹配之間的視差差異。
9. speckleWindowSize：用于去除孤立的視差點(diǎn)的窗口大小，默認(rèn)為0，表示不執(zhí)行孤立點(diǎn)過濾。
10. speckleRange：用于去除孤立的視差點(diǎn)的閾值范圍，默認(rèn)為0，表示不執(zhí)行孤立點(diǎn)過濾。

這些參數(shù)可以通過調(diào)用cv::StereoSGBM對(duì)象的相應(yīng)方法進(jìn)行設(shè)置。例如：

cv::StereoSGBM sgbm;
sgbm.setMinDisparity(minDisparity);
sgbm.setNumDisparities(numDisparities);
sgbm.setBlockSize(blockSize);
sgbm.setP1(P1);
sgbm.setP2(P2);
sgbm.setDisp12MaxDiff(disp12MaxDiff);
sgbm.setPreFilterCap(preFilterCap);
sgbm.setUniquenessRatio(uniquenessRatio);

例如

#include <opencv2/opencv.hpp>

int main() {
    cv::Mat imageLeft = cv::imread("left_image.png", cv::IMREAD_GRAYSCALE);
    cv::Mat imageRight = cv::imread("right_image.png", cv::IMREAD_GRAYSCALE);

    // 創(chuàng)建StereoSGBM對(duì)象
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create();

    // 設(shè)置SGBM算法的參數(shù)
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(64);
    sgbm->setBlockSize(5);
    sgbm->setP1(8 * imageLeft.channels() * sgbm->getBlockSize() * sgbm->getBlockSize());
    sgbm->setP2(32 * imageLeft.channels() * sgbm->getBlockSize() * sgbm->getBlockSize());
    sgbm->setDisp12MaxDiff(1);
    sgbm->setPreFilterCap(63);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(32);

    // 應(yīng)用SGBM算法進(jìn)行立體匹配
    cv::Mat disparity;
    sgbm->compute(imageLeft, imageRight, disparity);

    cv::imshow("Disparity", disparity);
    cv::waitKey(0);

    return 0;
}

后處理方式

要顯示層次更分明的深度圖，您可以對(duì)深度圖像進(jìn)行一些后處理操作，例如調(diào)整對(duì)比度、應(yīng)用顏色映射或進(jìn)行閾值化等。以下是幾種常見的方法：

1. 調(diào)整對(duì)比度和亮度：通過調(diào)整深度圖像的對(duì)比度和亮度，可以增強(qiáng)深度圖像中不同深度層次的可見性。您可以使用線性或非線性的像素值映射函數(shù)來(lái)實(shí)現(xiàn)這一點(diǎn)。

2. 顏色映射：將深度值映射到不同的顏色，可以更清晰地顯示深度圖像。例如，您可以使用熱度圖（從藍(lán)色到紅色）或彩虹色映射來(lái)表示不同深度值。

   cv::applyColorMap(depth, depthColor, cv::COLORMAP_JET);
   ```
   

3. 閾值化：根據(jù)具體的應(yīng)用需求，您可以對(duì)深度圖像進(jìn)行閾值化，并將不同深度范圍內(nèi)的像素設(shè)置為不同的值或顏色。這可以幫助您分割出特定深度層次的目標(biāo)。

   cv::Mat thresholdedDepth;
   cv::threshold(depth, thresholdedDepth, thresholdValue, maxValue, cv::THRESH_BINARY);
   ```
   

4. 深度圖像平滑化：通過應(yīng)用平滑化操作（如高斯濾波或中值濾波），可以去除深度圖像中的噪聲，使層次結(jié)構(gòu)更加清晰。

   cv::GaussianBlur(depth, smoothedDepth, cv::Size(5, 5), 0);
   ```
   

便于計(jì)算點(diǎn)云

具體的代碼為

// 將視差圖轉(zhuǎn)換為深度圖
    double baseline = 5.01066446304321;  // 基線長(zhǎng)度（單位：cm）
    double focal_length = 389.365356445312;  // 焦距（單位：像素）
    // cv::Mat depth_map = baseline * focal_length / disparity_map;


	  auto start = std::chrono::high_resolution_clock::now();

    // 定義SGBM參數(shù)
    int minDisparity = 0;  // 最小視差
    int numDisparities = 16 *6 ;  // 視差范圍的數(shù)量
    int blockSize = 16;  // 匹配塊的大小
    int P1 = 8 * image_left.channels() * blockSize * blockSize;  // P1參數(shù)
    int P2 = 32 * image_right.channels() * blockSize * blockSize;  // P2參數(shù)
    int disp12MaxDiff = 2;  // 左右視圖一致性檢查的最大差異

    // 創(chuàng)建SGBM對(duì)象并設(shè)置參數(shù)
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(minDisparity, numDisparities, blockSize);
    sgbm->setP1(P1);
    sgbm->setP2(P2);
    sgbm->setDisp12MaxDiff(disp12MaxDiff);

    // 計(jì)算視差圖像
    cv::Mat disparity;
    sgbm->compute(image_left, image_right, disparity);

    // 將視差圖像歸一化到0-255范圍內(nèi)
    cv::normalize(disparity, disparity, 0, 255, cv::NORM_MINMAX, CV_8U);

    cv::Mat depth_map = baseline * focal_length /(disparity);
    cv::Mat depthColor;
    cv::applyColorMap(depth_map, depthColor, cv::COLORMAP_JET);

    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> duration = end - start;
    double elapsedTime = duration.count();

    std::cout << "compute disparty map time cost = " << elapsedTime << " seconds. " << std::endl;

    // 顯示視差圖
    cv::imshow("Disparity Map", disparity);
    cv::imshow("Depth Map", depthColor);

    cv::waitKey(1);

示例如下

第二排的左圖為視差圖，右圖為深度的熱度圖

計(jì)算時(shí)間為

applyColorMap函數(shù)是OpenCV中用于應(yīng)用顏色映射的函數(shù)。它將灰度圖像（單通道圖像）映射到偽彩色圖像（多通道圖像），以提高圖像的可視化效果。以下是applyColorMap函數(shù)的用法說(shuō)明：

cv::applyColorMap(src, dst, colormap);

參數(shù)說(shuō)明：

• src：輸入的灰度圖像，必須是單通道圖像，如CV_8UC1或CV_32FC1。
• dst：輸出的偽彩色圖像，必須是多通道圖像，如CV_8UC3或CV_32FC3。
• colormap：顏色映射類型，可以是以下常用選項(xiàng)之一：
  • cv::COLORMAP_AUTUMN：秋季色調(diào)映射
  • cv::COLORMAP_BONE：骨骼色調(diào)映射
  • cv::COLORMAP_JET：噴射色調(diào)映射
  • cv::COLORMAP_WINTER：冬季色調(diào)映射
  • cv::COLORMAP_RAINBOW：彩虹色調(diào)映射
  • cv::COLORMAP_OCEAN：海洋色調(diào)映射
  • cv::COLORMAP_SUMMER：夏季色調(diào)映射
  • cv::COLORMAP_SPRING：春季色調(diào)映射
  • cv::COLORMAP_COOL：涼爽色調(diào)映射
  • cv::COLORMAP_HSV：HSV色調(diào)映射

根據(jù)源圖像的像素值，applyColorMap函數(shù)將適當(dāng)?shù)念伾成鋺?yīng)用到目標(biāo)圖像中的像素上，從而生成偽彩色圖像。映射的結(jié)果會(huì)保存在目標(biāo)圖像dst中。

顯示點(diǎn)云

點(diǎn)云PCL庫(kù)學(xué)習(xí)-雙目圖像轉(zhuǎn)化為點(diǎn)云PCD并顯示

點(diǎn)云PCL庫(kù)學(xué)習(xí)-雙目圖像轉(zhuǎn)化為點(diǎn)云PCD并顯示

生成的點(diǎn)云圖通過ROS發(fā)布，用rviz打開

但是生成的點(diǎn)云看起來(lái)很奇怪，經(jīng)常會(huì)有一道線

這是由于計(jì)算得到的視差圖提前歸一化了，導(dǎo)致后續(xù)計(jì)算時(shí)視差圖的值不是真值，而是歸一化之后的值

// 顯示視差圖
// 將視差圖像歸一化到0-255范圍內(nèi)
cv::normalize(disparity, disparity, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::imshow("Disparity Map", disparity);

只有在顯示前才應(yīng)該將視差圖或者深度圖歸一化到其便于顯示的值，否則就保持原值，用于計(jì)算

這里傳出的坐標(biāo)系為圖像坐標(biāo)系，在 rviz 中，紅綠藍(lán)分別代表 x,y,z 軸

要將其改為世界坐標(biāo)系后再輸出

世界坐標(biāo)系與圖像坐標(biāo)系的關(guān)系如下：

圖像坐標(biāo)系：

但是實(shí)際上應(yīng)該是如下的樣子

世界坐標(biāo)系：

或者是如下的形式，兩者等價(jià)

代碼中應(yīng)該寫為

//設(shè)為圖像坐標(biāo)系
pcl::PointXYZ point(camera_x, camera_y, camera_z);

//設(shè)為世界坐標(biāo)系
pcl::PointXYZ point(camera_z, -camera_x, -camera_y);

注意這里不是

pcl::PointXYZ point(camera_z, camera_x, -camera_y);

不然這里點(diǎn)云和圖像是相反的

步長(zhǎng)為3時(shí)生成的點(diǎn)云

這里的基線（baseline）單位設(shè)置為分米，顯示比較合適

如果設(shè)置為m，點(diǎn)云會(huì)更接近坐標(biāo)軸

如果設(shè)置為cm，點(diǎn)云會(huì)更遠(yuǎn)，稀疏到很難看見

彩色點(diǎn)云

前面生成點(diǎn)云之后，還可以進(jìn)一步生成彩色點(diǎn)云，用彩色相機(jī)給點(diǎn)云上色

 // 生成點(diǎn)云
        // 創(chuàng)建點(diǎn)云對(duì)象
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
        // 逐像素計(jì)算點(diǎn)云
        for (int y = 0; y < depth_map.rows; y += 1) {
            for (int x = 0; x < depth_map.cols; x += 1) {
                // 獲取當(dāng)前像素的深度值
                float depth = depth_map.at<float>(y, x);

                // 如果深度值為0，表示無(wú)效點(diǎn)，跳過
                if (depth == 0)
                    continue;

                // 計(jì)算相機(jī)坐標(biāo)系下的點(diǎn)坐標(biāo)
                float camera_x = (x - cx_424) * depth / fx_424;
                float camera_y = (y - cy_424) * depth / fy_424;
                float camera_z = depth;

                pcl::PointXYZRGB point;
                point.x = camera_z;
                point.y = -camera_x;
                point.z = -camera_y;
                point.r = image_color.at<cv::Vec3b>(y, x)[0];;
                point.g = image_color.at<cv::Vec3b>(y, x)[1];
                point.b = image_color.at<cv::Vec3b>(y, x)[2];

                point_cloud_tmp->push_back(point);
            }
        }

由于這里的彩色圖是接收D435發(fā)出的，其格式為 rgb8，

所以這里在填充點(diǎn)云的 RGB 分量時(shí)，對(duì)應(yīng)的通道為0，1，2，在填充點(diǎn)云的顏色時(shí)要注意通道的順序

彩色點(diǎn)云效果如下

文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-775413.html

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