INDEMIND双目视觉惯性模组实时生成点云并保存点云图

news2024/11/6 3:06:37

双目惯性相机最开始是从VINS中了解到的,2018年VINS中推荐过Loitor视觉惯性相机,但是后来看到GitHub Issue中有人反映Loitor丢帧、无技术支持等问题,加之购入渠道非官方故未入手Loitor,浏览知乎时关注到Indemind的该款产品,发现技术贴较多、SDK支持及销售渠道较为官方,故今年入手了一个,打算用于VI-SLAM用于视觉定位导航及双目三维重建等方面进行感知定位。

目前最近的SDK版本是1.4.2,这个相机成本便宜,有硬同步的IMU,频率也够高,自带标定,对于目前我只做视觉SLAM定位足够用了。然而封库,其他各种依赖库要跟着SDK的库,OpenCV不使用ROS自带的版本,使用单独版本3.4.3等等。这个SDK组织得真的是一言难尽,所以分析SDK中实时显示点云图代码并加以改进,记录下学习的过程。

文章目录

    • 一、获取点云图
      • 1、分析`get_points.cpp`源码
        • (1)包含头文件和命名空间:
        • (2)定义了一个名为clear的模板函数,用于清空队列。
        • (3)main函数开始:
      • 2、分析`util_pcl.h`源码
        • (1)包含头文件和命名空间
        • (2)定义了一个名为PCViewer的类
        • (3)头文件保护
      • 3、分析`util_pcl.h`源码
        • (1)CustomColorVis函数:这是一个辅助函数,用于创建具有自定义颜色的点云可视化对象。它创建一个PCLVisualizer对象,并将点云数据添加到视图中,设置背景颜色、点云渲染属性以及相机位置。
        • (2)PCViewer类的构造函数和析构函数:构造函数初始化viewer_成员变量为nullptr,析构函数在析构对象时关闭PCLVisualizer并释放相关资源。
        • (3)Update(const cv::Mat &xyz)函数:根据输入的3D坐标矩阵,创建一个pcl::PointCloudpcl::PointXYZ对象,并调用另一个重载的Update函数。
        • (4)Update(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc)函数:如果viewer_为空,创建一个PCLVisualizer对象并将其赋值给viewer_。然后更新视图中的点云数据,并调用spinOnce以刷新可视化。
        • (5)WasVisual()函数:检查viewer_是否存在(非空),以确定是否成功创建了PCLVisualizer对象。
        • (6)WasStopped()函数:检查viewer_是否存在且视图是否已停止(用户关闭了可视化窗口)。
        • (7)ConvertMatToPointCloud(const cv::Mat &xyz, pcl::PointCloud<pcl::PointXYZ>::Ptr pc)函数:根据输入的3D坐标矩阵,将有效的点转换为pcl::PointXYZ对象,并添加到点云对象中。最终,设置点云对象的宽度和高度。
    • 二、保存点云图至本地
      • 1、在get_points.cpp文件中添加对ConvertMatToPointCloud和SavePointCloudToFile函数的声明,以便在使用时正确识别它们
      • 2、SavePointCloudToFile函数是私有成员函数,无法在main函数中直接访问。我们需要将其更改为公有成员函数,以便在main函数中调用。将util_pcl.h文件中的PCViewer类进行如下修改
      • 3、在util_pcl.cpp中实现PCViewer类的SavePointCloudToFile函数,以便成功编译和链接。请按照之前提供的代码示例,在util_pcl.cpp中添加以下函数实现
    • 三、改进后的代码
      • 1、改进后的util_pcl.h
      • 2、改进后的util_pcl.cpp
      • 3、改进后的get_points.cpp

一、获取点云图

IMSEEE-SDK中的功能,可以初始化相机并获取实时的点云数据,然后使用PCL库进行可视化展示,首先我们分析get_points.cpp代码

1、分析get_points.cpp源码

// Copyright 2020 Indemind Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "imrdata.h"//INDEMIND相机数据处理的头文件
#include "imrsdk.h"// INDEMIND相机SDK的头文件
#include "logging.h"//日志记录的头文件
#include "times.h"//时间处理的头文件
#include "types.h"//类型定义的头文件
#include "util_pcl.h"//PCL(点云库)的实用函数的头文件
#include <queue>//队列容器的头文件
#include <mutex>// 互斥锁的头文件
using namespace indem;//使用indem命名空间

