1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
|
/**
* @file pcl_ensenso_cloud_images_viewer.cpp
* @brief Display both Ensenso images & cloud using the PCL Ensenso grabber (and OpenCV)
* @author Victor Lamoine
* @date November 2014
*/
#include <algorithm>
#include <iostream>
#include <thread>
#include <vector>
#include <pcl/common/common.h>
#include <pcl/memory.h>
#include <pcl/console/print.h>
#include <pcl/io/ensenso_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace std::chrono_literals;
/** @brief Convenience typedef */
typedef pcl::PointXYZ PointT;
/** @brief Convenience typedef */
typedef pcl::PointCloud<PointT> PointCloudT;
/** @brief Convenience typedef for the Ensenso grabber callback */
typedef std::pair<pcl::PCLImage, pcl::PCLImage> PairOfImages;
typedef pcl::shared_ptr<PairOfImages> PairOfImagesPtr;
/** @brief CloudViewer pointer */
pcl::visualization::CloudViewer::Ptr viewer_ptr;
/** @brief Pair of Ensenso images */
pcl::EnsensoGrabber::Ptr ensenso_ptr;
/** @brief Get OpenCV image type corresponding to the parameters given
* @param channels number of channels in the image
* @param bpe bytes per element
* @param isFlt is float
* @returns the OpenCV type
*/
int
getOpenCVType (const std::string &type)
{
if (type == "CV_32FC1")
return CV_32FC1;
else if (type == "CV_32FC2")
return CV_32FC2;
else if (type == "CV_32FC3")
return CV_32FC3;
else if (type == "CV_32FC4")
return CV_32FC4;
else if (type == "CV_64FC1")
return CV_64FC1;
else if (type == "CV_64FC2")
return CV_64FC2;
else if (type == "CV_64FC3")
return CV_64FC3;
else if (type == "CV_64FC4")
return CV_64FC4;
else if (type == "CV_8UC1")
return CV_8UC1;
else if (type == "CV_8UC2")
return CV_8UC2;
else if (type == "CV_8UC3")
return CV_8UC3;
else if (type == "CV_8UC4")
return CV_8UC4;
else if (type == "CV_16UC1")
return CV_16UC1;
else if (type == "CV_16UC2")
return CV_16UC2;
else if (type == "CV_16UC3")
return CV_16UC3;
else if (type == "CV_16UC4")
return CV_16UC4;
else if (type == "CV_32SC1")
return CV_32SC1;
else if (type == "CV_32SC2")
return CV_32SC2;
else if (type == "CV_32SC3")
return CV_32SC3;
else if (type == "CV_32SC4")
return CV_32SC4;
return (-1);
}
/** @brief Process and/or display Ensenso grabber images
* @param[in] cloud The Ensenso point cloud
* @param[in] images Pair of Ensenso images (raw or with overlay)
* @warning Image type changes if a calibration pattern is discovered/lost;
* check @c images->first.encoding */
void
grabberCallback (const PointCloudT::Ptr& cloud,
const PairOfImagesPtr& images)
{
viewer_ptr->showCloud (cloud);
unsigned char *l_image_array = reinterpret_cast<unsigned char *> (&images->first.data[0]);
unsigned char *r_image_array = reinterpret_cast<unsigned char *> (&images->second.data[0]);
std::cout << "Encoding: " << images->first.encoding << std::endl;
int type = getOpenCVType (images->first.encoding);
cv::Mat l_image (images->first.height, images->first.width, type, l_image_array);
cv::Mat r_image (images->first.height, images->first.width, type, r_image_array);
cv::Mat im (images->first.height, images->first.width * 2, type);
im.adjustROI (0, 0, 0, -images->first.width);
l_image.copyTo (im);
im.adjustROI (0, 0, -images->first.width, images->first.width);
r_image.copyTo (im);
im.adjustROI (0, 0, images->first.width, 0);
cv::imshow ("Ensenso images", im);
cv::waitKey (10);
}
/** @brief Main function
* @return Exit status */
int
main (void)
{
viewer_ptr.reset (new pcl::visualization::CloudViewer ("3D Viewer"));
ensenso_ptr.reset (new pcl::EnsensoGrabber);
ensenso_ptr->openDevice (0);
ensenso_ptr->openTcpPort ();
//ensenso_ptr->initExtrinsicCalibration (5); // Disable projector if you want good looking images.
// You won't be able to detect a calibration pattern with the projector enabled!
std::function<void (const PointCloudT::Ptr&, const PairOfImagesPtr&)> f = grabberCallback;
ensenso_ptr->registerCallback (f);
cv::namedWindow ("Ensenso images", cv::WINDOW_AUTOSIZE);
ensenso_ptr->start ();
while (!viewer_ptr->wasStopped ())
{
PCL_INFO("FPS: %f\n", ensenso_ptr->getFramesPerSecond ());
std::this_thread::sleep_for(500ms);
}
ensenso_ptr->stop ();
//cv::destroyAllWindows (); // Doesn't work
ensenso_ptr->closeDevice ();
ensenso_ptr->closeTcpPort ();
return 0;
}
|
