C 点云示例代码-Point Cloud
# 点云示例-PointCloud
功能描述：连接设备开流，生成深度点云或RGBD点云并保存成ply格式文件，并通过ESC_KEY键退出程序
> 本示例基于C++ High Level API进行演示
创建点云保存成ply格式文件函数，ply文件格式详细描述可在网络上查看
//保存点云数据到ply
void save_points_to_ply( ob_frame* frame, const char* fileName ) {
int pointsSize = ob_frame_data_size( frame, &error ) / sizeof( ob_point );
check_error( error );
FILE* fp = fopen( fileName, "wb+" );
fprintf( fp, "ply\n" );
fprintf( fp, "format ascii 1.0\n" );
fprintf( fp, "element vertex %d\n", pointsSize );
fprintf( fp, "property float x\n" );
fprintf( fp, "property float y\n" );
fprintf( fp, "property float z\n" );
fprintf( fp, "end_header\n" );
ob_point* point = ( ob_point* )ob_frame_data( frame, &error );
check_error( error );
for ( int i = 0; i < pointsSize; i++ ) {
fprintf( fp, "%.3f %.3f %.3f\n", point->x, point->y, point->z );
point++;
}
fflush( fp );
fclose( fp );
}
//保存彩色点云数据到ply
void save_rgb_points_to_ply( ob_frame* frame, const char* fileName ) {
int pointsSize = ob_frame_data_size( frame, &error ) / sizeof( ob_color_point );
check_error( error );
FILE* fp = fopen( fileName, "wb+" );
fprintf( fp, "ply\n" );
fprintf( fp, "format ascii 1.0\n" );
fprintf( fp, "element vertex %d\n", pointsSize );
fprintf( fp, "property float x\n" );
fprintf( fp, "property float y\n" );
fprintf( fp, "property float z\n" );
fprintf( fp, "property uchar red\n" );
fprintf( fp, "property uchar green\n" );
fprintf( fp, "property uchar blue\n" );
fprintf( fp, "end_header\n" );
ob_color_point* point = ( ob_color_point* )ob_frame_data( frame, &error );
check_error( error );
for ( int i = 0; i < pointsSize; i++ ) {
fprintf( fp, "%.3f %.3f %.3f %d %d %d\n", point->x, point->y, point->z, ( int )point->r, ( int )point->g, ( int )point->b );
point++;
}
fflush( fp );
fclose( fp );
}
创建Pipeline及流配置
//创建pipeline 用于连接设备后打开Color和Depth流
pipeline = ob_create_pipeline( &error );
//创建config，用于配置 Color 和 Depth 流的 分辨率、帧率、格式
ob_config* config = ob_create_config( &error );
//配置Color流
ob_stream_profile_list *colorProfiles = ob_pipeline_get_stream_profile_list(pipeline, OB_SENSOR_COLOR, &error);
if(error) {
printf("Current device is not support color sensor!\n");
ob_delete_error(error);
error = NULL;
// Turn on D2C alignment, which needs to be turned on when generating RGBD point clouds
ob_config_set_align_mode(config, ALIGN_DISABLE, &error);
check_error(error);
}
// Open the default profile of Color Sensor, which can be configured through the configuration file
if(colorProfiles) {
color_profile = ob_stream_profile_list_get_profile(colorProfiles, OB_PROFILE_DEFAULT, &error);
}
//配置Depth流
if(color_profile) {
// Try find supported depth to color align hardware mode profile
depthProfiles = ob_get_d2c_depth_profile_list(pipeline, color_profile, ALIGN_D2C_HW_MODE, &error);
check_error(error);
int d2cCount = ob_stream_profile_list_count(depthProfiles, &error);
check_error(error);
if(d2cCount > 0) {
align_mode = ALIGN_D2C_HW_MODE;
}
else {
// Try find supported depth to color align software mode profile
depthProfiles = ob_get_d2c_depth_profile_list(pipeline, color_profile, ALIGN_D2C_SW_MODE, &error);
check_error(error);
d2cCount = ob_stream_profile_list_count(depthProfiles, &error);
check_error(error);
if(d2cCount > 0) {
align_mode = ALIGN_D2C_SW_MODE;
}
}
}
else {
depthProfiles = ob_pipeline_get_stream_profile_list(pipeline, OB_SENSOR_DEPTH, &error);
check_error(error);
打开设备D2C功能
// Turn on D2C alignment, which needs to be turned on when generating RGBD point clouds
ob_config_set_align_mode(config, align_mode, &error)
开流
ob_pipeline_start_with_config(pipeline, config, &error);
在开流后创建点云filter。点云filter用于将depth和color帧数据转换成点云数据。点云filter在开流后创建目的是让SDK内部自动根据当前开流配置设置好相机参数。当然也可以自行设置。
// Create a point cloud Filter object (device parameters will be obtained inside the Pipeline when the point cloud filter is created, so try to configure
// the device before creating the filter)
ob_filter *point_cloud = ob_create_pointcloud_filter(&error);
check_error(error);
// Obtain the current open-stream camera parameters from the pipeline and pass them to the point cloud filter
ob_camera_param camera_param = ob_pipeline_get_camera_param(pipeline, &error);
check_error(error);
ob_pointcloud_filter_set_camera_param(point_cloud, camera_param, &error);
check_error(error);
启动主循环，循环内根据用户按键调用点云filter生成深度点云或RGBD点云数据，并保存成ply文件。
//等待一帧数据，超时时间为100ms
ob_frame *frameset = ob_pipeline_wait_for_frameset(pipeline, 100, &error);
check_error(error);
if(frameset != NULL) {
// get depth value scale
ob_frame *depth_frame = ob_frameset_depth_frame(frameset, &error);
check_error(error);
if(depth_frame == NULL) {
continue;
}
// get depth value scale
float depth_value_scale = ob_depth_frame_get_value_scale(depth_frame, &error);
check_error(error);
// delete depth frame
ob_delete_frame(depth_frame, &error);
check_error(error);
// point position value multiply depth value scale to convert uint to millimeter (for some devices, the default depth value uint is not
// millimeter)
ob_pointcloud_filter_set_position_data_scale(point_cloud, depth_value_scale, &error);
check_error(error);
ob_pointcloud_filter_set_point_format(point_cloud, OB_FORMAT_RGB_POINT, &error);
check_error(error);
ob_frame *pointsFrame = ob_filter_process(point_cloud, frameset, &error);
check_error(error);
if(pointsFrame != NULL) {
save_rgb_points_to_ply(pointsFrame, "rgb_points.ply");
printf("rgb_points.ply Saved\n");
ob_delete_frame(pointsFrame, &error);
check_error(error);
points_created = true;
}
ob_delete_frame(frameset, &error); // Destroy frameSet to reclaim memory
check_error(error);
if(points_created) {
break;
}
}
主循环退出后，停流并销毁回收资源
// stop pipeline
ob_pipeline_stop(pipeline, &error);
check_error(error);
// destroy pipeline
ob_delete_pipeline(pipeline, &error);
check_error(error);
// destroy config
ob_delete_config(config, &error);
check_error(error);
// destroy profile
ob_delete_stream_profile(depth_profile, &error);
check_error(error);
// destroy profile
ob_delete_stream_profile(color_profile, &error);
check_error(error);
// destroy profile list
ob_delete_stream_profile_list(colorProfiles, &error);
check_error(error);
ob_delete_stream_profile_list(depthProfiles, &error);
check_error(error);
预期输出：