template <typename T> void clear(std::queue<T> &q) {//定义了一个名为clear的模板函数,用于清空队列
  std::queue<T> empty;
  swap(empty, q);
}

int main(int argc, char **argv) {
  auto m_pSDK = new CIMRSDK();//创建一个CIMRSDK类的实例m_pSDK,该类是IMSEEE-SDK的主要接口
  MRCONFIG config = {0};//定义并初始化一个MRCONFIG结构体变量config,用于配置相机的参数,包括是否启用SLAM、图像分辨率、图像帧率和IMU频率等
  config.bSlam = false;
  config.imgResolution = IMG_640;
  config.imgFrequency = 50;
  config.imuFrequency = 1000;

  m_pSDK->Init(config);//调用m_pSDK->Init(config)初始化相机
  std::queue<cv::Mat> points_queue;//创建一个用于存储点云数据的std::queue<cv::Mat>类型的队列points_queue
  std::mutex mutex_points;//用于保护队列操作的互斥锁mutex_points
  int points_count = 0;//定义并初始化一个整型变量points_count,用于记录接收到的点云数量
  if (m_pSDK->EnablePointProcessor()) {//如果成功启用了点云处理功能
    // m_pSDK->EnableLRConsistencyCheck();
    // m_pSDK->SetDepthCalMode(DepthCalMode::HIGH_ACCURACY);
    m_pSDK->RegistPointCloudCallback(//注册一个回调函数,当收到新的点云数据时,将其加入队列,并增加points_count的计数
        [&points_count, &points_queue, &mutex_points](double time, cv::Mat points) {
          if (!points.empty()) {
            {
                std::unique_lock<std::mutex> lock(mutex_points);
                points_queue.push(points);
            }
            ++points_count;
          }
        });
  }
  PCViewer pcviewer;//创建一个PCViewer类的实例pcviewer,用于实时显示点云
  auto &&time_beg = times::now();//获取当前时间time_beg
  while (true) {//进入主循环
    if (!points_queue.empty()) {//不断检查点云队列是否非空
      std::unique_lock<std::mutex> lock(mutex_points);//如果队列非空,使用互斥锁锁定队列
      pcviewer.Update(points_queue.front());//调用pcviewer.Update()方法更新显示的点云,并清空队列
      clear(points_queue);
    }
    char key = static_cast<char>(cv::waitKey(1));
    if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q//检查是否按下了ESC键或者'q'键,如果是则退出循环
      break;
    }
    if (pcviewer.WasStopped()) {
      break;//检查pcviewer是否已停止,如果是则退出循环
    }
  }
  delete m_pSDK;//释放相机资源,删除m_pSDK实例

  auto &&time_end = times::now();//获取循环结束后的时间time_end

  float elapsed_ms =
      times::count<times::microseconds>(time_end - time_beg) * 0.001f;//计算从开始到结束的时间间隔,并转换为毫秒
#ifdef __linux//输出日志信息,包括开始时间、结束时间、运行时间和接收到的点云数量
  LOG(INFO) << "Time beg: " << times::to_local_string(time_beg)
            << ", end: " << times::to_local_string(time_end)
            << ", cost: " << elapsed_ms << "ms";
  LOG(INFO) << "depth count: " << points_count
            << ", fps: " << (1000.f * points_count / elapsed_ms);
#endif
  return 0;//返回0,表示程序正常结束
}

这是一个使用INDEMIND相机的IMSEEE-SDK来获取点云数据并实时显示的示例代码。下面是对代码的分析:

(1)包含头文件和命名空间:

  • imrdata.h: INDEMIND相机数据处理的头文件。
  • imrsdk.h: INDEMIND相机SDK的头文件。
  • logging.h: 日志记录的头文件。
  • times.h: 时间处理的头文件。
  • types.h: 类型定义的头文件。
  • util_pcl.h: PCL(点云库)的实用函数的头文件。
  • queue: 队列容器的头文件。
  • mutex: 互斥锁的头文件。
  • cv::Mat: OpenCV库中的矩阵类。
  • using namespace indem;: 使用indem命名空间。

(2)定义了一个名为clear的模板函数,用于清空队列。

