Debian Codegraph @

samples/hwdrivers_openni2_2d_icp_slam/test.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include <mrpt/gui/CDisplayWindow3D.h>
11 #include <mrpt/hwdrivers/COpenNI2Sensor.h>
12 #include <mrpt/img/TColor.h>
13 #include <mrpt/opengl.h>
14 #include <mrpt/opengl/CPlanarLaserScan.h> // This class is in mrpt-maps
15 #include <mrpt/system/CTicTac.h>
16
17 #include <iostream>
18
19 using namespace mrpt;
20 using namespace mrpt::obs;
21 using namespace mrpt::opengl;
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::img;
24 using namespace std;
25
26 const float vert_FOV = 4.0_deg;
27
28 // This demo records from an OpenNI2 device, convert observations to 2D scans
29 // and runs 2d-icp-slam with them.
30
31 int main(int argc, char** argv)
32 {
33 try
34 {
35 if (argc > 2)
36 {
37 cerr << "Usage: " << argv[0] << " <sensor_id/sensor_serial\n";
38 cerr << "Example: " << argv[0] << " 0 \n";
39 return 1;
40 }
41
42 // const unsigned sensor_id = 0;
43 COpenNI2Sensor rgbd_sensor;
44 // rgbd_sensor.loadConfig_sensorSpecific(const
45 // mrpt::config::CConfigFileBase &configSource, const std::string
46 // &iniSection );
47
48 unsigned sensor_id_or_serial = 0;
49 if (argc == 2)
50 {
51 sensor_id_or_serial = atoi(argv[1]);
52 if (sensor_id_or_serial > 10)
53 rgbd_sensor.setSerialToOpen(sensor_id_or_serial);
54 else
55 rgbd_sensor.setSensorIDToOpen(sensor_id_or_serial);
56 }
57
58 // Open:
59 // cout << "Calling COpenNI2Sensor::initialize()...";
60 rgbd_sensor.initialize();
61
62 if (rgbd_sensor.getNumDevices() == 0) return 0;
63
64 cout << "OK " << rgbd_sensor.getNumDevices() << " available devices." << endl;
65 cout << "\nUse device " << sensor_id_or_serial << endl << endl;
66
67 // Create window and prepare OpenGL object in the scene:
68 // --------------------------------------------------------
69 mrpt::gui::CDisplayWindow3D win3D("OpenNI2 3D view", 800, 600);
70
71 win3D.setCameraAzimuthDeg(140);
72 win3D.setCameraElevationDeg(20);
73 win3D.setCameraZoom(8.0);
74 win3D.setFOV(90);
75 win3D.setCameraPointingToPoint(2.5, 0, 0);
76
77 mrpt::opengl::CPointCloudColoured::Ptr gl_points = mrpt::opengl::CPointCloudColoured::Create();
78 gl_points->setPointSize(2.5);
79
80 // The 2D "laser scan" OpenGL object:
81 mrpt::opengl::CPlanarLaserScan::Ptr gl_2d_scan = mrpt::opengl::CPlanarLaserScan::Create();
82 gl_2d_scan->enablePoints(true);
83 gl_2d_scan->enableLine(true);
84 gl_2d_scan->enableSurface(true);
85 gl_2d_scan->setSurfaceColor(0, 0, 1, 0.3); // RGBA
86
87 opengl::Viewport::Ptr viewInt; // Extra viewports for the RGB images.
88 {
89 mrpt::opengl::Scene::Ptr& scene = win3D.get3DSceneAndLock();
90
91 // Create the Opengl object for the point cloud:
92 scene->insert(gl_points);
93 scene->insert(mrpt::opengl::CGridPlaneXY::Create());
94 scene->insert(mrpt::opengl::stock_objects::CornerXYZ());
95 scene->insert(gl_2d_scan);
96
97 const double aspect_ratio = 480.0 / 640.0;
98 const int VW_WIDTH = 400; // Size of the viewport into the window, in pixel units.
99 const int VW_HEIGHT = aspect_ratio * VW_WIDTH;
100
101 // Create an extra opengl viewport for the RGB image:
102 viewInt = scene->createViewport("view2d_int");
103 viewInt->setViewportPosition(5, 30, VW_WIDTH, VW_HEIGHT);
104 win3D.addTextMessage(10, 30 + VW_HEIGHT + 10, "Intensity data", 2);
105
106 win3D.addTextMessage(5, 5, "'o'/'i'-zoom out/in, ESC: quit", 110);
107
108 win3D.unlockAccess3DScene();
109 win3D.repaint();
110 }
111
112 // Grab frames continuously and show
113 //========================================================================================
114
115 bool bObs = false, bError = true;
116 mrpt::system::TTimeStamp last_obs_tim = INVALID_TIMESTAMP;
117
118 while (!win3D.keyHit()) // Push any key to exit // win3D.isOpen()
119 {
120 // cout << "Get new observation\n";
121 CObservation3DRangeScan::Ptr newObs = CObservation3DRangeScan::Create();
122 rgbd_sensor.getNextObservation(*newObs, bObs, bError);
123
124 CObservation2DRangeScan::Ptr obs_2d; // The equivalent 2D scan
125
126 if (bObs && !bError && newObs && newObs->timestamp != INVALID_TIMESTAMP &&
127 newObs->timestamp != last_obs_tim)
128 {
129 // It IS a new observation:
130 last_obs_tim = newObs->timestamp;
131
132 // Convert ranges to an equivalent 2D "fake laser" scan:
133 if (newObs->hasRangeImage)
134 {
135 // Convert to scan:
136 obs_2d = CObservation2DRangeScan::Create();
137
138 T3DPointsTo2DScanParams p2s;
139 p2s.angle_sup = .5f * vert_FOV;
140 p2s.angle_inf = .5f * vert_FOV;
141 p2s.sensorLabel = "KINECT_2D_SCAN";
142 newObs->convertTo2DScan(*obs_2d, p2s);
143 }
144
145 // Update visualization ---------------------------------------
146
147 win3D.get3DSceneAndLock();
148
149 // Estimated grabbing rate:
150 win3D.addTextMessage(
151 -350, -13,
152 mrpt::format(
153 "Timestamp: %s", mrpt::system::dateTimeLocalToString(last_obs_tim).c_str()),
154 100);
155
156 // Show intensity image:
157 if (newObs->hasIntensityImage)
158 {
159 viewInt->setImageView(newObs->intensityImage); // This is not "_fast" since
160 // the intensity image may be
161 // needed later on.
162 }
163 win3D.unlockAccess3DScene();
164
165 // -------------------------------------------------------
166 // Create 3D points from RGB+D data
167 //
168 // There are several methods to do this.
169 // Switch the #if's to select among the options:
170 // See also:
171 // https://www.mrpt.org/Generating_3D_point_clouds_from_RGB_D_observations
172 // -------------------------------------------------------
173 if (newObs->hasRangeImage)
174 {
175 // Pathway: RGB+D --> XYZ+RGB opengl
176 win3D.get3DSceneAndLock();
177 mrpt::obs::T3DPointsProjectionParams pp;
178 pp.takeIntoAccountSensorPoseOnRobot = false;
179 newObs->unprojectInto(*gl_points, pp /* without obs.sensorPose */);
180 win3D.unlockAccess3DScene();
181 }
182
183 // And load scan in the OpenGL object:
184 gl_2d_scan->setScan(*obs_2d);
185
186 win3D.repaint();
187 } // end update visualization:
188 }
189
190 cout << "\nClosing RGBD sensor...\n";
191
192 return 0;
193 }
194 catch (const std::exception& e)
195 {
196 std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
197 return -1;
198 }
199 }