Example: hwdrivers_openni2_driver_demo
C++ example source code:
/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include <mrpt/gui/CDisplayWindow3D.h> #include <mrpt/hwdrivers/COpenNI2Sensor.h> #include <mrpt/img/TColor.h> #include <mrpt/opengl.h> #include <mrpt/system/CTicTac.h> #include <chrono> #include <iostream> #include <thread> using namespace mrpt; using namespace mrpt::obs; using namespace mrpt::opengl; using namespace mrpt::hwdrivers; using namespace mrpt::system; using namespace mrpt::img; using namespace std; // This simple demo records form an OpenNI2 device into a rawlog as 3D // observations. It's meant as a temporary workaround before OpenNI2 is // integrated as a generic sensor so that it works with rawlog-grabber. int main(int argc, char** argv) { try { if (argc > 2) { cerr << "Usage: " << argv[0] << " <sensor_id/sensor_serial\n"; cerr << "Example: " << argv[0] << " 0 \n"; return 1; } // const unsigned sensor_id = 0; COpenNI2Sensor rgbd_sensor; // rgbd_sensor.loadConfig_sensorSpecific(const // mrpt::config::CConfigFileBase &configSource, const std::string // &iniSection ); unsigned sensor_id_or_serial = 0; // const string configFile = std::string( argv[2] ); if (argc == 2) { sensor_id_or_serial = atoi(argv[1]); if (sensor_id_or_serial > 10) rgbd_sensor.setSerialToOpen(sensor_id_or_serial); else rgbd_sensor.setSensorIDToOpen(sensor_id_or_serial); } // Open: // cout << "Calling COpenNI2Sensor::initialize()..."; rgbd_sensor.initialize(); if (rgbd_sensor.getNumDevices() == 0) return 0; cout << "OK " << rgbd_sensor.getNumDevices() << " available devices." << endl; cout << "\nUse device " << sensor_id_or_serial << endl << endl; bool showImages = false; if (showImages) { mrpt::gui::CDisplayWindow win("Video"); CTicTac tictac; tictac.Tic(); unsigned int nFrames = 0; CObservation3DRangeScan newObs; bool bObs = false, bError = true; rgbd_sensor.getNextObservation(newObs, bObs, bError); while (!system::os::kbhit()) { cout << "Get a new frame\n"; rgbd_sensor.getNextObservation(newObs, bObs, bError); double fps = ++nFrames / tictac.Tac(); // newObs->intensityImage.textOut(5,5,format("%.02f // fps",fps),TColor(0x80,0x80,0x80) ); cout << "FPS: " << fps << endl; if (nFrames > 100) { tictac.Tic(); nFrames = 0; } win.showImage(newObs.intensityImage); std::this_thread::sleep_for(10ms); } } else // Show point cloud and images { // Create window and prepare OpenGL object in the scene: // -------------------------------------------------------- mrpt::gui::CDisplayWindow3D win3D("OpenNI2 3D view", 800, 600); win3D.setCameraAzimuthDeg(140); win3D.setCameraElevationDeg(20); win3D.setCameraZoom(8.0); win3D.setFOV(90); win3D.setCameraPointingToPoint(2.5, 0, 0); mrpt::opengl::CPointCloudColoured::Ptr gl_points = mrpt::opengl::CPointCloudColoured::Create(); gl_points->setPointSize(2.5); opengl::Viewport::Ptr viewInt; // Extra viewports for the RGB images. { mrpt::opengl::Scene::Ptr& scene = win3D.get3DSceneAndLock(); // Create the Opengl object for the point cloud: scene->insert(gl_points); scene->insert(mrpt::opengl::CGridPlaneXY::Create()); scene->insert(mrpt::opengl::stock_objects::CornerXYZ()); const double aspect_ratio = 480.0 / 640.0; const int VW_WIDTH = 400; // Size of the viewport into the // window, in pixel units. const int VW_HEIGHT = aspect_ratio * VW_WIDTH; // Create an extra opengl viewport for the RGB image: viewInt = scene->createViewport("view2d_int"); viewInt->setViewportPosition(5, 30, VW_WIDTH, VW_HEIGHT); win3D.addTextMessage( 10, 30 + VW_HEIGHT + 10, "Intensity data", 2); win3D.addTextMessage( 5, 5, "'o'/'i'-zoom out/in, ESC: quit", 110); win3D.unlockAccess3DScene(); win3D.repaint(); } // Grab frames continuously and show //======================================================================================== bool bObs = false, bError = true; mrpt::system::TTimeStamp last_obs_tim = INVALID_TIMESTAMP; while (!win3D.keyHit()) // Push any key to exit // win3D.isOpen() { // cout << "Get new observation\n"; CObservation3DRangeScan::Ptr newObs = CObservation3DRangeScan::Create(); rgbd_sensor.getNextObservation(*newObs, bObs, bError); if (bObs && !bError && newObs && newObs->timestamp != INVALID_TIMESTAMP && newObs->timestamp != last_obs_tim) { // It IS a new observation: last_obs_tim = newObs->timestamp; // Update visualization // --------------------------------------- win3D.get3DSceneAndLock(); // Estimated grabbing rate: win3D.addTextMessage( -350, -13, format( "Timestamp: %s", mrpt::system::dateTimeLocalToString(last_obs_tim) .c_str()), 100); // Show intensity image: if (newObs->hasIntensityImage) { viewInt->setImageView( newObs->intensityImage); // This is not "_fast" // since the intensity // image may be needed // later on. } win3D.unlockAccess3DScene(); // ------------------------------------------------------- // Create 3D points from RGB+D data // // There are several methods to do this. // Switch the #if's to select among the options: // See also: // https://www.mrpt.org/Generating_3D_point_clouds_from_RGB_D_observations // ------------------------------------------------------- if (newObs->hasRangeImage) { #if 0 static pcl::PointCloud<pcl::PointXYZRGB> cloud; newObs->unprojectInto(cloud, false /* without obs.sensorPose */); win3D.get3DSceneAndLock(); gl_points->loadFromPointsMap(&cloud); win3D.unlockAccess3DScene(); #endif // Pathway: RGB+D --> XYZ+RGB opengl #if 1 win3D.get3DSceneAndLock(); mrpt::obs::T3DPointsProjectionParams pp; pp.takeIntoAccountSensorPoseOnRobot = false; newObs->unprojectInto( *gl_points, pp /* without obs.sensorPose */); win3D.unlockAccess3DScene(); #endif } win3D.repaint(); } // end update visualization: } } cout << "\nClosing RGBD sensor...\n"; return 0; } catch (const std::exception& e) { std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl; return -1; } }