(3)main函数开始:

  • 创建一个CIMRSDK类的实例m_pSDK,该类是IMSEEE-SDK的主要接口。
  • 定义并初始化一个MRCONFIG结构体变量config,用于配置相机的参数,包括是否启用SLAM、图像分辨率、图像帧率和IMU频率等。
  • 调用m_pSDK->Init(config)初始化相机。
    创建一个用于存储点云数据的std::queuecv::Mat类型的队列points_queue,以及一个用于保护队列操作的互斥锁mutex_points。
  • 定义并初始化一个整型变量points_count,用于记录接收到的点云数量。
  • 如果成功启用了点云处理功能(通过调用m_pSDK->EnablePointProcessor()):
    ①注册一个回调函数,当收到新的点云数据时,将其加入队列,并增加points_count的计数。
  • 创建一个PCViewer类的实例pcviewer,用于实时显示点云。
  • 获取当前时间time_beg。
  • 进入主循环,不断检查点云队列是否非空:
    ①如果队列非空,使用互斥锁锁定队列,然后调用pcviewer.Update()方法更新显示的点云,并清空队列。
    ②检查是否按下了ESC键或者’q’键,如果是则退出循环。
    ③检查pcviewer是否已停止,如果是则退出循环。
  • 释放相机资源,删除m_pSDK实例。
  • 获取循环结束后的时间time_end。
  • 计算从开始到结束的时间间隔,并转换为毫秒。
  • 输出日志信息,包括开始时间、结束时间、运行时间和接收到的点云数量。
  • 返回0,表示程序正常结束。

get_points.cpp引入的头文件util_pcl.h,分析util_pcl.h的代码

2、分析util_pcl.h源码

// Copyright 2020 Indemind Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// limitations under the License.
#ifndef UTIL_PCL_H_//头文件的保护宏,防止头文件的重复包含
#define UTIL_PCL_H_//头文件的保护宏,防止头文件的重复包含
#pragma once//另一种头文件保护的方法,确保头文件只被编译一次

#include <memory>//标准库头文件,用于使用智能指针

#include <opencv2/core/core.hpp>//OpenCV库的核心头文件

#include <pcl/visualization/pcl_visualizer.h>//PCL(点云库)的可视化模块头文件

class PCViewer {//定义了一个名为PCViewer的类
public:
  PCViewer();//构造函数,用于初始化点云可视化器
  ~PCViewer();//析构函数,用于释放点云可视化器资源

  void Update(const cv::Mat &xyz);//更新点云数据,并将其转换为PCL的点云格式进行显示

  void Update(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc);//更新点云数据,并直接显示PCL的点云格式

  bool WasVisual() const;//检查点云可视化器是否已创建成功
  bool WasStopped() const;//检查点云可视化是否已停止

private:
  void ConvertMatToPointCloud(const cv::Mat &xyz,
                              pcl::PointCloud<pcl::PointXYZ>::Ptr pc);//将OpenCV的矩阵格式的点云数据转换为PCL的点云格式。

  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;//PCL可视化器的智能指针
};

#endif//结束头文件的保护

(1)包含头文件和命名空间

  • : 标准库头文件,用于使用智能指针。
  • <opencv2/core/core.hpp>: OpenCV库的核心头文件。
  • <pcl/visualization/pcl_visualizer.h>: PCL(点云库)的可视化模块头文件。

(2)定义了一个名为PCViewer的类

  • 公共成员函数:
    ①PCViewer():构造函数,用于初始化点云可视化器。
    ②~PCViewer():析构函数,用于释放点云可视化器资源。
    ③Update(const cv::Mat &xyz):更新点云数据,并将其转换为PCL的点云格式进行显示。
    ④Update(pcl::PointCloudpcl::PointXYZ::ConstPtr pc):更新点云数据,并直接显示PCL的点云格式。
    ⑤WasVisual() const:检查点云可视化器是否已创建成功。
    ⑥WasStopped() const:检查点云可视化是否已停止。
  • 私有成员函数:
    ConvertMatToPointCloud(const cv::Mat &xyz, pcl::PointCloudpcl::PointXYZ::Ptr pc):将OpenCV的矩阵格式的点云数据转换为PCL的点云格式。
  • 私有成员变量:
    std::shared_ptrpcl::visualization::PCLVisualizer viewer_:PCL可视化器的智能指针。

