Example: hwdrivers_swissranger_example
C++ example source code:
/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2022, 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.h> #include <mrpt/hwdrivers/CSwissRanger3DCamera.h> #include <mrpt/img/TColor.h> #include <mrpt/maps/CColouredPointsMap.h> #include <mrpt/obs/CObservation3DRangeScan.h> #include <mrpt/opengl/CGridPlaneXY.h> #include <mrpt/opengl/CPointCloudColoured.h> #include <mrpt/opengl/CTexturedPlane.h> #include <mrpt/opengl/stock_objects.h> #include <mrpt/system/CTicTac.h> #include <chrono> #include <iostream> #include <thread> using namespace mrpt; using namespace mrpt::hwdrivers; using namespace mrpt::math; using namespace mrpt::gui; using namespace mrpt::maps; using namespace mrpt::obs; using namespace mrpt::opengl; using namespace mrpt::system; using namespace mrpt::img; using namespace std; // ------------------------------------------------------ // Test_SwissRanger // ------------------------------------------------------ void Test_SwissRanger() { CSwissRanger3DCamera cam; // Set params: cam.setOpenFromUSB(true); cam.setSave3D(true); cam.setSaveRangeImage(true); cam.setSaveIntensityImage(true); cam.setSaveConfidenceImage(false); // cam.enablePreviewWindow(true); // Open: cam.initialize(); if (cam.isOpen()) cout << "[Test_SwissRanger] Camera open, serial #" << cam.getCameraSerialNumber() << " resolution: " << cam.cols() << "x" << cam.rows() << " max. range: " << cam.getMaxRange() << endl; const double aspect_ratio = cam.rows() / double(cam.cols()); { std::string ver; cam.getMesaLibVersion(ver); cout << "[Test_SwissRanger] Version: " << ver << "\n"; } CObservation3DRangeScan obs; bool there_is_obs = true, hard_error; mrpt::gui::CDisplayWindow3D win3D("3D camera view", 800, 600); win3D.setCameraAzimuthDeg(140); win3D.setCameraElevationDeg(20); win3D.setCameraZoom(6.0); win3D.setCameraPointingToPoint(2.5, 0, 0); // mrpt::gui::CDisplayWindow win2D("2D range image",200,200); // mrpt::gui::CDisplayWindow winInt("Intensity range image",200,200); // win2D.setPos(10,10); // winInt.setPos(350,10); // win3D.setPos(10,290); // win3D.resize(400,200); // mrpt::opengl::CPointCloud::Ptr gl_points = // mrpt::opengl::CPointCloud::Create(); mrpt::opengl::CPointCloudColoured::Ptr gl_points = mrpt::opengl::CPointCloudColoured::Create(); gl_points->setPointSize(4.5); mrpt::opengl::CTexturedPlane::Ptr gl_img_range = mrpt::opengl::CTexturedPlane::Create( 0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio); mrpt::opengl::CTexturedPlane::Ptr gl_img_intensity = mrpt::opengl::CTexturedPlane::Create( 0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio); mrpt::opengl::CTexturedPlane::Ptr gl_img_intensity_rect = mrpt::opengl::CTexturedPlane::Create( 0.5, -0.5, -0.5 * aspect_ratio, 0.5 * aspect_ratio); { mrpt::opengl::COpenGLScene::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 int VW_WIDTH = 200; const int VW_HEIGHT = 150; const int VW_GAP = 10; // Create the Opengl objects for the planar images, as textured planes, // each in a separate viewport: win3D.addTextMessage( 30, -10 - 1 * (VW_GAP + VW_HEIGHT), "Range data", 1); opengl::COpenGLViewport::Ptr viewRange = scene->createViewport("view2d_range"); scene->insert(gl_img_range, "view2d_range"); viewRange->setViewportPosition( 5, -10 - 1 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT); viewRange->setTransparent(true); viewRange->getCamera().setOrthogonal(true); viewRange->getCamera().setAzimuthDegrees(90); viewRange->getCamera().setElevationDegrees(90); viewRange->getCamera().setZoomDistance(1.0); win3D.addTextMessage( 30, -10 - 2 * (VW_GAP + VW_HEIGHT), "Intensity data", 2); opengl::COpenGLViewport::Ptr viewInt = scene->createViewport("view2d_int"); scene->insert(gl_img_intensity, "view2d_int"); viewInt->setViewportPosition( 5, -10 - 2 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT); viewInt->setTransparent(true); viewInt->getCamera().setOrthogonal(true); viewInt->getCamera().setAzimuthDegrees(90); viewInt->getCamera().setElevationDegrees(90); viewInt->getCamera().setZoomDistance(1.0); win3D.addTextMessage( 30, -10 - 3 * (VW_GAP + VW_HEIGHT), "Intensity data (undistorted)", 3); opengl::COpenGLViewport::Ptr viewIntRect = scene->createViewport("view2d_intrect"); scene->insert(gl_img_intensity_rect, "view2d_intrect"); viewIntRect->setViewportPosition( 5, -10 - 3 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT); viewIntRect->setTransparent(true); viewIntRect->getCamera().setOrthogonal(true); viewIntRect->getCamera().setAzimuthDegrees(90); viewIntRect->getCamera().setElevationDegrees(90); viewIntRect->getCamera().setZoomDistance(1.0); win3D.unlockAccess3DScene(); win3D.repaint(); } CTicTac tictac; size_t nImgs = 0; bool endLoop = false; while (there_is_obs && !endLoop && win3D.isOpen()) { // Grab new observation from the camera: cam.getNextObservation(obs, there_is_obs, hard_error); // Show ranges as 2D: if (there_is_obs && obs.hasRangeImage) { mrpt::img::CImage img = obs.rangeImage_getAsImage(); win3D.get3DSceneAndLock(); gl_img_range->assignImage(std::move(img)); win3D.unlockAccess3DScene(); } // Show intensity image: if (there_is_obs && obs.hasIntensityImage) { win3D.get3DSceneAndLock(); gl_img_intensity->assignImage(obs.intensityImage); CImage undistortImg; obs.intensityImage.undistort(undistortImg, obs.cameraParams); gl_img_intensity_rect->assignImage(undistortImg); win3D.unlockAccess3DScene(); } // Show 3D points: if (there_is_obs && obs.hasPoints3D) { // mrpt::maps::CSimplePointsMap pntsMap; CColouredPointsMap pntsMap; pntsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage; pntsMap.loadFromRangeScan(obs); win3D.get3DSceneAndLock(); gl_points->loadFromPointsMap(&pntsMap); win3D.unlockAccess3DScene(); win3D.repaint(); } nImgs++; if (nImgs > 10) { win3D.get3DSceneAndLock(); win3D.addTextMessage( 0.01, 0.01, format("%.02f Hz", nImgs / tictac.Tac()), 100); win3D.unlockAccess3DScene(); nImgs = 0; tictac.Tic(); } // Process possible keyboard commands: // -------------------------------------- if (win3D.keyHit()) { const int key = tolower(win3D.getPushedKey()); // cout << "key: " << key << endl; switch (key) { case 'h': cam.enableImageHistEqualization( !cam.isEnabledImageHistEqualization()); break; case 'g': cam.enableConvGray(!cam.isEnabledConvGray()); break; case 'd': cam.enableDenoiseANF(!cam.isEnabledDenoiseANF()); break; case 'f': cam.enableMedianFilter(!cam.isEnabledMedianFilter()); break; case 27: endLoop = true; break; } } win3D.get3DSceneAndLock(); win3D.addTextMessage( 0.08, 0.02, format( "Keyboard switches: H (hist.equal: %s) | G (convGray: %s) | D " "(denoise: %s) | F (medianFilter: %s)", cam.isEnabledImageHistEqualization() ? "ON" : "OFF", cam.isEnabledConvGray() ? "ON" : "OFF", cam.isEnabledDenoiseANF() ? "ON" : "OFF", cam.isEnabledMedianFilter() ? "ON" : "OFF"), 110); win3D.unlockAccess3DScene(); std::this_thread::sleep_for(1ms); } } int main(int argc, char** argv) { try { Test_SwissRanger(); return 0; } catch (const std::exception& e) { std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl; return -1; } catch (...) { printf("Another exception!!"); return -1; } }