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插件的特性与安装！

AI绘画：Roop插件的特性与安装！

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

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

目录 什么是二叉树 满二叉树完全二叉树二叉树的存储 数组存储链表存储与二叉树相关的算法 深度优先遍历广度优先遍历先序遍历中序遍历后序遍历 前言: 上一篇文章中介绍了树的概念、深度优先遍历和广度优先遍历&#xff0c;这篇文章我们来学习一个特殊的树——二叉树。 什么是…
阅读更多...
【计算机图形学】期末复习，选择题+判断题篇

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

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

App压力稳定性测试之Monkey

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

起动元件框图原理

&#xff08;一&#xff09;起动元件作用 为了提高保护动作的可靠性&#xff0c;保护装置的出口均经起动元件闭锁&#xff0c;只有在保护起动元 件起动后&#xff0c;保护装置出口闭锁才被解除。在微机保护装置里&#xff0c;起动元件是由软件来完成的。起动元件起动后&#…
阅读更多...
鞣花酸爆增1226%？油橄榄、雪绒花大展拳脚？ | 5月功效成分TOP100

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

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

RabbitMQ消息队列的工作模式

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

mysql死锁问题分析

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

智能本质上是人性的拓扑

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

适配器模式（Adapter）

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

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

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

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搭建本地npm仓库

背景 Verdaccio 是一个 Node.js创建的轻量的私有npm proxy registry 我们在开发npm包的时候&#xff0c;经常需要验证发包流程&#xff0c;或者开发的npm包仅局限于公司内部使用时&#xff0c;就可以借助Verdaccio搭建一个npm仓库&#xff0c;搭建完之后&#xff0c;只要更改np…
阅读更多...
DataLeap的全链路智能监控报警实践（二）：概念介绍

DataLeap的全链路智能监控报警实践（二）：概念介绍

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

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. 摆动序列

【算法系列之贪心算法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环境php8、mysql8

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

Copyright @ 2022~2023