(3)头文件保护

  • #ifndef UTIL_PCL_H_ #define UTIL_PCL_H_:头文件的保护宏,防止头文件的重复包含。
  • #pragma once:另一种头文件保护的方法,确保头文件只被编译一次。
  • #endif:结束头文件的保护。

该头文件定义了一个简单的点云可视化类PCViewer,其中包含了点云数据的更新和显示函数,以及相应的辅助函数

3、分析util_pcl.h源码

// Copyright 2020 Indemind Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "util_pcl.h"

// #include <pcl/common/common_headers.h>

#include <cmath>

std::shared_ptr<pcl::visualization::PCLVisualizer>
CustomColorVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc) {
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  //这是一个辅助函数,用于创建具有自定义颜色的点云可视化对象。它创建一个PCLVisualizer对象,并将点云数据添加到视图中,设置背景颜色、点云渲染属性以及相机位置。
  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
      new pcl::visualization::PCLVisualizer("PointCloud Viewer"));
  viewer->setBackgroundColor(0, 0, 0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(
      pc, 255, 255, 255);
  viewer->addPointCloud<pcl::PointXYZ>(pc, single_color, "point cloud");
  viewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "point cloud");
  // viewer->addCoordinateSystem(1.0);
  viewer->addCoordinateSystem(1000.0);
  viewer->initCameraParameters();
  viewer->setCameraPosition(0, 0, -150, 0, -1, 0);
  return (viewer);
}
//构造函数初始化viewer_成员变量为nullptr
PCViewer::PCViewer() : viewer_(nullptr) {}
//析构函数在析构对象时关闭PCLVisualizer并释放相关资源
PCViewer::~PCViewer() {
  // VLOG(2) << __func__;
  if (viewer_) {
    // viewer_->saveCameraParameters("pcl_camera_params.txt");
    viewer_->close();
    viewer_ == nullptr;
  }
}
//根据输入的3D坐标矩阵,创建一个pcl::PointCloudpcl::PointXYZ对象,并调用另一个重载的Update函数
void PCViewer::Update(const cv::Mat &xyz) {
  pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
  ConvertMatToPointCloud(xyz, pc);
  Update(pc);
}
//如果viewer_为空,创建一个PCLVisualizer对象并将其赋值给viewer_。然后更新视图中的点云数据,并调用spinOnce以刷新可视化
void PCViewer::Update(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc) {
  if (viewer_ == nullptr) {
    viewer_ = CustomColorVis(pc);
  }
  viewer_->updatePointCloud(pc, "point cloud");
  viewer_->spinOnce();
}
//检查viewer_是否存在(非空),以确定是否成功创建了PCLVisualizer对象
bool PCViewer::WasVisual() const { return viewer_ != nullptr; }
//检查viewer_是否存在且视图是否已停止(用户关闭了可视化窗口)
bool PCViewer::WasStopped() const {
  return viewer_ != nullptr && viewer_->wasStopped();
}
//根据输入的3D坐标矩阵,将有效的点转换为pcl::PointXYZ对象,并添加到点云对象中。最终,设置点云对象的宽度和高度
void PCViewer::ConvertMatToPointCloud(const cv::Mat &xyz,
                                      pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
  // cv::Mat channels[3];
  // cv::split(xyz, channels);
  // double min, max;
  // cv::minMaxLoc(channels[2], &min, &max);

  for (int i = 0; i < xyz.rows; i++) {
    for (int j = 0; j < xyz.cols; j++) {
      auto &&p = xyz.at<cv::Point3f>(i, j);
      if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) {
        // LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y
        // << ", z: " << p.z;
        pcl::PointXYZ point;
        point.x = p.x * 1000.0;
        point.y = p.y * 1000.0;
        point.z = p.z * 1000.0;
        // point.z = p.z - min;
        pc->points.push_back(point);
      }
    }
  }

  pc->width = static_cast<int>(pc->points.size());
  pc->height = 1;
}

(1)CustomColorVis函数:这是一个辅助函数,用于创建具有自定义颜色的点云可视化对象。它创建一个PCLVisualizer对象,并将点云数据添加到视图中,设置背景颜色、点云渲染属性以及相机位置。

(2)PCViewer类的构造函数和析构函数:构造函数初始化viewer_成员变量为nullptr,析构函数在析构对象时关闭PCLVisualizer并释放相关资源。

