源代码忘了从哪抄的了，应该有小修改。
D435i能运行。
/***********************************************************
* Author: Daniel Tran
* Liam Gogley
*
* Purpose: The following .cpp file will utilize the Intel
* realsense camera to stream and capture frame
* data of the environment. Color is then applied
* and a point cloud is generated and saved to
* a point cloud data format (.pcd).
* 用途：以下.cpp文件将使用英特尔
* realsense摄像头，用于流式传输和捕获帧
* 环境数据。然后应用颜色
* 并生成一个点云并保存到
* 点云数据格式（.pcd）。
*
* Version 0.09 - Last Editted 11/07/18
*
* Rev: Implementation of RGB Texture function to map
* color to point cloud data.
*
***********************************************************/
#include <iostream>
#include <algorithm>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
#include <string>
// Intel Realsense Headers
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
// PCL Headers
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <boost/thread/thread.hpp>
#include <pcl/io/io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/pcl_macros.h>
#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"
using namespace std;
typedef pcl::PointXYZRGB RGB_Cloud;
typedef pcl::PointCloud<RGB_Cloud> point_cloud;
typedef point_cloud::Ptr cloud_pointer;
typedef point_cloud::Ptr prevCloud;
// Prototypes
void Load_PCDFile(void);
bool userInput(void);
void cloudViewer(void);
// Global Variables
string cloudFile; // .pcd file name文件名
string prevCloudFile; // .pcd file name (Old cloud)之前的文件名
int i = 1; // Index for incremental file name增量文件名的索引
//======================================================
// RGB Texture
// - Function is utilized to extract the RGB data from
// a single point return R, G, and B values.
// Normals are stored as RGB components and
// correspond to the specific depth (XYZ) coordinate.
// By taking these normals and converting them to
// texture coordinates, the RGB components can be
// "mapped" to each individual point (XYZ).
//RGB_Texture
//-函数用于从中提取RGB数据
//单点返回R、G和B值。
//法线存储为RGB组件和
//对应于特定深度（XYZ）坐标。
//通过获取这些法线并将其转换为
//纹理坐标，RGB组件可以
//“映射”到每个单独的点（XYZ）。
//======================================================
std::tuple<int, int, int> RGB_Texture(rs2::video_frame texture, rs2::texture_coordinate Texture_XY)
{
// Get Width and Height coordinates of texture获取纹理的宽度和高度坐标
int width = texture.get_width(); // Frame width in pixels帧宽（像素）
int height = texture.get_height(); // Frame height in pixels帧高度（像素）
// Normals to Texture Coordinates conversion法线到纹理坐标的转换
int x_value = min(max(int(Texture_XY.u * width + .5f), 0), width - 1);
int y_value = min(max(int(Texture_XY.v * height + .5f), 0), height - 1);
int bytes = x_value * texture.get_bytes_per_pixel(); // Get # of bytes per pixel每像素获取#个字节
int strides = y_value * texture.get_stride_in_bytes(); // Get line width in bytes获取以字节为单位的线宽
int Text_Index = (bytes + strides);
const auto New_Texture = reinterpret_cast<const uint8_t*>(texture.get_data());
// RGB components to save in tuple要保存在元组中的RGB组件
int NT1 = New_Texture[Text_Index];
int NT2 = New_Texture[Text_Index + 1];
int NT3 = New_Texture[Text_Index + 2];
return std::tuple<int, int, int>(NT1, NT2, NT3);
}
//===================================================
// PCL_Conversion
// - Function is utilized to fill a point cloud
// object with depth and RGB data from a single
// frame captured using the Realsense.
// PCL_转换
// -函数用于填充点云
// 对象，该对象具有来自单个对象的深度和RGB数据
// 使用Realsense捕获的帧。
//===================================================
cloud_pointer PCL_Conversion(const rs2::points& points, const rs2::video_frame& color) {
// Object Declaration (Point Cloud)对象声明（点云）
cloud_pointer cloud(new point_cloud);
// Declare Tuple for RGB value Storage (<t0>, <t1>, <t2>)为RGB值存储声明元组（<t0>，<t1>，<t2>）
std::tuple<uint8_t, uint8_t, uint8_t> RGB_Color;
//================================
// PCL Cloud Object Configuration云对象配置
//================================
// Convert data captured from Realsense camera to Point Cloud将从Realsense相机捕获的数据转换为点云
auto sp = points.get_profile().as<rs2::video_stream_profile>();
cloud->width = static_cast<uint32_t>(sp.width());
cloud->height = static_cast<uint32_t>(sp.height());
cloud->is_dense = false;
cloud->points.resize(points.size());
auto Texture_Coord = points.get_texture_coordinates();
auto Vertex = points.get_vertices();
// Iterating through all points and setting XYZ coordinates遍历所有点并设置XYZ坐标
// and RGB values和RGB值
for (int i = 0; i < points.size(); i++)
{
//===================================
// Mapping Depth Coordinates测绘深度坐标
// - Depth data stored as XYZ values深度数据存储为XYZ值
//===================================
cloud->points[i].x = Vertex[i].x;
cloud->points[i].y = Vertex[i].y;
cloud->points[i].z = Vertex[i].z;
// Obtain color texture for specific point获取特定点的颜色纹理
RGB_Color = RGB_Texture(color, Texture_Coord[i]);
// Mapping Color (BGR due to Camera Model)贴图颜色（因相机型号而异的BGR）
cloud->points[i].r = get<0>(RGB_Color); // Reference tuple<2>
cloud->points[i].g = get<1>(RGB_Color); // Reference tuple<1>
cloud->points[i].b = get<2>(RGB_Color); // Reference tuple<0>
}
return cloud; // PCL RGB Point Cloud generated生成的PCL RGB点云
}
int main() try
{
//======================
// Variable Declaration变量声明
//======================
bool captureLoop = true; // Loop control for generating point clouds生成点云的循环控制
//====================
// Object Declaration对象声明
//====================
pcl::PointCloud<pcl::PointXYZRGB>::Ptr newCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
boost::shared_ptr<pcl::visualization::PCLVisualizer> openCloud;
// Declare pointcloud object, for calculating pointclouds and texture mappings声明点云对象，用于计算点云和纹理映射
rs2::pointcloud pc;
// Declare RealSense pipeline, encapsulating the actual device and sensors声明RealSense管道，封装实际设备和传感器
rs2::pipeline pipe;
// Create a configuration for configuring the pipeline with a non default profile创建配置以使用非默认配置文件配置管道
rs2::config cfg;
//======================
// Stream configuration with parameters resolved internally. See enable_stream() overloads for extended usage+
// 内部解析参数的流配置。有关扩展用法，请参见启用_stream（）重载
//======================
cfg.enable_stream(RS2_STREAM_COLOR);
cfg.enable_stream(RS2_STREAM_INFRARED);
cfg.enable_stream(RS2_STREAM_DEPTH);
rs2::pipeline_profile selection = pipe.start(cfg);
rs2::device selected_device = selection.get_device();
auto depth_sensor = selected_device.first<rs2::depth_sensor>();
if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED))
{
depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1.f); // Enable emitter启用发射器
pipe.wait_for_frames();
depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0.f); // Disable emitter禁用发射器
}
if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
{
// Query min and max values:查询最小值和最大值：
auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.max); // Set max power设置最大功率
Sleep(1);
depth_sensor.set_option(RS2_OPTION_LASER_POWER, 0.f); // Disable laser禁用激光
}
// Begin Stream with default configs使用默认配置开始流
// Loop and take frame captures upon user input在用户输入时循环并获取帧捕获
while (captureLoop == true) {
// Set loop flag based on user input根据用户输入设置循环标志
captureLoop = userInput();
if (captureLoop == false) { break; }
if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED))
{
depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1.f); // Enable emitter启用发射器
}
if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
{
// Query min and max values:查询最小值和最大值：
auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.max); // Set max power设置最大功率
}
// Wait for frames from the camera to settle等待来自摄影机的帧稳定下来
for (int i = 0; i < 30; i++) {
auto frames = pipe.wait_for_frames(); //Drop several frames for auto-exposure放下几帧进行自动曝光
}
// Capture a single frame and obtain depth + RGB values from it 捕获单个帧并从中获取深度+RGB值
auto frames = pipe.wait_for_frames();
auto depth = frames.get_depth_frame();
auto RGB = frames.get_color_frame();
std::string png_file;
png_file = "Captured_Frame_RGB" + to_string(i) + ".png";
stbi_write_png(png_file.c_str(), RGB.get_width(), RGB.get_height(),
RGB.get_bytes_per_pixel(), RGB.get_data(), RGB.get_stride_in_bytes());
std::cout << "Saved RGB" << png_file << std::endl;
png_file = "Captured_Frame_D" + to_string(i) + ".png";
stbi_write_png(png_file.c_str(), depth.get_width(), depth.get_height(),
depth.get_bytes_per_pixel(), depth.get_data(), depth.get_stride_in_bytes());
std::cout << "Saved RGB" << png_file << std::endl;
// Map Color texture to each point将颜色纹理映射到每个点
pc.map_to(RGB);
// Generate Point Cloud生成点云
auto points = pc.calculate(depth);
if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED))
{
depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0.f); // Disable emitter禁用发射器
}
if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
{
depth_sensor.set_option(RS2_OPTION_LASER_POWER, 0.f); // Disable laser禁用激光
}
// Convert generated Point Cloud to PCL Formatting将生成的点云转换为PCL格式
cloud_pointer cloud = PCL_Conversion(points, RGB);
//========================================
// Filter PointCloud (PassThrough Method)
// 过滤点云（通过方法）
//========================================
pcl::PassThrough<pcl::PointXYZRGB> Cloud_Filter; // Create the filtering object创建过滤对象
Cloud_Filter.setInputCloud(cloud); // Input generated cloud to filter输入生成的云进行过滤
Cloud_Filter.setFilterFieldName("z"); // Set field name to Z-coordinate将字段名称设置为Z坐标
Cloud_Filter.setFilterLimits(0.0, 1.0); // Set accepted interval values设置可接受的间隔值
Cloud_Filter.filter(*newCloud); // Filtered Cloud Outputted过滤云输出
cloudFile = "Captured_Frame" + to_string(i) + ".pcd";
//==============================
// Write PC to .pcd File Format 将PC写入.pcd文件格式
//==============================
// Take Cloud Data and write to .PCD File Format获取云数据并写入.PCD文件格式
cout << "Generating PCD Point Cloud File... " << endl;
pcl::io::savePCDFileASCII(cloudFile, *cloud); // Input cloud to be saved to .pcd
cout << cloudFile << " successfully generated. " << endl;
//Load generated PCD file for viewing加载生成的PCD文件以供查看
Load_PCDFile();
i++; // Increment File Name增量文件名
}//End-while结束循环
if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED))
{
depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1.f); // Enable emitter启用发射器
}
if (depth_sensor.supports(RS2_OPTION_LASER_POWER))
{
// Query min and max values:查询最小值和最大值：
auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.max); // Set max power设置最大功率
}
cout << "Exiting Program... " << endl;
return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
return EXIT_FAILURE;
}
catch (const std::exception & e)
{
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
void Load_PCDFile(void)
{
string openFileName;
// Generate object to store cloud in .pcd file生成对象以将云存储在.pcd文件中
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudView(new pcl::PointCloud<pcl::PointXYZRGB>);
openFileName = "Captured_Frame" + to_string(i) + ".pcd";
pcl::io::loadPCDFile(openFileName, *cloudView); // Load .pcd File 加载.pcd文件
//==========================
// Pointcloud Visualization点云可视化
//==========================
// Create viewer object titled "Captured Frame"创建标题为“捕获帧”的查看器对象
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Captured Frame"));
// Set background of viewer to black将查看器的背景设置为黑色
viewer->setBackgroundColor(0, 0, 0);
// Add generated point cloud and identify with string "Cloud"添加生成的点云并用字符串“cloud”标识
viewer->addPointCloud<pcl::PointXYZRGB>(cloudView, "Cloud");
// Default size for rendered points
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "Cloud");
// Viewer Properties
viewer->initCameraParameters(); // Camera Parameters for ease of viewing便于查看的摄像机参数
cout << endl;
cout << "Press [Q] in viewer to continue. " << endl;
viewer->spin(); // Allow user to rotate point cloud and view it允许用户旋转点云并查看它
// Note: No method to close PC visualizer, pressing Q to continue software flow only solution.注意：没有关闭PC visualizer的方法，按Q继续软件流仅解决方案。
}
//========================================
// userInput
// - Prompts user for a char to 提示用户输入要添加的字符
// test for decision making.test for decision making.
// [y|Y] - Capture frame and save as .pcd捕获帧并另存为.pcd
// [n|N] - Exit program退出程序
//========================================
bool userInput(void) {
bool setLoopFlag;
bool inputCheck = false;
char takeFrame; // Utilize to trigger frame capture from key press ('y')利用从按键（'y'）触发帧捕获
do {
// Prompt User to execute frame capture algorithm提示用户执行帧捕获算法
cout << endl;
cout << "Generate a Point Cloud? [y/n] ";
cin >> takeFrame;
cout << endl;
// Condition [Y] - Capture frame, store in PCL object and display条件[Y]-捕获帧，存储在PCL对象中并显示
if (takeFrame == 'y' || takeFrame == 'Y') {
setLoopFlag = true;
inputCheck = true;
takeFrame = 0;
}
// Condition [N] - Exit Loop and close program条件[N]-退出循环并关闭程序
else if (takeFrame == 'n' || takeFrame == 'N') {
setLoopFlag = false;
inputCheck = true;
takeFrame = 0;
}
// Invalid Input, prompt user again.输入无效，请再次提示用户。
else {
inputCheck = false;
cout << "Invalid Input." << endl;
takeFrame = 0;
}
} while (inputCheck == false);
return setLoopFlag;
}
5057
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
