INDEMIND binocular visual inertial module generates point cloud in real time and saves the point cloud image

Enterprise 2023-08-12 18:02:32 views: null

The binocular inertial camera was first learned from VINS. In 2018, VINS recommended the Loitor visual inertial camera, but later I saw some people in the GitHub Issue reporting that Loitor lost frames and had no technical support. In addition, the purchase channel is unofficial So I didn’t buy Loitor. I noticed this product from Indemind when I browsed Zhihu. I found that there are many technical posts, SDK support and sales channels are more official, so I bought one this year. I plan to use it in VI-SLAM for visual positioning and navigation. 3D reconstruction and other aspects of perceptual positioning.

At present, the latest SDK version is 1.4.2. This camera is cheap, has a hard-synchronized IMU, the frequency is high enough, and comes with calibration. It is enough for me to only do visual SLAM positioning at present. However, to close the library, other various dependent libraries must follow the SDK library. OpenCV does not use the version that comes with ROS, but uses a separate version 3.4.3 and so on. The organization of this SDK is really indescribable, so analyze the real-time display point cloud image code in the SDK and improve it, and record the learning process.

Article directory

1. Obtain point cloud image

The functions in IMSEEEE-SDK can initialize the camera and obtain real-time point cloud data, and then use the PCL library for visual display. First, we analyze the get_points.cppcode

1. Analyze get_points.cppthe source code

// 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，表示程序正常结束
}

This is a sample code that uses the IMSEEE-SDK of the INDEMIND camera to acquire point cloud data and display it in real time. The following is an analysis of the code:

(1) Include header files and namespaces:

  • imrdata.h: Header file for INDEMIND camera data processing.
  • imrsdk.h: Header file of INDEMIND Camera SDK.
  • logging.h: Header file for logging.
  • times.h: Header file for time handling.
  • types.h: header file for type definitions.
  • util_pcl.h: Header file for utility functions of PCL (Point Cloud Library).
  • queue: The header file of the queue container.
  • mutex: header file for mutex.
  • cv::Mat: The matrix class in the OpenCV library.
  • using namespace indem;: Use the indem namespace.

(2) A template function called clear is defined to clear the queue.

(3) The main function starts:

  • Create an instance m_pSDK of the CIMRSDK class, which is the main interface of the IMSEEE-SDK.
  • Define and initialize a MRCONFIG structure variable config, which is used to configure camera parameters, including whether to enable SLAM, image resolution, image frame rate, and IMU frequency.
  • Call m_pSDK->Init(config) to initialize the camera.
    Create a queue points_queue of std::queuecv::Mat type for storing point cloud data, and a mutex lock mutex_points for protecting queue operations.
  • Define and initialize an integer variable points_count to record the number of received point clouds.
  • If the point cloud processing function is successfully enabled (by calling m_pSDK->EnablePointProcessor()):
    ① Register a callback function, when new point cloud data is received, add it to the queue and increase the count of points_count.
  • Create an instance pcviewer of the PCViewer class to display the point cloud in real time.
  • Get the current time time_beg.
  • Enter the main loop and constantly check whether the point cloud queue is not empty:
    ① If the queue is not empty, use a mutex to lock the queue, then call the pcviewer.Update() method to update the displayed point cloud, and clear the queue.
    ② Check whether the ESC key or the 'q' key is pressed, and if so, exit the loop.
    ③ Check whether pcviewer has stopped, if so, exit the loop.
  • Release the camera resources and delete the m_pSDK instance.
  • Get the time time_end after the loop ends.
  • Calculates the time interval from start to end and converts to milliseconds.
  • Output log information including start time, end time, run time and number of point clouds received.
  • Returns 0, indicating that the program ended normally.

get_points.cppImported header files util_pcl.h, analyzed util_pcl.hcode

2. Analyze util_pcl.hthe source code

// 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) Include header files and namespaces

  • : Standard library header file for using smart pointers.
  • <opencv2/core/core.hpp>: The core header file of the OpenCV library.
  • <pcl/visualization/pcl_visualizer.h>: The visualization module header file of PCL (Point Cloud Library).