(3)Update(const cv::Mat &xyz)函数:根据输入的3D坐标矩阵,创建一个pcl::PointCloudpcl::PointXYZ对象,并调用另一个重载的Update函数。

(4)Update(pcl::PointCloudpcl::PointXYZ::ConstPtr pc)函数:如果viewer_为空,创建一个PCLVisualizer对象并将其赋值给viewer_。然后更新视图中的点云数据,并调用spinOnce以刷新可视化。

(5)WasVisual()函数:检查viewer_是否存在(非空),以确定是否成功创建了PCLVisualizer对象。

(6)WasStopped()函数:检查viewer_是否存在且视图是否已停止(用户关闭了可视化窗口)。

(7)ConvertMatToPointCloud(const cv::Mat &xyz, pcl::PointCloudpcl::PointXYZ::Ptr pc)函数:根据输入的3D坐标矩阵,将有效的点转换为pcl::PointXYZ对象,并添加到点云对象中。最终,设置点云对象的宽度和高度。

二、保存点云图至本地

想将保存在队列中的点云数据保存为点云图形文件(如PCD、PLY等格式),可以在合适的时机将队列中的点云数据取出,并使用PCL库将其保存到本地

1、在get_points.cpp文件中添加对ConvertMatToPointCloud和SavePointCloudToFile函数的声明,以便在使用时正确识别它们

// 保存点云图到本地
      pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
      pcviewer.ConvertMatToPointCloud(points_queue.front(), pointcloud);
      std::string filename = "point_cloud.pcd";
      pcviewer.SavePointCloudToFile(filename, pointcloud);

2、SavePointCloudToFile函数是私有成员函数,无法在main函数中直接访问。我们需要将其更改为公有成员函数,以便在main函数中调用。将util_pcl.h文件中的PCViewer类进行如下修改

void SavePointCloudToFile(const std::string& filename,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr pc); // 添加这一行

3、在util_pcl.cpp中实现PCViewer类的SavePointCloudToFile函数,以便成功编译和链接。请按照之前提供的代码示例,在util_pcl.cpp中添加以下函数实现

void PCViewer::SavePointCloudToFile(const std::string& filename,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
  pcl::PCDWriter writer;
  writer.write(filename, *pc);
}

三、改进后的代码

1、改进后的util_pcl.h

#ifndef UTIL_PCL_H_
#define UTIL_PCL_H_
#pragma once

#include <memory>
#include <string> // 添加这一行

#include <opencv2/core/core.hpp>

#include <pcl/visualization/pcl_visualizer.h>

class PCViewer {
public:
  PCViewer();
  ~PCViewer();

  void Update(const cv::Mat &xyz);

  void Update(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc);

  bool WasVisual() const;
  bool WasStopped() const;

  void ConvertMatToPointCloud(const cv::Mat &xyz,
                              pcl::PointCloud<pcl::PointXYZ>::Ptr pc);

  void SavePointCloudToFile(const std::string& filename,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr pc); // 添加这一行

private:
  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
};

#endif
#ifndef UTIL_PCL_H_
#define UTIL_PCL_H_
#pragma once

#include <memory>
#include <string> // 添加这一行

#include <opencv2/core/core.hpp>

#include <pcl/visualization/pcl_visualizer.h>

class PCViewer {
public:
  PCViewer();
  ~PCViewer();

  void Update(const cv::Mat &xyz);

  void Update(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc);

  bool WasVisual() const;
  bool WasStopped() const;

  void ConvertMatToPointCloud(const cv::Mat &xyz,
                              pcl::PointCloud<pcl::PointXYZ>::Ptr pc);

  void SavePointCloudToFile(const std::string& filename,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr pc); // 添加这一行

private:
  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
};

#endif

2、改进后的util_pcl.cpp

// Copyright 2020 Indemind Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "util_pcl.h"
#include <pcl/io/pcd_io.h>
// #include <pcl/common/common_headers.h>

#include <cmath>

std::shared_ptr<pcl::visualization::PCLVisualizer>
CustomColorVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc) {
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
      new pcl::visualization::PCLVisualizer("PointCloud Viewer"));
  viewer->setBackgroundColor(0, 0, 0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(
      pc, 255, 255, 255);
  viewer->addPointCloud<pcl::PointXYZ>(pc, single_color, "point cloud");
  viewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "point cloud");
  // viewer->addCoordinateSystem(1.0);
  viewer->addCoordinateSystem(1000.0);
  viewer->initCameraParameters();
  viewer->setCameraPosition(0, 0, -150, 0, -1, 0);
  return (viewer);
}

