1 #include "simulation_io.hpp"
2
3 #include <pcl/io/png_io.h>
4
SimExample(int argc,char ** argv,int height,int width)5 pcl::simulation::SimExample::SimExample(int argc, char** argv, int height, int width)
6 : height_(height), width_(width)
7 {
8
9 initializeGL(argc, argv);
10
11 // 1. construct member elements:
12 camera_ = Camera::Ptr(new Camera());
13 scene_ = Scene::Ptr(new Scene());
14
15 // rl_ = RangeLikelihoodGLSL::Ptr(new RangeLikelihoodGLSL(1, 1, height, width, scene_,
16 // 0));
17 rl_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, height, width, scene_));
18 // rl_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
19 // rl_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, height_, width_, scene_));
20
21 // Actually corresponds to default parameters:
22 rl_->setCameraIntrinsicsParameters(
23 width_, height_, 576.09757860, 576.09757860, 321.06398107, 242.97676897);
24 rl_->setComputeOnCPU(false);
25 rl_->setSumOnCPU(true);
26 rl_->setUseColor(true);
27
28 // 2. read mesh and setup model:
29 std::cout << "About to read: " << argv[2] << std::endl;
30 pcl::PolygonMesh mesh; // (new pcl::PolygonMesh);
31 pcl::io::loadPolygonFile(argv[2], mesh);
32 pcl::PolygonMesh::Ptr cloud(new pcl::PolygonMesh(mesh));
33
34 // Not sure if PolygonMesh assumes triangles if to, TODO: Ask a developer
35 PolygonMeshModel::Ptr model =
36 PolygonMeshModel::Ptr(new PolygonMeshModel(GL_POLYGON, cloud));
37 scene_->add(model);
38
39 std::cout << "Just read " << argv[2] << std::endl;
40 std::cout << mesh.polygons.size() << " polygons and " << mesh.cloud.data.size()
41 << " triangles\n";
42
43 // works well for MIT CSAIL model 3rd floor:
44 // camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);
45
46 // works well for MIT CSAIL model 2nd floor:
47 // camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);
48
49 // works for small files:
50 // camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
51 camera_->set(0.471703, 1.59862, 3.10937, 0, 0.418879, -12.2129);
52 camera_->setPitch(0.418879); // not sure why this is here:
53
54 for (int i = 0; i < 2048; i++) {
55 float v = i / 2048.0;
56 v = powf(v, 3) * 6;
57 t_gamma[i] = v * 6 * 256;
58 }
59 }
60
61 void
initializeGL(int argc,char ** argv)62 pcl::simulation::SimExample::initializeGL(int argc, char** argv)
63 {
64 glutInit(&argc, argv);
65 glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB); // was GLUT_RGBA
66 glutInitWindowPosition(10, 10);
67 glutInitWindowSize(10, 10);
68 // glutInitWindowSize (window_width_, window_height_);
69 glutCreateWindow("OpenGL range likelihood");
70
71 GLenum err = glewInit();
72 if (GLEW_OK != err) {
73 std::cerr << "Error: " << glewGetErrorString(err) << std::endl;
74 exit(-1);
75 }
76
77 std::cout << "Status: Using GLEW " << glewGetString(GLEW_VERSION) << std::endl;
78 if (glewIsSupported("GL_VERSION_2_0"))
79 std::cout << "OpenGL 2.0 supported" << std::endl;
80 else {
81 std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
82 exit(1);
83 }
84
85 std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;
86 const GLubyte* version = glGetString(GL_VERSION);
87 std::cout << "OpenGL Version: " << version << std::endl;
88 }
89
90 void
doSim(Eigen::Isometry3d pose_in)91 pcl::simulation::SimExample::doSim(Eigen::Isometry3d pose_in)
92 {
93 // No reference image - but this is kept for compatibility with range_test_v2:
94 float* reference = new float[rl_->getRowHeight() * rl_->getColWidth()];
95 const float* depth_buffer = rl_->getDepthBuffer();
96 // Copy one image from our last as a reference.
97 for (int i = 0, n = 0; i < rl_->getRowHeight(); ++i) {
98 for (int j = 0; j < rl_->getColWidth(); ++j) {
99 reference[n++] = depth_buffer[i * rl_->getWidth() + j];
100 }
101 }
102
103 std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
104 std::vector<float> scores;
105 poses.push_back(pose_in);
106 rl_->computeLikelihoods(reference, poses, scores);
107 std::cout << "camera: " << camera_->getX() << " " << camera_->getY() << " "
108 << camera_->getZ() << " " << camera_->getRoll() << " "
109 << camera_->getPitch() << " " << camera_->getYaw() << std::endl;
110
111 delete[] reference;
112 }
113
114 void
write_score_image(const float * score_buffer,std::string fname)115 pcl::simulation::SimExample::write_score_image(const float* score_buffer,
116 std::string fname)
117 {
118 int npixels = rl_->getWidth() * rl_->getHeight();
119 std::uint8_t* score_img = new std::uint8_t[npixels * 3];
120
121 float min_score = score_buffer[0];
122 float max_score = score_buffer[0];
123 for (int i = 1; i < npixels; i++) {
124 if (score_buffer[i] < min_score)
125 min_score = score_buffer[i];
126 if (score_buffer[i] > max_score)
127 max_score = score_buffer[i];
128 }
129
130 for (int y = 0; y < height_; ++y) {
131 for (int x = 0; x < width_; ++x) {
132 int i = y * width_ + x;
133 int i_in = (height_ - 1 - y) * width_ + x; // flip up
134
135 float d = (score_buffer[i_in] - min_score) / (max_score - min_score);
136 score_img[3 * i + 0] = 0;
137 score_img[3 * i + 1] = d * 255;
138 score_img[3 * i + 2] = 0;
139 }
140 }
141
142 // Write to file:
143 pcl::io::saveRgbPNGFile(fname, score_img, width_, height_);
144
145 delete[] score_img;
146 }
147
148 void
write_depth_image(const float * depth_buffer,std::string fname)149 pcl::simulation::SimExample::write_depth_image(const float* depth_buffer,
150 std::string fname)
151 {
152 int npixels = rl_->getWidth() * rl_->getHeight();
153 std::uint8_t* depth_img = new std::uint8_t[npixels * 3];
154
155 float min_depth = depth_buffer[0];
156 float max_depth = depth_buffer[0];
157 for (int i = 1; i < npixels; i++) {
158 if (depth_buffer[i] < min_depth)
159 min_depth = depth_buffer[i];
160 if (depth_buffer[i] > max_depth)
161 max_depth = depth_buffer[i];
162 }
163
164 for (int y = 0; y < height_; ++y) {
165 for (int x = 0; x < width_; ++x) {
166 int i = y * width_ + x;
167 int i_in = (height_ - 1 - y) * width_ + x; // flip up down
168
169 float zn = 0.7;
170 float zf = 20.0;
171 float d = depth_buffer[i_in];
172 float z = -zf * zn / ((zf - zn) * (d - zf / (zf - zn)));
173 float b = 0.075;
174 float f = 580.0;
175 std::uint16_t kd = static_cast<std::uint16_t>(1090 - b * f / z * 8);
176 if (kd > 2047)
177 kd = 2047;
178
179 int pval = t_gamma[kd];
180 int lb = pval & 0xff;
181 switch (pval >> 8) {
182 case 0:
183 depth_img[3 * i + 0] = 255;
184 depth_img[3 * i + 1] = 255 - lb;
185 depth_img[3 * i + 2] = 255 - lb;
186 break;
187 case 1:
188 depth_img[3 * i + 0] = 255;
189 depth_img[3 * i + 1] = lb;
190 depth_img[3 * i + 2] = 0;
191 break;
192 case 2:
193 depth_img[3 * i + 0] = 255 - lb;
194 depth_img[3 * i + 1] = 255;
195 depth_img[3 * i + 2] = 0;
196 break;
197 case 3:
198 depth_img[3 * i + 0] = 0;
199 depth_img[3 * i + 1] = 255;
200 depth_img[3 * i + 2] = lb;
201 break;
202 case 4:
203 depth_img[3 * i + 0] = 0;
204 depth_img[3 * i + 1] = 255 - lb;
205 depth_img[3 * i + 2] = 255;
206 break;
207 case 5:
208 depth_img[3 * i + 0] = 0;
209 depth_img[3 * i + 1] = 0;
210 depth_img[3 * i + 2] = 255 - lb;
211 break;
212 default:
213 depth_img[3 * i + 0] = 0;
214 depth_img[3 * i + 1] = 0;
215 depth_img[3 * i + 2] = 0;
216 break;
217 }
218 }
219 }
220
221 // Write to file:
222 pcl::io::saveRgbPNGFile(fname, depth_img, width_, height_);
223
224 delete[] depth_img;
225 }
226
227 void
write_depth_image_uint(const float * depth_buffer,std::string fname)228 pcl::simulation::SimExample::write_depth_image_uint(const float* depth_buffer,
229 std::string fname)
230 {
231 int npixels = rl_->getWidth() * rl_->getHeight();
232 unsigned short* depth_img = new unsigned short[npixels];
233
234 float min_depth = depth_buffer[0];
235 float max_depth = depth_buffer[0];
236 for (int i = 1; i < npixels; i++) {
237 if (depth_buffer[i] < min_depth)
238 min_depth = depth_buffer[i];
239 if (depth_buffer[i] > max_depth)
240 max_depth = depth_buffer[i];
241 }
242
243 for (int y = 0; y < height_; ++y) {
244 for (int x = 0; x < width_; ++x) {
245 int i = y * width_ + x;
246 int i_in = (height_ - 1 - y) * width_ + x; // flip up down
247
248 float zn = 0.7;
249 float zf = 20.0;
250 float d = depth_buffer[i_in];
251
252 unsigned short z_new = static_cast<unsigned short>(std::min(
253 std::floor(1000 * (-zf * zn / ((zf - zn) * (d - zf / (zf - zn))))), 65535.f));
254
255 if (z_new < 18000) {
256 std::cout << z_new << " " << d << " " << x << "\n";
257 }
258
259 depth_img[i] = z_new;
260 }
261 }
262
263 // Write to file:
264 pcl::io::saveShortPNGFile(fname, depth_img, width_, height_, 1);
265
266 delete[] depth_img;
267 }
268
269 void
write_rgb_image(const std::uint8_t * rgb_buffer,std::string fname)270 pcl::simulation::SimExample::write_rgb_image(const std::uint8_t* rgb_buffer,
271 std::string fname)
272 {
273 int npixels = rl_->getWidth() * rl_->getHeight();
274 std::uint8_t* rgb_img = new std::uint8_t[npixels * 3];
275
276 for (int y = 0; y < height_; ++y) {
277 for (int x = 0; x < width_; ++x) {
278 int px = y * width_ + x;
279 int px_in = (height_ - 1 - y) * width_ + x; // flip up down
280 rgb_img[3 * (px) + 0] = rgb_buffer[3 * px_in + 0];
281 rgb_img[3 * (px) + 1] = rgb_buffer[3 * px_in + 1];
282 rgb_img[3 * (px) + 2] = rgb_buffer[3 * px_in + 2];
283 }
284 }
285
286 // Write to file:
287 pcl::io::saveRgbPNGFile(fname, rgb_img, width_, height_);
288
289 delete[] rgb_img;
290 }
291