(2) Define a class named PCViewer

  • Public member functions:
    ①PCViewer(): Constructor, used to initialize the point cloud visualizer.
    ②~PCViewer(): Destructor, used to release point cloud visualizer resources.
    ③Update(const cv::Mat &xyz): Update point cloud data and convert it to PCL point cloud format for display.
    ④Update(pcl::PointCloudpcl::PointXYZ::ConstPtr pc): Update point cloud data and directly display the point cloud format of PCL.
    ⑤WasVisual() const: Check whether the point cloud visualizer has been successfully created.
    ⑥WasStopped() const: Check whether the point cloud visualization has stopped.
  • Private member function:
    ConvertMatToPointCloud(const cv::Mat &xyz, pcl::PointCloudpcl::PointXYZ::Ptr pc): Convert the point cloud data in the matrix format of OpenCV to the point cloud format of PCL.
  • Private member variable:
    std::shared_ptrpcl::visualization::PCLVisualizer viewer_: Smart pointer for PCL visualizer.

(3) Header file protection

  • #ifndef UTIL_PCL_H_ And #define UTIL_PCL_H_: The protection macro of the header file to prevent the repeated inclusion of the header file.
  • #pragma once: Another way to protect header files, ensuring that header files are only compiled once.
  • #endif: End protection of the header file.

This header file defines a simple point cloud visualization class PCViewer, which contains update and display functions of point cloud data, as well as corresponding auxiliary functions

3. Analyze util_pcl.hthe source code

// 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 function: This is an auxiliary function for creating point cloud visualization objects with custom colors. It creates a PCLVisualizer object and adds point cloud data to the view, setting the background color, point cloud rendering properties, and camera position.

(2) The constructor and destructor of the PCViewer class: the constructor initializes the viewer_ member variable to nullptr, and the destructor closes the PCLVisualizer and releases related resources when the object is destructed.

(3) Update(const cv::Mat &xyz) function: Create a pcl::PointCloudpcl::PointXYZ object according to the input 3D coordinate matrix, and call another overloaded Update function.

(4) Update(pcl::PointCloudpcl::PointXYZ::ConstPtr pc) function: If viewer_ is empty, create a PCLVisualizer object and assign it to viewer_. Then update the point cloud data in the view and call spinOnce to refresh the visualization.

(5) WasVisual() function: Check whether viewer_ exists (non-empty) to determine whether the PCLVisualizer object has been successfully created.

(6) WasStopped() function: Check whether viewer_ exists and whether the view has stopped (the user closed the visualization window).

(7) ConvertMatToPointCloud(const cv::Mat &xyz, pcl::PointCloudpcl::PointXYZ::Ptr pc) function: According to the input 3D coordinate matrix, convert valid points into pcl::PointXYZ objects and add them to the point cloud object. Finally, set the width and height of the point cloud object.

2. Save the point cloud image to the local

If you want to save the point cloud data stored in the queue as a point cloud graphics file (such as PCD, PLY, etc.), you can take out the point cloud data in the queue at an appropriate time and use the PCL library to save it locally

1. Add the declaration of the ConvertMatToPointCloud and SavePointCloudToFile functions in the get_points.cpp file so that they can be correctly identified when used

// 保存点云图到本地
      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. The SavePointCloudToFile function is a private member function and cannot be directly accessed in the main function. We need to change it to a public member function to be called in the main function. Modify the PCViewer class in the util_pcl.h file as follows

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

3. Implement the SavePointCloudToFile function of the PCViewer class in util_pcl.cpp to compile and link successfully. Please add the following function implementation in util_pcl.cpp according to the code example provided before

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

3. Improved code

1. Improved 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. Improved 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. Improved 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;
}

insert image description here

Guess you like

Origin blog.csdn.net/Prototype___/article/details/131392711
Recommended
The “35-year-old curse” of Chinese coders
蘭雅 CorelDRAW 插件 2024.5.1 国际劳动节版，免费下载
Ranking
Methods and Practices of Manufacturing Data Quality Improvement
Serial port upgrade program for efficient transmission of large firmware
Use KITTI to run LIOSAM and complete the EVO evaluation
Calling methods on string literals (Java)
ActiveMQ and Spring integration (4)
You have to know the HTTPS! ! !
PAT whether Structures and Algorithms 7-4 with a binary search tree (40 rows streamlined structure)
el-upload brings request background
UVA - 1204 Fun Game-like pressure dp
2019-12-28
Daily
More
2024-05-04(18)
2024-05-03(8)
2024-05-02(0)
2024-05-01(4)
2024-04-30(36)
2024-04-29(5)
2024-04-28(12)
2024-04-27(29)
2024-04-26(22)
2024-04-25(32)

Code World

© 2018 codetd.com