PCViewer::PCViewer() : viewer_(nullptr) {}

PCViewer::~PCViewer() {
  // VLOG(2) << __func__;
  if (viewer_) {
    // viewer_->saveCameraParameters("pcl_camera_params.txt");
    viewer_->close();
    viewer_ == nullptr;
  }
}

void PCViewer::Update(const cv::Mat &xyz) {
  pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
  ConvertMatToPointCloud(xyz, pc);
  Update(pc);
}

void PCViewer::Update(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc) {
  if (viewer_ == nullptr) {
    viewer_ = CustomColorVis(pc);
  }
  viewer_->updatePointCloud(pc, "point cloud");
  viewer_->spinOnce();
}

bool PCViewer::WasVisual() const { return viewer_ != nullptr; }

bool PCViewer::WasStopped() const {
  return viewer_ != nullptr && viewer_->wasStopped();
}

 void PCViewer::SavePointCloudToFile(const std::string& filename,pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
    pcl::PCDWriter writer;
    writer.write(filename, *pc);
  }

void PCViewer::ConvertMatToPointCloud(const cv::Mat &xyz,
                                      pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
  // cv::Mat channels[3];
  // cv::split(xyz, channels);
  // double min, max;
  // cv::minMaxLoc(channels[2], &min, &max);

  for (int i = 0; i < xyz.rows; i++) {
    for (int j = 0; j < xyz.cols; j++) {
      auto &&p = xyz.at<cv::Point3f>(i, j);
      if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) {
        // LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y
        // << ", z: " << p.z;
        pcl::PointXYZ point;
        point.x = p.x * 1000.0;
        point.y = p.y * 1000.0;
        point.z = p.z * 1000.0;
        // point.z = p.z - min;
        pc->points.push_back(point);
      }
    }
  }

  pc->width = static_cast<int>(pc->points.size());
  pc->height = 1;
}

3、改进后的get_points.cpp

#include "imrdata.h"
#include "imrsdk.h"
#include "logging.h"
#include "times.h"
#include "types.h"
#include "util_pcl.h"
#include <queue>
#include <mutex>
#include <pcl/io/pcd_io.h>

using namespace indem;

template <typename T> void clear(std::queue<T> &q) {
  std::queue<T> empty;
  swap(empty, q);
}

int main(int argc, char **argv) {
  auto m_pSDK = new CIMRSDK();
  MRCONFIG config = {0};
  config.bSlam = false;
  config.imgResolution = IMG_640;
  config.imgFrequency = 50;
  config.imuFrequency = 1000;

  m_pSDK->Init(config);
  std::queue<cv::Mat> points_queue;
  std::mutex mutex_points;
  int points_count = 0;
  if (m_pSDK->EnablePointProcessor()) {
    // m_pSDK->EnableLRConsistencyCheck();
    // m_pSDK->SetDepthCalMode(DepthCalMode::HIGH_ACCURACY);
    m_pSDK->RegistPointCloudCallback(
        [&points_count, &points_queue, &mutex_points](double time, cv::Mat points) {
          if (!points.empty()) {
            {
                std::unique_lock<std::mutex> lock(mutex_points);
                points_queue.push(points);
            }
            ++points_count;
          }
        });
  }
  PCViewer pcviewer;
  auto &&time_beg = times::now();
  while (true) {
    if (!points_queue.empty()) {
      std::unique_lock<std::mutex> lock(mutex_points);
      pcviewer.Update(points_queue.front());
      
      // 保存点云图到本地
      pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
      pcviewer.ConvertMatToPointCloud(points_queue.front(), pointcloud);
      std::string filename = "point_cloud.pcd";
      pcviewer.SavePointCloudToFile(filename, pointcloud);
      
      clear(points_queue);
    }
    char key = static_cast<char>(cv::waitKey(1));
    if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q
      break;
    }
    if (pcviewer.WasStopped()) {
      break;
    }
  }
  delete m_pSDK;

  auto &&time_end = times::now();

  float elapsed_ms =
      times::count<times::microseconds>(time_end - time_beg) * 0.001f;
