| 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 |
} |