15 #ifndef VISIONTRANSFER_RECONSTRUCT3D_PCL_H 16 #define VISIONTRANSFER_RECONSTRUCT3D_PCL_H 18 #ifdef PCL_MAJOR_VERSION 28 typename pcl::PointCloud<T>::Ptr Reconstruct3D::initPointCloud(
const ImageSet& imageSet,
const char* frameId) {
30 imageSet.getTimestamp(sec, microsec);
32 typename pcl::PointCloud<T>::Ptr ret(
33 new pcl::PointCloud<T>(imageSet.getWidth(), imageSet.getHeight()));
35 ret->header.frame_id = frameId;
36 ret->header.seq = imageSet.getSequenceNumber();
37 ret->header.stamp = sec * 1000000LL + microsec;
38 ret->width = imageSet.getWidth();
39 ret->height = imageSet.getHeight();
45 const char* frameId,
unsigned short minDisparity) {
47 pcl::PointCloud<pcl::PointXYZ>::Ptr ret = initPointCloud<pcl::PointXYZ>(imageSet, frameId);
48 memcpy(&ret->points[0].x, pointMap, ret->width*ret->height*
sizeof(
float)*4);
53 const char* frameId,
unsigned short minDisparity) {
55 pcl::PointCloud<pcl::PointXYZI>::Ptr ret = initPointCloud<pcl::PointXYZI>(imageSet, frameId);
57 pcl::PointXYZI* dstPtr = &ret->points[0];
59 for(
int y = 0; y < imageSet.getHeight(); y++) {
60 unsigned char* rowPtr = imageSet.getPixelData(0) + y*imageSet.getRowStride(0);
61 unsigned char* endPtr = rowPtr + imageSet.getWidth();
62 for(; rowPtr < endPtr; rowPtr++) {
63 dstPtr->intensity =
static_cast<float>(*rowPtr)/255.0F;
64 dstPtr->x = *pointMap++;
65 dstPtr->y = *pointMap++;
66 dstPtr->z = *pointMap++;
73 for(
int y = 0; y < imageSet.getHeight(); y++) {
74 unsigned short* rowPtr =
reinterpret_cast<unsigned short*
>(imageSet.getPixelData(0) + y*imageSet.getRowStride(0));
75 unsigned short* endPtr = rowPtr + imageSet.getWidth();
76 for(; rowPtr < endPtr; rowPtr++) {
77 dstPtr->intensity =
static_cast<float>(*rowPtr)/4095.0F;
78 dstPtr->x = *pointMap++;
79 dstPtr->y = *pointMap++;
80 dstPtr->z = *pointMap++;
87 throw std::runtime_error(
"Left image does not have a valid greyscale format");
94 const char* frameId,
unsigned short minDisparity) {
96 pcl::PointCloud<pcl::PointXYZRGB>::Ptr ret = initPointCloud<pcl::PointXYZRGB>(imageSet, frameId);
98 pcl::PointXYZRGB* dstPtr = &ret->points[0];
100 throw std::runtime_error(
"Left image is not an RGB image");
103 for(
int y = 0; y < imageSet.getHeight(); y++) {
104 unsigned char* rowPtr = imageSet.getPixelData(0) + y*imageSet.getRowStride(0);
105 unsigned char* endPtr = rowPtr + 3*imageSet.getWidth();
106 for(; rowPtr < endPtr;rowPtr +=3) {
107 dstPtr->r = rowPtr[0];
108 dstPtr->g = rowPtr[1];
109 dstPtr->b = rowPtr[2];
110 dstPtr->x = *pointMap++;
111 dstPtr->y = *pointMap++;
112 dstPtr->z = *pointMap++;
pcl::PointCloud< pcl::PointXYZ >::Ptr createXYZCloud(const ImageSet &imageSet, const char *frameId, unsigned short minDisparity=0)
Projects the given disparity map to a PCL point cloud without pixel intensities.
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createXYZRGBCloud(const ImageSet &imageSet, const char *frameId, unsigned short minDisparity=0)
Projects the given disparity map to a PCL point cloud, including pixel RGB data.
A set of one to three images, but usually two (the left camera image and the disparity map)...
pcl::PointCloud< pcl::PointXYZI >::Ptr createXYZICloud(const ImageSet &imageSet, const char *frameId, unsigned short minDisparity=0)
Projects the given disparity map to a PCL point cloud, including pixel intensities.
float * createPointMap(const unsigned short *dispMap, int width, int height, int rowStride, const float *q, unsigned short minDisparity=1)
Reconstructs the 3D location of each pixel in the given disparity map.