#ifdef __linux
  LOG(INFO) << "Time beg: " << times::to_local_string(time_beg)
            << ", end: " << times::to_local_string(time_end)
            << ", cost: " << elapsed_ms << "ms";
  LOG(INFO) << "depth count: " << points_count
            << ", fps: " << (1000.f * points_count / elapsed_ms);
#endif
  return 0;
}

在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/686982.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

AI绘画:Roop插件的特性与安装!

交叉”学科”来了&#xff01; 我们之前讲过可以实现单图换脸的Roop&#xff0c;也讲过可以通过文字描述画画的项目Stable-Diffusion-WebUI。现在这两者要通过sd-webui-roop产生交汇。 我们先来简单的看一下这个插件可以干什么&#xff01; 功能特点 根据项目作者的说法&…

JavaScript二叉树及各种遍历算法详情

目录 什么是二叉树 满二叉树完全二叉树二叉树的存储 数组存储链表存储与二叉树相关的算法 深度优先遍历广度优先遍历先序遍历中序遍历后序遍历 前言: 上一篇文章中介绍了树的概念、深度优先遍历和广度优先遍历&#xff0c;这篇文章我们来学习一个特殊的树——二叉树。 什么是…

【计算机图形学】期末复习,选择题+判断题篇

【计算机图形学】期末复习&#xff0c;选择题判断题篇 题目来源于百度、B站、中国大学慕课网&#xff0c;适用于期末复习&#xff0c;内容仅供参考&#xff0c;祝大家考试顺利通过&#xff01;&#xff01;&#xff01; 文章目录 【计算机图形学】期末复习&#xff0c;选择题判…

App压力稳定性测试之Monkey

目录 前言&#xff1a; 一、Monkey简介 二、monkey常见命令 三、日志导出 前言&#xff1a; Monkey测试是一种黑盒测试方法&#xff0c;用于测试Android应用程序的压力稳定性&#xff0c;目的是评估应用在极端情况下是否能够稳定、可靠地工作。它是Android SDK自带的一个工…

起动元件框图原理

&#xff08;一&#xff09;起动元件作用 为了提高保护动作的可靠性&#xff0c;保护装置的出口均经起动元件闭锁&#xff0c;只有在保护起动元 件起动后&#xff0c;保护装置出口闭锁才被解除。在微机保护装置里&#xff0c;起动元件是由软件来完成的。起动元件起动后&#…

鞣花酸爆增1226%?油橄榄、雪绒花大展拳脚? | 5月功效成分TOP100

数说故事联合用户说共创的5月功效成分榜单如约而至。 本期依旧是你最关注的两大榜单&#xff1a;5月用户最关注功效成分声量TOP100和5月用户最关注功效成分变量TOP100。 榜单通过整合全网社交媒体的声量&#xff0c;并构建指数体系&#xff0c;实时动态监控互联网关键成分的声…

RabbitMQ消息队列的工作模式

文章目录 1.RabbitMQ常用的工作模式2.简单模式3.WorkQueues工作队列模式4.Pub/Sub发布订阅模式5.Routing路由模式6.Topics通配符模式 1.RabbitMQ常用的工作模式 官方文档地址&#xff1a;https://www.rabbitmq.com/getstarted.html 工作模式其实就是消息队列分发消息的路由方…

mysql死锁问题分析

死锁问题分析 起因 起因是线上报了一个死锁问题&#xff0c;然后我就去查看下死锁的原因。 思路 死锁问题的排查&#xff0c; **日常工作中&#xff0c;应对各类线上异常都要有我们自己的 SOP (标准作业流程) ** &#xff0c;这样不仅能够提高自己的处理问题效率&#xff…

智能本质上是人性的拓扑

智能技术的发展是基于人类智慧和思维方式的延伸和拓展&#xff0c;人类的智慧和思维方式是智能的基础&#xff0c;人类是智能技术的创造者和主导者。然而&#xff0c;人工智能技术却与人性并不一致&#xff0c;根本上&#xff0c;人工智能技术并不具备人类的情感、道德、意识等…

适配器模式(Adapter)

定义 适配器是一种结构型设计模式&#xff0c;它能使接口不兼容的对象能够相互合作。 别名 封装器模式&#xff08;Wrapper&#xff09;。 前言 1. 问题 假如你正在开发一款股票市场监测程序&#xff0c;它会从不同来源下载 XML 格式的股票数据&#xff0c;然后向用户呈现…

