feat(slam): add rtabmap_ros

2025-07-14 11:34:38 +08:00
parent 3b6641c1fb
commit 943ce5b06f
1635 changed files with 603092 additions and 0 deletions
@@ -0,0 +1,689 @@
+//
+//  PCLWrapper.cpp
+//  ThreeDScanner
+//
+//  Created by Steven Roach on 2/9/18.
+//  Copyright © 2018 Steven Roach. All rights reserved.
+//
+
+#include "NativeWrapper.hpp"
+#include "RTABMapApp.h"
+#include <rtabmap/core/DBDriverSqlite3.h>
+
+inline RTABMapApp *native(const void *object) {
+  return (RTABMapApp *)object;
+}
+
+const void * createNativeApplication()
+{
+    RTABMapApp *app = new RTABMapApp();
+    return (void *)app;
+}
+
+void setupCallbacksNative(const void *object, void * classPtr,
+                          void(*progressCallback)(void*, int, int),
+                          void(*initCallback)(void *, int, const char*),
+                          void(*statsUpdatedCallback)(void *,
+                                                   int, int, int, int,
+                                                   float,
+                                                   int, int, int, int, int ,int,
+                                                   float,
+                                                   int,
+                                                   float,
+                                                   int,
+                                                   float, float, float, float,
+                                                   int, int,
+                                                   float, float, float, float, float, float),
+                          void(*cameraInfoEventCallback)(void *, int, const char*, const char*))
+{
+    if(object)
+    {
+        native(object)->setupSwiftCallbacks(classPtr, progressCallback, initCallback, statsUpdatedCallback, cameraInfoEventCallback);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void destroyNativeApplication(const void *object)
+{
+    if(object)
+    {
+        delete native(object);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void setScreenRotationNative(const void *object, int displayRotation)
+{
+    if(object)
+    {
+        return native(object)->setScreenRotation(displayRotation, 0);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
+{
+    if(object)
+    {
+        return native(object)->openDatabase(databasePath, databaseInMemory, optimize, clearDatabase);
+    }
+    else
+    {
+        UERROR("object is null!");
+        return -1;
+    }
+}
+
+void saveNative(const void *object, const char * databasePath)
+{
+    if(object)
+    {
+        return native(object)->save(databasePath);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+bool recoverNative(const void *object, const char * from, const char * to)
+{
+    if(object)
+    {
+        return native(object)->recover(from, to);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+void cancelProcessingNative(const void *object)
+{
+    if(object)
+    {
+        native(object)->cancelProcessing();
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+int postProcessingNative(const void *object, int approach)
+{
+    if(object)
+    {
+        return native(object)->postProcessing(approach);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return -1;
+}
+
+bool exportMeshNative(
+            const void *object,
+            float cloudVoxelSize,
+            bool regenerateCloud,
+            bool meshing,
+            int textureSize,
+            int textureCount,
+            int normalK,
+            bool optimized,
+            float optimizedVoxelSize,
+            int optimizedDepth,
+            int optimizedMaxPolygons,
+            float optimizedColorRadius,
+            bool optimizedCleanWhitePolygons,
+            int optimizedMinClusterSize,
+            float optimizedMaxTextureDistance,
+            int optimizedMinTextureClusterSize,
+            int textureVertexColorPolicy,
+            bool blockRendering)
+{
+    if(object)
+    {
+        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, textureVertexColorPolicy, blockRendering);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+bool postExportationNative(const void *object, bool visualize)
+{
+    if(object)
+    {
+        return native(object)->postExportation(visualize);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+bool writeExportedMeshNative(const void *object, const char * directory, const char * name)
+{
+    if(object)
+    {
+        return native(object)->writeExportedMesh(directory, name);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+void initGlContentNative(const void *object) {
+    if(object)
+    {
+        native(object)->InitializeGLContent();
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void setupGraphicNative(const void *object, int width, int height) {
+    if(object)
+    {
+        native(object)->SetViewPort(width, height);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void onTouchEventNative(const void *object, int touch_count, int event, float x0, float y0, float x1,
+    float y1) {
+    if(object)
+    {
+        using namespace tango_gl;
+        GestureCamera::TouchEvent touch_event =
+          static_cast<GestureCamera::TouchEvent>(event);
+        native(object)->OnTouchEvent(touch_count, touch_event, x0, y0, x1, y1);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void setPausedMappingNative(const void *object, bool paused)
+{
+    if(object)
+    {
+        return native(object)->setPausedMapping(paused);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+int renderNative(const void *object) {
+    if(object)
+    {
+        return native(object)->Render();
+    }
+    else
+    {
+        UERROR("object is null!");
+        return -1;
+    }
+}
+
+bool startCameraNative(const void *object) {
+    if(object)
+    {
+        return native(object)->startCamera();
+    }
+    else
+    {
+        UERROR("object is null!");
+        return false;
+    }
+}
+
+void stopCameraNative(const void *object) {
+    if(object)
+    {
+        native(object)->stopCamera();
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void setCameraNative(const void *object, int type) {
+    if(object)
+    {
+        native(object)->SetCameraType(tango_gl::GestureCamera::CameraType(type));
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void postOdometryEventNative(const void *object,
+        float x, float y, float z, float qx, float qy, float qz, float qw,
+        float fx, float fy, float cx, float cy,
+        double stamp,
+        const void * yPlane,  const void * uPlane,  const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+        const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+        const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
+        const void * points, int pointsLen, int pointsChannels,
+        float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw,
+        float p00, float p11, float p02, float p12, float p22, float p32, float p23,
+        float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7)
+{
+    if(object)
+    {
+        native(object)->postOdometryEvent(
+                (qx==0.0f && qy==0.0f && qz==0.0f && qw==0.0f)?rtabmap::Transform():rtabmap::Transform(x,y,z,qx,qy,qz,qw),
+                fx,fy,cx,cy, 0,0,0,0,
+                rtabmap::Transform(), rtabmap::Transform(),
+                stamp, 0,
+                yPlane, uPlane, vPlane, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
+                depth, depthLen, depthWidth, depthHeight, depthFormat,
+                conf, confLen, confWidth, confHeight, confFormat,
+                (const float *)points, pointsLen, pointsChannels,
+                rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
+                p00, p11, p02, p12, p22, p32, p23,
+                t0, t1, t2, t3, t4, t5, t6, t7);
+    }
+    else
+    {
+        UERROR("object is null!");
+        return;
+    }
+}
+
+ImageNative getPreviewImageNative(const char * databasePath)
+{
+    ImageNative imageNative;
+    imageNative.data = 0;
+    imageNative.objectPtr = 0;
+    rtabmap::DBDriverSqlite3 driver;
+    if(driver.openConnection(databasePath))
+    {
+        cv::Mat image = driver.loadPreviewImage();
+        if(image.empty())
+        {
+            return imageNative;
+        }
+        cv::Mat * imagePtr = new cv::Mat();
+        // We should add alpha channel
+        cv::cvtColor(image, *imagePtr, cv::COLOR_BGR2BGRA);
+        std::vector<cv::Mat> channels;
+        cv::split(*imagePtr, channels);
+        channels.back() = cv::Scalar(255);
+        cv::merge(channels, *imagePtr);
+        imageNative.objectPtr = imagePtr;
+        imageNative.data = imagePtr->data;
+        imageNative.width = imagePtr->cols;
+        imageNative.height = imagePtr->rows;
+        imageNative.channels = imagePtr->channels();
+    }
+    return imageNative;
+}
+
+void releasePreviewImageNative(ImageNative image)
+{
+    if(image.objectPtr)
+    {
+        delete (cv::Mat*)image.objectPtr;
+    }
+}
+
+
+// Parameters
+void setOnlineBlendingNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setOnlineBlending(enabled);
+    else
+        UERROR("object is null!");
+}
+void setMapCloudShownNative(const void *object, bool shown)
+{
+    if(object)
+        native(object)->setMapCloudShown(shown);
+    else
+        UERROR("object is null!");
+}
+void setOdomCloudShownNative(const void *object, bool shown)
+{
+    if(object)
+        native(object)->setOdomCloudShown(shown);
+    else
+        UERROR("object is null!");
+}
+void setMeshRenderingNative(const void *object, bool enabled, bool withTexture)
+{
+    if(object)
+        native(object)->setMeshRendering(enabled, withTexture);
+    else
+        UERROR("object is null!");
+}
+void setPointSizeNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setPointSize(value);
+    else
+        UERROR("object is null!");
+}
+void setFOVNative(const void *object, float angle)
+{
+    if(object)
+        native(object)->setFOV(angle);
+    else
+        UERROR("object is null!");
+}
+void setOrthoCropFactorNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setOrthoCropFactor(value);
+    else
+        UERROR("object is null!");
+}
+void setGridRotationNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setGridRotation(value);
+    else
+        UERROR("object is null!");
+}
+void setLightingNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setLighting(enabled);
+    else
+        UERROR("object is null!");
+}
+void setBackfaceCullingNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setBackfaceCulling(enabled);
+    else
+        UERROR("object is null!");
+}
+void setWireframeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setWireframe(enabled);
+    else
+        UERROR("object is null!");
+}
+void setTextureColorSeamsHiddenNative(const void *object, bool hidden)
+{
+    if(object)
+        native(object)->setTextureColorSeamsHidden(hidden);
+    else
+        UERROR("object is null!");
+}
+void setLocalizationModeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setLocalizationMode(enabled);
+    else
+        UERROR("object is null!");
+}
+void setDataRecorderModeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setDataRecorderMode(enabled);
+    else
+        UERROR("object is null!");
+}
+void setTrajectoryModeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setTrajectoryMode(enabled);
+    else
+        UERROR("object is null!");
+}
+void setGraphOptimizationNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setGraphOptimization(enabled);
+    else
+        UERROR("object is null!");
+}
+void setNodesFilteringNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setNodesFiltering(enabled);
+    else
+        UERROR("object is null!");
+}
+void setGraphVisibleNative(const void *object, bool visible)
+{
+    if(object)
+        native(object)->setGraphVisible(visible);
+    else
+        UERROR("object is null!");
+}
+void setGridVisibleNative(const void *object, bool visible)
+{
+    if(object)
+        native(object)->setGridVisible(visible);
+    else
+        UERROR("object is null!");
+}
+void setFullResolutionNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setFullResolution(enabled);
+    else
+        UERROR("object is null!");
+}
+void setSmoothingNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setSmoothing(enabled);
+    else
+        UERROR("object is null!");
+}
+void setDepthBleedingErrorNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setDepthBleedingError(value);
+    else
+        UERROR("object is null!");
+}
+void setAppendModeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setAppendMode(enabled);
+    else
+        UERROR("object is null!");
+}
+void setUpstreamRelocalizationAccThrNative(const void * object, float value)
+{
+    if(object)
+        native(object)->setUpstreamRelocalizationAccThr(value);
+    else
+        UERROR("object is null!");
+}
+void setMaxCloudDepthNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMaxCloudDepth(value);
+    else
+        UERROR("object is null!");
+}
+void setMinCloudDepthNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMinCloudDepth(value);
+    else
+        UERROR("object is null!");
+}
+void setCloudDensityLevelNative(const void *object, int value)
+{
+    if(object)
+        native(object)->setCloudDensityLevel(value);
+    else
+        UERROR("object is null!");
+}
+void setMeshAngleToleranceNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMeshAngleTolerance(value);
+    else
+        UERROR("object is null!");
+}
+void setMeshDecimationFactorNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMeshDecimationFactor(value);
+    else
+        UERROR("object is null!");
+}
+void setMeshTriangleSizeNative(const void *object, int value)
+{
+    if(object)
+        native(object)->setMeshTriangleSize(value);
+    else
+        UERROR("object is null!");
+}
+void setClusterRatioNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setClusterRatio(value);
+    else
+        UERROR("object is null!");
+}
+void setMaxGainRadiusNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMaxGainRadius(value);
+    else
+        UERROR("object is null!");
+}
+void setRenderingTextureDecimationNative(const void *object, int value)
+{
+    if(object)
+        native(object)->setRenderingTextureDecimation(value);
+    else
+        UERROR("object is null!");
+}
+void setBackgroundColorNative(const void *object, float gray)
+{
+    if(object)
+        native(object)->setBackgroundColor(gray);
+    else
+        UERROR("object is null!");
+}
+void setDepthConfidenceNative(const void *object, int value)
+{
+    if(object)
+        native(object)->setDepthConfidence(value);
+    else
+        UERROR("object is null!");
+}
+
+void setExportPointCloudFormatNative(const void *object, const char * format)
+{
+    if(object)
+        native(object)->setExportPointCloudFormat(format);
+    else
+        UERROR("object is null!");
+}
+
+int setMappingParameterNative(const void *object, const char * key, const char * value)
+{
+    if(object)
+        return native(object)->setMappingParameter(key, value);
+    else
+        UERROR("object is null!");
+    return -1;
+}
+
+void setGPSNative(const void *object, double stamp, double longitude, double latitude, double altitude, double accuracy, double bearing)
+{
+    rtabmap::GPS gps(stamp, longitude, latitude, altitude, accuracy, bearing);
+    if(object)
+        return native(object)->setGPS(gps);
+    else
+        UERROR("object is null!");
+}
+
+void addEnvSensorNative(const void *object, int type, float value)
+{
+    if(object)
+        return native(object)->addEnvSensor(type, value);
+    else
+        UERROR("object is null!");
+}
+
+void removeMeasureNative(const void *object)
+{
+    if(object)
+        return native(object)->removeMeasure();
+    else
+        UERROR("object is null!");
+}
+void addMeasureNative(const void *object)
+{
+    if(object)
+        return native(object)->addMeasureButtonClicked();
+    else
+        UERROR("object is null!");
+}
+void teleportNative(const void *object)
+{
+    if(object)
+        return native(object)->teleportButtonClicked();
+    else
+        UERROR("object is null!");
+}
+void setMeasuringModeNative(const void *object, int mode)
+{
+    if(object)
+        return native(object)->setMeasuringMode(mode);
+    else
+        UERROR("object is null!");
+}
+void setMetricSystemNative(const void *object, bool enabled)
+{
+    if(object)
+        return native(object)->setMetricSystem(enabled);
+    else
+        UERROR("object is null!");
+}
+void setMeasuringTextSizeNative(const void *object, float size)
+{
+    if(object)
+        return native(object)->setMeasuringTextSize(size);
+    else
+        UERROR("object is null!");
+}
+void clearMeasuresNative(const void *object)
+{
+    if(object)
+        return native(object)->clearMeasures();
+    else
+        UERROR("object is null!");
+}