28 #include <opencv2/opencv.hpp>
30 #include <SimoxUtility/algorithm/string/string_tools.h>
45 #include <Calibration/Calibration.h>
46 #include <Image/ByteImage.h>
47 #include <Image/ImageProcessor.h>
55 FakePointCloudProviderPropertyDefinitions::FakePointCloudProviderPropertyDefinitions(
59 defineOptionalProperty<std::string>(
61 "VisionX/examples/point_clouds/",
62 "The point cloud to read. If directory, all *.pcd files of this and all subdirectories "
64 "All point clouds are read in before the pointcloud providing starts.");
66 defineOptionalProperty<bool>(
"rewind",
true,
"loop through the point clouds");
67 defineOptionalProperty<float>(
68 "scaleFactor", -1.0f,
"scale point cloud. only applied if value is larger than zero");
69 defineOptionalProperty<Eigen::Vector2i>(
"dimensions", Eigen::Vector2i(640, 480),
"")
70 .map(
"320x240", Eigen::Vector2i(320, 240))
71 .map(
"640x480", Eigen::Vector2i(640, 480))
72 .map(
"320x240", Eigen::Vector2i(320, 240))
73 .map(
"640x480", Eigen::Vector2i(640, 480))
74 .map(
"800x600", Eigen::Vector2i(800, 600))
75 .map(
"768x576", Eigen::Vector2i(768, 576))
76 .map(
"1024x768", Eigen::Vector2i(1024, 768))
77 .map(
"1024x1024", Eigen::Vector2i(1024, 1024))
78 .map(
"1280x960", Eigen::Vector2i(1280, 960))
79 .map(
"1600x1200", Eigen::Vector2i(1600, 1200));
81 defineOptionalProperty<std::string>(
82 "depthCameraCalibrationFile",
"",
"Camera depth calibration file (YAML)");
83 defineOptionalProperty<std::string>(
84 "RGBCameraCalibrationFile",
"",
"Camera RGB calibration file (YAML)");
86 defineOptionalProperty<std::string>(
87 "sourceFrameName",
"",
"Assume point cloud was stored in this frame.");
88 defineOptionalProperty<std::string>(
91 "Coordinate frame in which point cloud will be transformed and provided.\n"
92 "If left empty, no transformation will be applied and source frame name\n"
93 "will be returned as reference frame.");
95 defineOptionalProperty<std::string>(
"RobotStateComponentName",
96 "RobotStateComponent",
97 "Name of the robot state component to use.");
98 defineOptionalProperty<bool>(
"RemoveNAN",
true,
"Remove NAN from point cloud");
99 defineOptionalProperty<std::string>(
100 "ProvidedPointCloudFormat",
102 "Format of the provided point cloud (XYZRGBA, XYZL, XYZRGBL)");
108 return "FakePointCloudProvider";
150 getProperty(pointCloudFileName,
"pointCloudFileName");
156 getProperty(providedPointCloudFormat,
"ProvidedPointCloudFormat");
157 ARMARX_INFO <<
"Providing point clouds in format " << providedPointCloudFormat <<
".";
159 getProperty(robotStateComponentName,
"RobotStateComponentName");
160 if (!robotStateComponentName.empty())
166 if (!(sourceFrameName.empty() && targetFrameName.empty()))
168 if (!sourceFrameName.empty() && !targetFrameName.empty() &&
169 robotStateComponentName.empty())
171 ARMARX_ERROR <<
"Source and target frames specified, but no RobotStateComponent is "
173 "No transformation will be applied.";
175 else if (sourceFrameName.empty() && !targetFrameName.empty())
177 ARMARX_WARNING <<
"Target frame was specified (" << targetFrameName
178 <<
"), but no source frame is specified.\n"
179 <<
"Did you set the property `sourceFrameName`? (No transformation "
182 else if (!sourceFrameName.empty() && targetFrameName.empty())
184 ARMARX_INFO <<
"Source frame was specified (" << sourceFrameName
185 <<
"), but no target frame is specified.\n"
186 <<
"No transformation will be applied and source frame is returned as "
192 if (!ArmarXDataPath::getAbsolutePath(pointCloudFileName, pointCloudFileName))
194 ARMARX_ERROR <<
"Could not find point cloud file in ArmarXDataPath: "
195 << pointCloudFileName;
199 if (std::filesystem::is_directory(pointCloudFileName))
201 loadPointCloudDirectory(pointCloudFileName);
205 loadPointCloud(pointCloudFileName);
208 if (pointClouds.empty())
210 ARMARX_FATAL <<
"Unable to load point cloud: " << pointCloudFileName;
211 throw LocalException(
"Unable to load point cloud");
214 ARMARX_INFO <<
"Loaded " << pointClouds.size() <<
" point clouds.";
217 visionx::CameraParameters RGBCameraIntrinsics;
218 RGBCameraIntrinsics.focalLength.resize(2);
219 RGBCameraIntrinsics.principalPoint.resize(2);
221 if (!getProperty<std::string>(
"RGBCameraCalibrationFile").getValue().
empty() &&
222 !loadCalibrationFile(getProperty<std::string>(
"RGBCameraCalibrationFile").getValue(),
223 RGBCameraIntrinsics))
226 << getProperty<std::string>(
"RGBCameraCalibrationFile").getValue()
227 <<
" - using camera uncalibrated";
230 visionx::CameraParameters depthCameraIntrinsics;
231 depthCameraIntrinsics.focalLength.resize(2);
232 depthCameraIntrinsics.principalPoint.resize(2);
234 if (!getProperty<std::string>(
"depthCameraCalibrationFile").getValue().
empty() &&
235 !loadCalibrationFile(getProperty<std::string>(
"depthCameraCalibrationFile").getValue(),
236 depthCameraIntrinsics))
239 << getProperty<std::string>(
"depthCameraCalibrationFile").getValue()
240 <<
" - using camera uncalibrated";
245 calibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
246 calibration.calibrationRight.cameraParam = depthCameraIntrinsics;
250 FakePointCloudProvider::loadPointCloud(
const std::string& fileName)
254 pcl::PCLPointCloud2Ptr cloudptr(
new pcl::PCLPointCloud2());
257 if (io::loadPCDFile(fileName.c_str(), *cloudptr) == -1)
259 ARMARX_WARNING <<
"Unable to load point cloud from file " << fileName;
264 pointClouds.push_back(cloudptr);
273 FakePointCloudProvider::loadPointCloudDirectory(
const std::string& directoryName)
276 std::vector<std::string> fileNames;
278 for (std::filesystem::recursive_directory_iterator dir(directoryName), end;
279 dir != end &&
getState() < eManagedIceObjectExiting;
283 if (dir->path().extension() ==
".pcd")
285 fileNames.push_back(dir->path().string());
289 ARMARX_INFO <<
"In total " << fileNames.size() <<
" point clouds found.";
292 std::sort(fileNames.begin(), fileNames.end());
294 for (std::size_t f = 0; f < fileNames.size(); f++)
296 ARMARX_INFO <<
" File " << f <<
" / " << fileNames.size() <<
" is being loaded... "
300 loadPointCloud(fileNames[f]);
318 MetaPointCloudFormatPtr
321 MetaPointCloudFormatPtr format =
new MetaPointCloudFormat();
323 if (providedPointCloudFormat ==
"XYZL")
325 format->type = PointContentType::eLabeledPoints;
327 else if (providedPointCloudFormat ==
"XYZRGBL")
329 format->type = PointContentType::eColoredLabeledPoints;
333 format->type = PointContentType::eColoredPoints;
338 format->size = format->capacity;
345 Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>(
"dimensions");
346 setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
348 rgbImages =
new CByteImage*[1];
349 rgbImages[0] =
new CByteImage(dimensions(0), dimensions(1), CByteImage::eRGB24);
350 ::ImageProcessor::Zero(rgbImages[0]);
359 if (!robotStateComponentName.empty())
361 getProxy(robotStateComponent, robotStateComponentName);
362 if (!robotStateComponent)
364 ARMARX_ERROR <<
"Failed to obtain robot state component.";
380 pcl::PCLPointCloud2Ptr cloudptr;
383 std::scoped_lock lock(pointCloudMutex);
385 if (rewind &&
static_cast<size_t>(counter.load()) >= pointClouds.size())
390 cloudptr = pointClouds.at(counter);
394 if (providedPointCloudFormat ==
"XYZRGBA")
396 return processAndProvide<pcl::PointXYZRGBA>(cloudptr);
398 else if (providedPointCloudFormat ==
"XYZL")
400 return processAndProvide<pcl::PointXYZL>(cloudptr);
402 else if (providedPointCloudFormat ==
"XYZRGBL")
404 return processAndProvide<pcl::PointXYZRGBL>(cloudptr);
408 ARMARX_ERROR <<
"Could not provide point cloud, because format '"
409 << providedPointCloudFormat <<
"' is unknown";
415 FakePointCloudProvider::loadCalibrationFile(std::string fileName,
416 visionx::CameraParameters& calibration)
418 ArmarXDataPath::getAbsolutePath(fileName, fileName, {},
false);
419 cv::FileStorage fs(fileName, cv::FileStorage::READ);
426 std::string cameraName = fs[
"camera_name"];
427 ARMARX_LOG <<
"loading calibration file for " << cameraName;
430 Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>(
"dimensions").getValue();
431 int imageWidth = fs[
"image_width"];
432 int imageHeight = fs[
"image_height"];
434 if (imageWidth != dimensions(0) || imageHeight != dimensions(1))
440 cv::Mat cameraMatrix, distCoeffs;
442 fs[
"camera_matrix"] >> cameraMatrix;
443 fs[
"distortion_coefficients:"] >> distCoeffs;
445 for (
int i = 0; i < 5; i++)
447 calibration.distortion[
static_cast<std::size_t
>(i)] = distCoeffs.at<
float>(0, i);
450 calibration.focalLength[0] = cameraMatrix.at<
float>(0, 0);
451 calibration.focalLength[1] = cameraMatrix.at<
float>(1, 1);
453 calibration.principalPoint[0] = cameraMatrix.at<
float>(2, 1);
454 calibration.principalPoint[1] = cameraMatrix.at<
float>(2, 2);
469 std::scoped_lock lock(pointCloudMutex);
483 template <
typename Po
intT>
485 FakePointCloudProvider::processAndProvide(
const PCLPointCloud2Ptr& pointCloud)
487 using PointCloudPtrT =
typename pcl::PointCloud<PointT>::Ptr;
489 PointCloudPtrT cloudptr(
new pcl::PointCloud<PointT>());
490 pcl::fromPCLPointCloud2(*pointCloud, *cloudptr);
492 if (cloudptr->isOrganized())
494 processRGBImage(pointCloud);
497 PointCloudPtrT resultCloudPtr(
new pcl::PointCloud<PointT>());
501 Eigen::Affine3f
transform(Eigen::Scaling(scaleFactor));
502 pcl::transformPointCloud(*cloudptr, *resultCloudPtr,
transform);
507 pcl::copyPointCloud(*cloudptr, *resultCloudPtr);
511 resultCloudPtr->header.stamp =
static_cast<uint64_t
>(time.toMicroSeconds());
513 if (!sourceFrameName.empty() || !targetFrameName.empty())
515 if (!sourceFrameName.empty() && !targetFrameName.empty())
521 localRobot, robotStateComponent, time.toMicroSeconds());
523 framedPointCloud.
changeFrame(targetFrameName, localRobot);
532 else if (sourceFrameName.empty())
538 else if (targetFrameName.empty())
550 pcl::removeNaNFromPointCloud(*resultCloudPtr, *resultCloudPtr,
indices);
554 << resultCloudPtr->points.size();
560 counter = freezeImageIdx.load();
570 FakePointCloudProvider::processRGBImage(
const PCLPointCloud2Ptr& pointCloud)
572 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudptr(
new pcl::PointCloud<pcl::PointXYZRGB>());
573 pcl::fromPCLPointCloud2(*pointCloud, *cloudptr);
575 if (!cloudptr->isOrganized())
578 <<
"The point cloud is not organized, so no RGB image is generated";
582 if (
static_cast<int>(cloudptr->width) != rgbImages[0]->width ||
583 static_cast<int>(cloudptr->height) != rgbImages[0]->height)
586 <<
"Dimensions of point cloud and image do not match, so no RGB image is generated";
590 ::ImageProcessor::Zero(rgbImages[0]);
591 const int width =
static_cast<int>(cloudptr->width);
592 const int height =
static_cast<int>(cloudptr->height);
594 for (
int i = 0; i < height; i++)
596 for (
int j = 0; j < width; j++)
598 pcl::PointXYZRGB& point = cloudptr->at(
static_cast<std::size_t
>(i * width + j));
599 rgbImages[0]->pixels[3 * (i * width + j) + 0] = point.r;
600 rgbImages[0]->pixels[3 * (i * width + j) + 1] = point.g;
601 rgbImages[0]->pixels[3 * (i * width + j) + 2] = point.b;
611 for (std::size_t i = 0; i < 1; i++)
634 return !targetFrameName.empty() ? targetFrameName : sourceFrameName;
638 FakePointCloudProvider::buildGui()
666 prx.
getValue(removeNAN,
"removeNAN");
667 prx.
getValue(scaleFactor,
"scaleFactor");
668 prx.
getValue(freezeImage,
"freezeImage");
671 prx.
getValue(freezeImageIdx,
"freezeImageIdx");
675 prx.
setValue(counter,
"freezeImageIdx");