通过skia导出pdf 生成超链接 skia pdfdocument annotation pdflink

如题 最近导出pdf &#xff0c;想实现文本支持超链接跳转, 看了下skia的官网文档, 翻墙找各种资料 就是找不到关于怎么实现&#xff0c;毫无头绪咋办呢 我想了一下 1.粗略翻阅了下pdf的格式 了解了下基本的构成 啥root page text 啥的 2.通过pdf格式了解到 链接是通过LinkAn…

npm发布自己的公网包步骤详解

初始化项目 比如我&#xff0c;创建了code-transfor-text_vue项目 根目录初始化git git init .建立开源协议 给项目根目录手动创建LICENSE文件文件&#xff0c;没有后缀名 MIT LicenseCopyright (c) 2023 quanyiPermission is hereby granted, free of charge, to any pers…

Verdaccio搭建本地npm仓库

背景 Verdaccio 是一个 Node.js创建的轻量的私有npm proxy registry 我们在开发npm包的时候&#xff0c;经常需要验证发包流程&#xff0c;或者开发的npm包仅局限于公司内部使用时&#xff0c;就可以借助Verdaccio搭建一个npm仓库&#xff0c;搭建完之后&#xff0c;只要更改np…

DataLeap的全链路智能监控报警实践(二):概念介绍

更多技术交流、求职机会&#xff0c;欢迎关注字节跳动数据平台微信公众号&#xff0c;回复【1】进入官方交流群 概念介绍 基线监控 根据监控规则和任务运行情况&#xff0c;DataLeap的基线监控能够决策是否报警、何时报警、如何报警以及给谁报警。它保障的是任务整体产出链路&a…

MySQL 数据库操作指南:学习如何使用 Python 进行增删改查操作

文章目录 MySQL 知识点1.1 数据库创建和选择1.2 数据表创建和修改1.3 插入数据1.4 查询数据1.5 更新和删除数据 1.6 索引的创建和使用1.7 外键的使用 Python 中使用 MySQL2.1 连接数据库2.2 创建数据库和数据表2.3 插入数据2.4 查询数据2.5 更新和删除数据2.6 关闭连接 2.7 数据…

【算法系列之贪心算法I】leetcode376. 摆动序列

455.分发饼干 力扣题目链接 假设你是一位很棒的家长&#xff0c;想要给你的孩子们一些小饼干。但是&#xff0c;每个孩子最多只能给一块饼干。 对每个孩子 i&#xff0c;都有一个胃口值 g[i]&#xff0c;这是能让孩子们满足胃口的饼干的最小尺寸&#xff1b;并且每块饼干 j&…

详细讲解!接口性能测试方案

目录 前言&#xff1a; 性能测试术语解释 性能测试方法及目标 性能需求分析 性能测试范围 性能测试用例与场景 性能测试工具选择 性能测试结果分析 性能测试通过标准 前言&#xff1a; 接口性能测试是指测试系统中各个接口的性能&#xff0c;包括响应时间、吞吐量、并…

现在企业都在强调的客户体验,如何从官网帮助文档入手?

在当前激烈的市场竞争中&#xff0c;企业已经逐渐意识到客户体验的重要性。客户体验是指通过产品和服务所提供的一系列互动和接触&#xff0c;客户对企业的全面感受和评价。而在客户体验中&#xff0c;官网帮助文档作为企业与客户之间互动的重要环节&#xff0c;也扮演着重要的…

性能测试之测试指标

目录 前言 系统性能指标 资源指标 中间件指标 数据库指标 前端指标 稳定性指标 批量处理指标 可扩展性指标 可靠性指标 前言 性能测试是测试一个系统在特定条件下的响应时间、并发用户数、吞吐量、内存使用率、CPU利用率、网络延迟等各项指标的过程。测试指标是根据…

Alibaba Cloud Linux 3.2104 LTS 64位 安装lnmp环境php8、mysql8

Alibaba Cloud Linux 3.2104 LTS 64位服务器安装lnmp环境全过程 以下都为阿里云购买的服务器为例 前言 购买了阿里云的服务器之后切记切记切记&#xff01; 第一步设置&#xff1a;更多> 网络和安全组> 安全组配置>入方向 第二步 设置root账户的密码&#xff08;如…