From b7b25b4da71b49f73d5f63ac9ec96f487a7e7a92 Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Wed, 10 Jan 2024 09:02:04 +0200 Subject: [PATCH 1/9] Remove temporary fix from Python mapping_visu --- python/oak/mapping_visu.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/oak/mapping_visu.py b/python/oak/mapping_visu.py index 7de9e63..ce7a973 100644 --- a/python/oak/mapping_visu.py +++ b/python/oak/mapping_visu.py @@ -66,7 +66,6 @@ def inputStreamLoop(): for output in vioSource: if 'cameraPoses' in output: vioOutput = MockVioOutput(output) - vioOutput.status = spectacularAI.TrackingStatus.TRACKING # TODO: remove, temporary fix for SDK v.1.26.2 onVioOutput(vioOutput) else: onMappingOutput(MockMapperOutput(output)) thread = threading.Thread(target=inputStreamLoop) From 7783b9abd8f261e33f51922cf64cc46f3b275071 Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Fri, 12 Jan 2024 14:28:59 +0200 Subject: [PATCH 2/9] Use Python visualizer in mixed_reality_replay.py --- python/oak/mixed_reality.py | 8 +- python/oak/mixed_reality_replay.py | 144 +++++++++++++++-------------- 2 files changed, 78 insertions(+), 74 deletions(-) diff --git a/python/oak/mixed_reality.py b/python/oak/mixed_reality.py index 66f8230..df05a62 100644 --- a/python/oak/mixed_reality.py +++ b/python/oak/mixed_reality.py @@ -57,19 +57,17 @@ def init_display(w, h): def draw_cube(origin): CUBE_VERTICES = ( - (1, -1, -1), (1, 1, -1), (-1, 1, -1), (-1, -1, -1), - (1, -1, 1), (1, 1, 1), (-1, -1, 1), (-1, 1, 1) + (0.1, -0.1, -0.1), (0.1, 0.1, -0.1), (-0.1, 0.1, -0.1), (-0.1, -0.1, -0.1), + (0.1, -0.1, 0.1), (0.1, 0.1, 0.1), (-0.1, -0.1, 0.1), (-0.1, 0.1, 0.1) ) - CUBE_EDGES = ( (0,1), (0,3), (0,4), (2,1), (2,3), (2,7), (6,3), (6,4), (6,7), (5,1), (5,4), (5,7) ) + glPushMatrix() # cube world position glTranslatef(origin[0], origin[1], origin[2]) - glScalef(*([0.1] * 3)) - glBegin(GL_LINES) for edge in CUBE_EDGES: for vertex in edge: diff --git a/python/oak/mixed_reality_replay.py b/python/oak/mixed_reality_replay.py index a2f57f2..f246800 100644 --- a/python/oak/mixed_reality_replay.py +++ b/python/oak/mixed_reality_replay.py @@ -1,94 +1,100 @@ """ -Mixed reality example using PyOpenGL. Requirements: - - pip install pygame PyOpenGL PyOpenGL_accelerate +Mixed reality example using PyOpenGL. Requirements: spectacularai[full] """ -import depthai + import spectacularAI -import pygame -import time -import numpy as np -# import init_display from mixed_reality -from mixed_reality import init_display, draw_cube, draw, load_obj +from spectacularAI.cli.visualization.visualizer import Visualizer, VisualizerArgs, CameraMode +from mixed_reality import load_obj from OpenGL.GL import * # all prefixed with gl so OK to import * -def parse_args(): +def parseArgs(): import argparse p = argparse.ArgumentParser(__doc__) p.add_argument("dataFolder", help="Folder containing the recorded session for replay", default="data") + p.add_argument("--useRectification", help="This parameter must be set if the videos inputs are not rectified", action="store_true") + p.add_argument('--cameraInd', help="Which camera to use. Typically 0=left, 1=right, 2=auxiliary/RGB (OAK-D default)", type=int, default=2) p.add_argument("--mapLoadPath", help="SLAM map path", default=None) p.add_argument('--objLoadPath', help="Load scene as .obj", default=None) p.add_argument('--latitude', help="Scene coordinate system geographic origin (WGS84): latitude in degrees", default=None) p.add_argument('--longitude', help="Scene coordinate system geographic origin (WGS84): longitude in degrees", default=None) p.add_argument('--altitude', help="Scene coordinate system geographic origin (WGS84): altitude in meters", default=None) + p.add_argument("--recordWindow", help="Window recording filename") return p.parse_args() -args = parse_args() - -display_initialized = False -shouldQuit = False -obj = None -objPos = None # Position in WGS84 coordinates when GPS fusion is enabled -if args.latitude and args.longitude and args.altitude: - objPos = spectacularAI.WgsCoordinates() - objPos.latitude = float(args.latitude) - objPos.longitude = float(args.longitude) - objPos.altitude = float(args.altitude) - -def onOutput(output, frameSet): - global display_initialized - global shouldQuit - global objPos - global obj - global args - - if shouldQuit: return - - if output.globalPose and objPos == None: - # If we receive global pose i.e. recording contains GPS coordinates, then - # place object at the first received device coordinates if not provide - # through CLI arguments - objPos = output.globalPose.coordinates - - for frame in frameSet: - if frame.image is None: continue - if frame.image.getColorFormat() == spectacularAI.ColorFormat.RGB: - img = frame.image.toArray() - # Flip the image upside down for OpenGL - img = np.ascontiguousarray(np.flipud(img)) - width = img.shape[1] - height = img.shape[0] - if not display_initialized: - display_initialized = True - init_display(width, height) - obj = load_obj(args.objLoadPath) +if __name__ == '__main__': + args = parseArgs() + + configInternal = {} + if args.useRectification: + configInternal["useRectification"] = "true" # Undistort images for visualization (assumes undistorted pinhole model) + else: + configInternal["alreadyRectified"] = "true" + + if args.mapLoadPath: + configInternal["mapLoadPath"] = args.mapLoadPath + + obj = None + objPos = None # Position in WGS84 coordinates when GPS fusion is enabled + if args.latitude and args.longitude and args.altitude: + objPos = spectacularAI.WgsCoordinates() + objPos.latitude = float(args.latitude) + objPos.longitude = float(args.longitude) + objPos.altitude = float(args.altitude) + + def renderObj(): + global obj + if obj is None: + obj = load_obj(args.objLoadPath) - for event in pygame.event.get(): - if event.type == pygame.QUIT: - shouldQuit = True - pygame.quit() - return + viewMatrix = visualizer.getViewMatrix() + projectionMatrix = visualizer.getProjectionMatrix() - is_tracking = output.status == spectacularAI.TrackingStatus.TRACKING + modelView = viewMatrix + glMatrixMode(GL_MODELVIEW) + glLoadMatrixf(modelView.transpose()) + glMatrixMode(GL_PROJECTION) + glLoadMatrixf(projectionMatrix.transpose()) + + glColor3f(1, 0, 1) + glLineWidth(2.0) + glCallList(obj) + + visArgs = VisualizerArgs() + visArgs.recordPath = args.recordWindow + visArgs.cameraMode = CameraMode.AR + visArgs.showPoseTrail = False + visArgs.showKeyFrames = False + visArgs.showGrid = False + visArgs.customRenderCallback = renderObj + visualizer = Visualizer(visArgs) + + def replayOnVioOutput(output, frameSet): + if output.globalPose and objPos == None: + # If we receive global pose i.e. recording contains GPS coordinates, then + # place object at the first received device coordinates if not provide + # through CLI arguments + objPos = output.globalPose.coordinates + + for frame in frameSet: + if not frame.image: continue + if not frame.index == args.cameraInd: continue + img = frame.image.toArray() + width = img.shape[1] + height = img.shape[0] + colorFormat = frame.image.getColorFormat() if output.globalPose: - cameraIndex = 0 # Gives camera pose relative to objPos that sits at [0,0,0] in ENU - cameraPose = output.globalPose.getEnuCameraPose(cameraIndex, objPos) + cameraPose = output.globalPose.getEnuCameraPose(args.cameraInd, objPos) else: cameraPose = frame.cameraPose - draw(cameraPose, width, height, img.data, obj, is_tracking) - pygame.display.flip() - break # Skip other frames - -configInternal = { "useRectification": "true" } # Undistort images for visualization (assumes undistorted pinhole model) -replay = spectacularAI.Replay(args.dataFolder, configuration=configInternal) -replay.setExtendedOutputCallback(onOutput) -replay.startReplay() -while not shouldQuit: - time.sleep(0.05) -print("Quitting...") -replay.close() + visualizer.onVioOutput(cameraPose, img, width, height, colorFormat, output.status) + + replay = spectacularAI.Replay(args.dataFolder, configuration=configInternal) + replay.setExtendedOutputCallback(replayOnVioOutput) + replay.startReplay() + visualizer.run() + replay.close() From 9dcba315a1474ae68043bc18bfb217de41287b18 Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Mon, 15 Jan 2024 15:19:44 +0200 Subject: [PATCH 3/9] Fix draw_cube with Python visualizer --- python/oak/mixed_reality.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/python/oak/mixed_reality.py b/python/oak/mixed_reality.py index df05a62..b099f92 100644 --- a/python/oak/mixed_reality.py +++ b/python/oak/mixed_reality.py @@ -56,24 +56,24 @@ def init_display(w, h): pygame.display.set_mode((w, h), DOUBLEBUF | OPENGL) def draw_cube(origin): - CUBE_VERTICES = ( - (0.1, -0.1, -0.1), (0.1, 0.1, -0.1), (-0.1, 0.1, -0.1), (-0.1, -0.1, -0.1), - (0.1, -0.1, 0.1), (0.1, 0.1, 0.1), (-0.1, -0.1, 0.1), (-0.1, 0.1, 0.1) - ) - CUBE_EDGES = ( - (0,1), (0,3), (0,4), (2,1), (2,3), (2,7), - (6,3), (6,4), (6,7), (5,1), (5,4), (5,7) - ) - - glPushMatrix() - # cube world position - glTranslatef(origin[0], origin[1], origin[2]) + import numpy as np + + CUBE_VERTICES = np.array([ + [1, -1, -1], [1, 1, -1], [-1, 1, -1], [-1, -1, -1], + [1, -1, 1], [1, 1, 1], [-1, -1, 1], [-1, 1, 1] + ], dtype=np.float32) + CUBE_EDGES = np.array([ + [0,1], [0,3], [0,4], [2,1], [2,3], [2,7], + [6,3], [6,4], [6,7], [5,1], [5,4], [5,7] + ]) + CUBE_VERTICES *= 0.1 + CUBE_VERTICES += origin + glBegin(GL_LINES) for edge in CUBE_EDGES: for vertex in edge: glVertex3fv(CUBE_VERTICES[vertex]) glEnd() - glPopMatrix() def load_and_draw_obj_as_wireframe(in_stream): vertices = [] From 6c0b36f9adabfaa795dba14ed140e7308caf9d58 Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Mon, 15 Jan 2024 17:09:07 +0200 Subject: [PATCH 4/9] Clean custom render callback --- python/oak/mixed_reality_replay.py | 9 --------- 1 file changed, 9 deletions(-) diff --git a/python/oak/mixed_reality_replay.py b/python/oak/mixed_reality_replay.py index f246800..0613383 100644 --- a/python/oak/mixed_reality_replay.py +++ b/python/oak/mixed_reality_replay.py @@ -48,15 +48,6 @@ def renderObj(): if obj is None: obj = load_obj(args.objLoadPath) - viewMatrix = visualizer.getViewMatrix() - projectionMatrix = visualizer.getProjectionMatrix() - - modelView = viewMatrix - glMatrixMode(GL_MODELVIEW) - glLoadMatrixf(modelView.transpose()) - glMatrixMode(GL_PROJECTION) - glLoadMatrixf(projectionMatrix.transpose()) - glColor3f(1, 0, 1) glLineWidth(2.0) glCallList(obj) From f97c6b40be8a39e133091875e7972049b6dbd9a4 Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Mon, 15 Jan 2024 17:09:37 +0200 Subject: [PATCH 5/9] Clean OpenGL stuff in mixed_reality.py --- python/oak/mixed_reality.py | 33 ++++++++++++++++++--------------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/python/oak/mixed_reality.py b/python/oak/mixed_reality.py index b099f92..91e42fe 100644 --- a/python/oak/mixed_reality.py +++ b/python/oak/mixed_reality.py @@ -56,24 +56,26 @@ def init_display(w, h): pygame.display.set_mode((w, h), DOUBLEBUF | OPENGL) def draw_cube(origin): - import numpy as np - - CUBE_VERTICES = np.array([ - [1, -1, -1], [1, 1, -1], [-1, 1, -1], [-1, -1, -1], - [1, -1, 1], [1, 1, 1], [-1, -1, 1], [-1, 1, 1] - ], dtype=np.float32) - CUBE_EDGES = np.array([ - [0,1], [0,3], [0,4], [2,1], [2,3], [2,7], - [6,3], [6,4], [6,7], [5,1], [5,4], [5,7] - ]) - CUBE_VERTICES *= 0.1 - CUBE_VERTICES += origin + CUBE_VERTICES = ( + (1, -1, -1), (1, 1, -1), (-1, 1, -1), (-1, -1, -1), + (1, -1, 1), (1, 1, 1), (-1, -1, 1), (-1, 1, 1) + ) + CUBE_EDGES = ( + (0,1), (0,3), (0,4), (2,1), (2,3), (2,7), + (6,3), (6,4), (6,7), (5,1), (5,4), (5,7) + ) + + glPushMatrix() + # cube world position + glTranslatef(origin[0], origin[1], origin[2]) + glScalef(*([0.1] * 3)) glBegin(GL_LINES) for edge in CUBE_EDGES: for vertex in edge: glVertex3fv(CUBE_VERTICES[vertex]) glEnd() + glPopMatrix() def load_and_draw_obj_as_wireframe(in_stream): vertices = [] @@ -109,10 +111,11 @@ def draw(cam, width, height, data, obj, is_tracking): if not is_tracking: return # setup OpenGL camera based on VIO output - glLoadIdentity() near, far = 0.01, 100.0 # clip - glMultMatrixd(cam.camera.getProjectionMatrixOpenGL(near, far).transpose()) - glMultMatrixd(cam.getWorldToCameraMatrix().transpose()) + glMatrixMode(GL_PROJECTION) + glLoadMatrixf(cam.camera.getProjectionMatrixOpenGL(near, far).transpose()) + glMatrixMode(GL_MODELVIEW) + glLoadMatrixf(cam.getWorldToCameraMatrix().transpose()) glClear(GL_DEPTH_BUFFER_BIT) glColor3f(1, 0, 1) From dc8aaf1f087855d896e5417bad0ebbce76b61f5a Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Fri, 26 Jan 2024 10:43:00 +0200 Subject: [PATCH 6/9] Move mapping visualization example to sdk-repo (integrate with cpp-record-tool) --- cpp/common/serialize_output.cpp | 130 ------------------------------ cpp/common/serialize_output.hpp | 28 ------- cpp/k4a/CMakeLists.txt | 11 --- cpp/k4a/build_windows.sh | 2 - cpp/k4a/mapping_visu.cpp | 107 ------------------------ cpp/mapping_visu/CMakeLists.txt | 14 ---- cpp/mapping_visu/README.md | 45 ----------- cpp/mapping_visu/mapping_visu.cpp | 92 --------------------- cpp/orbbec/CMakeLists.txt | 12 --- cpp/orbbec/build_windows.sh | 2 - cpp/orbbec/mapping_visu.cpp | 102 ----------------------- cpp/realsense/CMakeLists.txt | 5 +- cpp/realsense/vio_mapper.cpp | 65 +++++++-------- 13 files changed, 33 insertions(+), 582 deletions(-) delete mode 100644 cpp/common/serialize_output.cpp delete mode 100644 cpp/common/serialize_output.hpp delete mode 100644 cpp/k4a/mapping_visu.cpp delete mode 100644 cpp/mapping_visu/CMakeLists.txt delete mode 100644 cpp/mapping_visu/README.md delete mode 100644 cpp/mapping_visu/mapping_visu.cpp delete mode 100644 cpp/orbbec/mapping_visu.cpp diff --git a/cpp/common/serialize_output.cpp b/cpp/common/serialize_output.cpp deleted file mode 100644 index 99f5403..0000000 --- a/cpp/common/serialize_output.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include - -#include "serialize_output.hpp" - -namespace { - - nlohmann::json serializeCamera(const spectacularAI::Camera &camera) { - float near = 0.01f, far = 100.0f; - const Matrix3d &intrinsics = camera.getIntrinsicMatrix(); - const Matrix4d &projectionMatrixOpenGL = camera.getProjectionMatrixOpenGL(near, far); - - nlohmann::json json; - json["intrinsics"] = intrinsics; - json["projectionMatrixOpenGL"] = projectionMatrixOpenGL; - return json; - } - -} // anonymous namespace - -void Serializer::serializeVioOutput(std::ofstream &outputStream, spectacularAI::VioOutputPtr vioOutput) { - const spectacularAI::Camera &camera = *vioOutput->getCameraPose(0).camera; - const Matrix4d &cameraToWorld = vioOutput->getCameraPose(0).getCameraToWorldMatrix(); - - // Only properties used in current visualization are serialized, i.e. add more stuff if needed. - nlohmann::json json; - json["cameraPoses"] = { - { - {"camera", serializeCamera(camera)}, - {"cameraToWorld", cameraToWorld} - } - }; - json["trackingStatus"] = static_cast(vioOutput->status); - - std::string jsonStr = json.dump(); - uint32_t jsonLength = jsonStr.length(); - - MessageHeader header = { - .magicBytes = MAGIC_BYTES, - .messageId = messageIdCounter, - .jsonSize = jsonLength, - .binarySize = 0 - }; - - outputStream.write(reinterpret_cast(&header), sizeof(MessageHeader)); - outputStream.write(jsonStr.c_str(), jsonStr.size()); - outputStream.flush(); - - messageIdCounter++; -} - -void Serializer::serializeMappingOutput(std::ofstream &outputStream, spectacularAI::mapping::MapperOutputPtr mapperOutput) { - std::map jsonKeyFrames; - std::size_t binaryLength = 0; - - for (auto keyFrameId : mapperOutput->updatedKeyFrames) { - auto search = mapperOutput->map->keyFrames.find(keyFrameId); - if (search == mapperOutput->map->keyFrames.end()) continue; // deleted frame, skip - auto& frameSet = search->second->frameSet; - auto& pointCloud = search->second->pointCloud; - const spectacularAI::Camera &camera = *frameSet->primaryFrame->cameraPose.camera; - const Matrix4d &cameraToWorld = frameSet->primaryFrame->cameraPose.getCameraToWorldMatrix(); - nlohmann::json keyFrameJson; - keyFrameJson["id"] = std::to_string(keyFrameId); - keyFrameJson["frameSet"] = { - {"primaryFrame", { - {"cameraPose", { - {"camera", serializeCamera(camera)}, - {"cameraToWorld", cameraToWorld} - }} - }} - }; - std::size_t points = pointCloud->size(); - if (points > 0) { - keyFrameJson["pointCloud"] = { - {"size", points }, - {"hasNormals", pointCloud->hasNormals() }, - {"hasColors", pointCloud->hasColors() }, - }; - binaryLength += points * sizeof(spectacularAI::Vector3f); - if (pointCloud->hasNormals()) binaryLength += points * sizeof(spectacularAI::Vector3f); - if (pointCloud->hasColors()) binaryLength += points * sizeof(std::uint8_t) * 3; - } - jsonKeyFrames[keyFrameJson["id"]] = keyFrameJson; - } - - nlohmann::json json; - json["updatedKeyFrames"] = mapperOutput->updatedKeyFrames; - json["map"] = {{"keyFrames", jsonKeyFrames}}; - json["finalMap"] = mapperOutput->finalMap; - - std::string jsonStr = json.dump(); - uint32_t jsonLength = jsonStr.length(); - MessageHeader header = { - .magicBytes = MAGIC_BYTES, - .messageId = messageIdCounter, - .jsonSize = jsonLength, - .binarySize = (uint32_t)binaryLength - }; - - outputStream.write(reinterpret_cast(&header), sizeof(MessageHeader)); - outputStream.write(jsonStr.c_str(), jsonStr.size()); - - for (auto keyFrameId : mapperOutput->updatedKeyFrames) { - auto search = mapperOutput->map->keyFrames.find(keyFrameId); - if (search == mapperOutput->map->keyFrames.end()) continue; - auto& pointCloud = search->second->pointCloud; - std::size_t points = pointCloud->size(); - if (points > 0) { - outputStream.write( - reinterpret_cast(pointCloud->getPositionData()), - sizeof(spectacularAI::Vector3f) * points); - - if (pointCloud->hasNormals()) { - outputStream.write( - reinterpret_cast(pointCloud->getNormalData()), - sizeof(spectacularAI::Vector3f) * points); - } - - if (pointCloud->hasColors()) { - outputStream.write( - reinterpret_cast(pointCloud->getRGB24Data()), - sizeof(std::uint8_t) * 3 * points); - } - } - } - - outputStream.flush(); - messageIdCounter++; -} - diff --git a/cpp/common/serialize_output.hpp b/cpp/common/serialize_output.hpp deleted file mode 100644 index 1e5ba5e..0000000 --- a/cpp/common/serialize_output.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include - -#include -#include - -// Random number indicating start of a MessageHeader -#define MAGIC_BYTES 2727221974 - -using Matrix3d = std::array, 3>; -using Matrix4d = std::array, 4>; -using SAI_BOOL = uint8_t; - -struct MessageHeader { - uint32_t magicBytes; - uint32_t messageId; // Counter for debugging - uint32_t jsonSize; - uint32_t binarySize; -}; - -class Serializer { -public: - void serializeVioOutput(std::ofstream &outputStream, spectacularAI::VioOutputPtr vioOutput); - void serializeMappingOutput(std::ofstream &outputStream, spectacularAI::mapping::MapperOutputPtr mapperOutput); - -private: - uint32_t messageIdCounter = 0; -}; diff --git a/cpp/k4a/CMakeLists.txt b/cpp/k4a/CMakeLists.txt index 0a55202..d3356b3 100644 --- a/cpp/k4a/CMakeLists.txt +++ b/cpp/k4a/CMakeLists.txt @@ -49,14 +49,3 @@ if(MSVC) COMMAND_EXPAND_LISTS ) endif() - -set(BUILD_MAPPING_VISU ON CACHE STRING "Build VIO mapper example (requires nlohmann_json)") -if (BUILD_MAPPING_VISU) - set(JSON_Install OFF CACHE INTERNAL "") - set(JSON_BuildTests OFF CACHE INTERNAL "") - add_subdirectory("../offline/json" json) - - add_executable(mapping_visu mapping_visu.cpp ../common/serialize_output.cpp) - target_link_libraries(mapping_visu PRIVATE ${EXAMPLE_LIBS} nlohmann_json::nlohmann_json Threads::Threads) - target_include_directories(mapping_visu PRIVATE "../common") -endif() diff --git a/cpp/k4a/build_windows.sh b/cpp/k4a/build_windows.sh index 6a62db4..94e817b 100644 --- a/cpp/k4a/build_windows.sh +++ b/cpp/k4a/build_windows.sh @@ -13,7 +13,6 @@ spectacularAI_k4aPlugin_DIR=$1 : "${WINDOWS_SDK_VERSION:=10.0.19041.0}" : "${WINDOWS_SDK_VERSION:=10.0.19041.0}" : "${VISUAL_STUDIO_VERSION:=Visual Studio 16 2019}" -: "${BUILD_MAPPING_VISU:=ON}" CMAKE_FLAGS=(-G "${VISUAL_STUDIO_VERSION}" -A x64 -DCMAKE_SYSTEM_VERSION=${WINDOWS_SDK_VERSION}) @@ -30,7 +29,6 @@ cd "$TARGET" cmake "${CMAKE_FLAGS[@]}" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" \ -Dk4a_DIR="${K4A_SDK_FOLDER}" \ -DspectacularAI_k4aPlugin_DIR="$spectacularAI_k4aPlugin_DIR" \ - -DBUILD_MAPPING_VISU="$BUILD_MAPPING_VISU" \ .. cmake --build . --config $BUILD_TYPE diff --git a/cpp/k4a/mapping_visu.cpp b/cpp/k4a/mapping_visu.cpp deleted file mode 100644 index e39d548..0000000 --- a/cpp/k4a/mapping_visu.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include "serialize_output.hpp" - -const std::string SEPARATOR = "/"; - -void showUsage() { - std::cout << "Valid input arguments are:\n " - "-o output_file,\n " - "-r recording_folder,\n " - "-fps [5, 15, 30 (default)]\n " - "-h" - << std::endl; -} - -int main(int argc, char *argv[]) { - std::vector arguments(argv, argv + argc); - std::string outputFile; - spectacularAI::k4aPlugin::Configuration config; - config.internalParameters = { - {"computeStereoPointCloud", "true"} // enables point cloud colors - }; - - for (size_t i = 1; i < arguments.size(); ++i) { - const std::string &argument = arguments.at(i); - if (argument == "-o") outputFile = arguments.at(++i); - else if (argument == "-r") config.recordingFolder = arguments.at(++i); - else if (argument == "-fps") { - int fps = std::stoi(arguments.at(++i)); - if (fps == 5) { - config.k4aConfig.camera_fps = K4A_FRAMES_PER_SECOND_5; - } else if (fps == 15) { - config.k4aConfig.camera_fps = K4A_FRAMES_PER_SECOND_15; - } else if (fps == 30) { - config.k4aConfig.camera_fps = K4A_FRAMES_PER_SECOND_30; - } else { - std::cout << "Valid camera FPS options are [5, 15, 30 (default)]: " << fps << std::endl; - exit(1); - } - } else if (argument == "-h") { - showUsage(); - exit(0); - } else { - std::cout << "Unknown argument: " << argument << std::endl; - showUsage(); - exit(1); - } - } - - if (outputFile.empty()) { - std::cout << "Please specify output `-o`."<< std::endl; - return 0; - } - - std::ofstream outputStream(outputFile.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); - if (!outputStream.is_open()) { - std::cerr << "Failed to open file: " << outputFile << std::endl; - return EXIT_FAILURE; - } - - // Vio and Mapping outputs can come from different threads (`separateOutputThreads: True`), prevent mixing output stream by mutex - std::mutex m; - Serializer serializer; - - // Create vio pipeline using the config, and then start k4a device and vio with a mapper callback. - spectacularAI::k4aPlugin::Pipeline vioPipeline(config, - [&](spectacularAI::mapping::MapperOutputPtr mappingOutput) { - // Serialize mapping output for point cloud - std::lock_guard lock(m); - serializer.serializeMappingOutput(outputStream, mappingOutput); - } - ); - - std::atomic shouldQuit(false); - std::thread inputThread([&]() { - std::cout << "Press Enter to quit." << std::endl << std::endl; - getchar(); - shouldQuit = true; - }); - - // Add scope so that session dtor is called and final SLAM map is also serialized. - { - // Start k4a device and vio - auto session = vioPipeline.startSession(); - - while (!shouldQuit) { - if (session->hasOutput()) { - auto vioOutput = session->getOutput(); - std::lock_guard lock(m); - serializer.serializeVioOutput(outputStream, vioOutput); - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - } - - std::cout << "Exiting." << std::endl; - inputThread.join(); - outputStream.close(); - - return 0; -} diff --git a/cpp/mapping_visu/CMakeLists.txt b/cpp/mapping_visu/CMakeLists.txt deleted file mode 100644 index f8c2cf2..0000000 --- a/cpp/mapping_visu/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.3) - -project(spectacularAI_mapping_visu_example) - -find_package(spectacularAI REQUIRED) -find_package(Threads REQUIRED) - -set(JSON_Install OFF CACHE INTERNAL "") -set(JSON_BuildTests OFF CACHE INTERNAL "") -add_subdirectory("../offline/json" json) - -add_executable(mapping_visu mapping_visu.cpp ../common/serialize_output.cpp) -target_link_libraries(mapping_visu PRIVATE spectacularAI::spectacularAI nlohmann_json::nlohmann_json Threads::Threads) -target_include_directories(mapping_visu PRIVATE ".") diff --git a/cpp/mapping_visu/README.md b/cpp/mapping_visu/README.md deleted file mode 100644 index 45a8968..0000000 --- a/cpp/mapping_visu/README.md +++ /dev/null @@ -1,45 +0,0 @@ -# Spectacular AI Mapping visualization example - -See replay example for how to use Replay API. This example combines it with Mapping API and serializes the output in custom format tha can be sent to Python for visualization. - -* Tested platforms: Linux -* Dependencies: CMake, FFmpeg (for video input) - -## Setup - -* Install the Spectacular AI SDK -* Build this example using CMake: - -``` -mkdir target -cd target -cmake -DspectacularAI_DIR= .. -make -``` - -The `-DspectacularAI_DIR` option is not needed is you have used `sudo make install` for the SDK. - -## Usage - -Create a new pipe with: -``` -mkfifo ~/my_pipe -``` - -Launch python visualization and leave it running with: -``` -python mapping_visu.py --file ~/my_pipe -``` - -And finally launch this example file with: -``` -./mapping_visu -i -o ~/my_pipe -``` - -You should now the visualization running in Open3D window. - -## Copyright - -For access to the C++ SDK, contact us at . - -Available for multiple OSes and CPU architectures. diff --git a/cpp/mapping_visu/mapping_visu.cpp b/cpp/mapping_visu/mapping_visu.cpp deleted file mode 100644 index 6a75494..0000000 --- a/cpp/mapping_visu/mapping_visu.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -#include "serialize_output.hpp" - -const std::string SEPARATOR = "/"; - -std::string readFileToString(const std::string &filename) { - std::ifstream f(filename); - std::ostringstream oss; - oss << f.rdbuf(); - return oss.str(); -} - -bool fileExists(const std::string &filePath) { - std::ifstream dataFile(filePath); - return dataFile.is_open(); -} - -int main(int argc, char *argv[]) { - std::vector arguments(argv, argv + argc); - std::string inputFolder; - std::string outputFile; - - for (size_t i = 1; i < arguments.size(); ++i) { - const std::string &argument = arguments.at(i); - if (argument == "-i") inputFolder = arguments.at(++i); - else if (argument == "-o") outputFile = arguments.at(++i); - else { - std::cout << "Unknown argument: " << argument << std::endl; - exit(1); - } - } - if (inputFolder.empty()) { - std::cout << "Please specify input directory using `-i`."<< std::endl; - return 0; - } - - if (outputFile.empty()) { - std::cout << "Please specify output `-o`."<< std::endl; - return 0; - } - - std::ostringstream configurationYaml; - std::string dataConfigurationYamlPath = inputFolder + SEPARATOR + "vio_config.yaml"; - if (fileExists(dataConfigurationYamlPath)) { - configurationYaml << readFileToString(dataConfigurationYamlPath) << std::endl; - } - // Ensure pointcloud output is enabled - configurationYaml << "computeDenseStereoDepthKeyFramesOnly: True\n"; - configurationYaml << "computeStereoPointCloud: True\n"; - configurationYaml << "useRectification: True\n"; // TODO: Should check for alreadyRectified - - Serializer serializer; - // Vio and Mapping outputs come from different threads, prevent mixing output stream by mutex - std::mutex m; - - std::ofstream outputStream(outputFile.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); - if (!outputStream.is_open()) { - std::cerr << "Failed to open file: " << outputFile << std::endl; - return EXIT_FAILURE; - } - - spectacularAI::Vio::Builder vioBuilder = spectacularAI::Vio::builder() - .setConfigurationYAML(configurationYaml.str()); - - vioBuilder.setMapperCallback([&](spectacularAI::mapping::MapperOutputPtr mappingOutput) { - std::lock_guard lock(m); - serializer.serializeMappingOutput(outputStream, mappingOutput); - }); - - std::unique_ptr replay - = spectacularAI::Replay::builder(inputFolder, vioBuilder).build(); - replay->setPlaybackSpeed(-1); - - replay->setOutputCallback([&](spectacularAI::VioOutputPtr vioOutput) { - std::lock_guard lock(m); - serializer.serializeVioOutput(outputStream, vioOutput); - }); - - replay->runReplay(); - - outputStream.close(); - - return 0; -} diff --git a/cpp/orbbec/CMakeLists.txt b/cpp/orbbec/CMakeLists.txt index 43e9a24..b3e7250 100644 --- a/cpp/orbbec/CMakeLists.txt +++ b/cpp/orbbec/CMakeLists.txt @@ -49,15 +49,3 @@ if(MSVC) COMMAND_EXPAND_LISTS ) endif() - -# Mapping example, used together with mapping_visu.py visulization -set(BUILD_MAPPING_VISU ON CACHE STRING "Build VIO mapper example (requires nlohmann_json)") -if (BUILD_MAPPING_VISU) - set(JSON_Install OFF CACHE INTERNAL "") - set(JSON_BuildTests OFF CACHE INTERNAL "") - add_subdirectory("../offline/json" json) - - add_executable(mapping_visu mapping_visu.cpp ../common/serialize_output.cpp) - target_link_libraries(mapping_visu PRIVATE ${EXAMPLE_LIBS} nlohmann_json::nlohmann_json Threads::Threads) - target_include_directories(mapping_visu PRIVATE "../common") -endif() diff --git a/cpp/orbbec/build_windows.sh b/cpp/orbbec/build_windows.sh index 47419fd..2ef7532 100644 --- a/cpp/orbbec/build_windows.sh +++ b/cpp/orbbec/build_windows.sh @@ -19,7 +19,6 @@ OrbbecSDK_DIR=$2 : "${WINDOWS_SDK_VERSION:=10.0.19041.0}" : "${WINDOWS_SDK_VERSION:=10.0.19041.0}" : "${VISUAL_STUDIO_VERSION:=Visual Studio 16 2019}" -: "${BUILD_MAPPING_VISU:=ON}" CMAKE_FLAGS=(-G "${VISUAL_STUDIO_VERSION}" -A x64 -DCMAKE_SYSTEM_VERSION=${WINDOWS_SDK_VERSION}) @@ -31,6 +30,5 @@ cd "$TARGET" cmake "${CMAKE_FLAGS[@]}" \ -DOrbbecSDK_DIR="${OrbbecSDK_DIR}" \ -DspectacularAI_orbbecPlugin_DIR="$spectacularAI_orbbecPlugin_DIR" \ - -DBUILD_MAPPING_VISU="$BUILD_MAPPING_VISU" \ .. cmake --build . --config $BUILD_TYPE diff --git a/cpp/orbbec/mapping_visu.cpp b/cpp/orbbec/mapping_visu.cpp deleted file mode 100644 index a76cf67..0000000 --- a/cpp/orbbec/mapping_visu.cpp +++ /dev/null @@ -1,102 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include "serialize_output.hpp" - -const std::string SEPARATOR = "/"; - -void showUsage() { - std::cout << "Valid input arguments are:\n " - "-o output_file,\n " - "-r recording_folder,\n " - "-h" - << std::endl; -} - -int main(int argc, char *argv[]) { - std::vector arguments(argv, argv + argc); - std::string outputFile; - - ob::Context::setLoggerSeverity(OB_LOG_SEVERITY_OFF); - - // Create OrbbecSDK pipeline (with default device). - ob::Pipeline obPipeline; - - // Create Spectacular AI orbbec plugin configuration (depends on device type). - spectacularAI::orbbecPlugin::Configuration config(obPipeline); - config.internalParameters = { - {"computeStereoPointCloud", "true"} // enables point cloud colors - }; - - for (size_t i = 1; i < arguments.size(); ++i) { - const std::string &argument = arguments.at(i); - if (argument == "-o") outputFile = arguments.at(++i); - else if (argument == "-r") config.recordingFolder = arguments.at(++i); - else if (argument == "-h") { - showUsage(); - exit(0); - } else { - std::cout << "Unknown argument: " << argument << std::endl; - showUsage(); - exit(1); - } - } - - if (outputFile.empty()) { - std::cout << "Please specify output `-o`."<< std::endl; - return 0; - } - - std::ofstream outputStream(outputFile.c_str(), std::ios::out | std::ios::binary | std::ios::trunc); - if (!outputStream.is_open()) { - std::cerr << "Failed to open file: " << outputFile << std::endl; - return EXIT_FAILURE; - } - - // Vio and Mapping outputs can come from different threads (`separateOutputThreads: True`), prevent mixing output stream by mutex - std::mutex m; - Serializer serializer; - - // Create vio pipeline using the config & setup orbbec pipeline - spectacularAI::orbbecPlugin::Pipeline vioPipeline(obPipeline, config, - [&](spectacularAI::mapping::MapperOutputPtr mappingOutput) { - // Serialize mapping output for point cloud - std::lock_guard lock(m); - serializer.serializeMappingOutput(outputStream, mappingOutput); - } - ); - - std::atomic shouldQuit(false); - std::thread inputThread([&]() { - std::cout << "Press Enter to quit." << std::endl << std::endl; - getchar(); - shouldQuit = true; - }); - - // Add scope so that session dtor is called and final SLAM map is also serialized. - { - // Start orbbec device and vio - auto session = vioPipeline.startSession(); - - while (!shouldQuit) { - if (session->hasOutput()) { - auto vioOutput = session->getOutput(); - std::lock_guard lock(m); - serializer.serializeVioOutput(outputStream, vioOutput); - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - } - - std::cout << "Exiting." << std::endl; - inputThread.join(); - outputStream.close(); - - return 0; -} diff --git a/cpp/realsense/CMakeLists.txt b/cpp/realsense/CMakeLists.txt index 87a4746..d7c3698 100644 --- a/cpp/realsense/CMakeLists.txt +++ b/cpp/realsense/CMakeLists.txt @@ -38,13 +38,14 @@ endif() set(BUILD_MAPPER ON CACHE STRING "Build VIO mapper example (requires OpenCV)") if (BUILD_MAPPER) + find_package(Threads REQUIRED) set(USE_STATIC_OPENCV OFF CACHE STRING "Use OpenCV as statically linked library (internal and unsupported flag, do not use)") if (USE_STATIC_OPENCV) find_package(mobile-cv-suite REQUIRED) - set(MAPPER_LIBS mobile-cv-suite::static mobile-cv-suite::imgcodecs) + set(MAPPER_LIBS mobile-cv-suite::static mobile-cv-suite::imgcodecs Threads::Threads) else() find_package(OpenCV REQUIRED) - set(MAPPER_LIBS "${OpenCV_LIBS}") + set(MAPPER_LIBS "${OpenCV_LIBS}" Threads::Threads) endif() add_executable(vio_mapper vio_mapper.cpp) diff --git a/cpp/realsense/vio_mapper.cpp b/cpp/realsense/vio_mapper.cpp index eef08fe..b328bfe 100644 --- a/cpp/realsense/vio_mapper.cpp +++ b/cpp/realsense/vio_mapper.cpp @@ -113,48 +113,20 @@ int main(int argc, char** argv) { return 1; } - spectacularAI::rsPlugin::Configuration vioConfig; - vioConfig.useSlam = true; - spectacularAI::rsPlugin::Pipeline vioPipeline(vioConfig); - - { - // Find RealSense device - rs2::context rsContext; - rs2::device_list devices = rsContext.query_devices(); - if (devices.size() != 1) { - std::cout << "Connect exactly one RealSense device." << std::endl; - return EXIT_SUCCESS; - } - rs2::device device = devices.front(); - vioPipeline.configureDevice(device); - } - - // Start pipeline - rs2::config rsConfig; - vioPipeline.configureStreams(rsConfig); - - // VIO works fine with BGR-flipped data too - rsConfig.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_BGR8); - // The RS callback thread should not be blocked for long. // Using worker threads for image encoding and disk I/O + std::atomic shouldQuit(false); constexpr int N_WORKER_THREADS = 4; std::mutex queueMutex; std::deque imageQueue; - std::atomic shouldQuit(false); - - std::vector imageWriterThreads; - for (int i = 0; i < N_WORKER_THREADS; ++i) { - imageWriterThreads.emplace_back(buildImageWriter(imageQueue, queueMutex, shouldQuit)); - } - + std::ofstream posesFile = std::ofstream(recordingFolder + "/poses.jsonl"); + std::set savedFrames; std::vector fileNameBuf; fileNameBuf.resize(1000, 0); - std::shared_ptr vioSession; - std::ofstream posesFile = std::ofstream(recordingFolder + "/poses.jsonl"); - std::set savedFrames; - vioPipeline.setMapperCallback([&](std::shared_ptr output){ + // Create vio pipeline with mapper callback + spectacularAI::rsPlugin::Configuration vioConfig; + spectacularAI::rsPlugin::Pipeline vioPipeline(vioConfig, [&](std::shared_ptr output) { for (int64_t frameId : output->updatedKeyFrames) { auto search = output->map->keyFrames.find(frameId); if (search == output->map->keyFrames.end()) { @@ -189,8 +161,31 @@ int main(int argc, char** argv) { } }); - vioSession = vioPipeline.startSession(rsConfig); + { + // Find RealSense device + rs2::context rsContext; + rs2::device_list devices = rsContext.query_devices(); + if (devices.size() != 1) { + std::cout << "Connect exactly one RealSense device." << std::endl; + return EXIT_SUCCESS; + } + rs2::device device = devices.front(); + vioPipeline.configureDevice(device); + } + + // Start pipeline + rs2::config rsConfig; + vioPipeline.configureStreams(rsConfig); + + // VIO works fine with BGR-flipped data too + rsConfig.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_BGR8); + + std::vector imageWriterThreads; + for (int i = 0; i < N_WORKER_THREADS; ++i) { + imageWriterThreads.emplace_back(buildImageWriter(imageQueue, queueMutex, shouldQuit)); + } + auto vioSession = vioPipeline.startSession(rsConfig); std::thread inputThread([&]() { std::cerr << "Press Enter to quit." << std::endl << std::endl; std::getchar(); From 90db817cb38d259bb88c44a72191b74dc98a6a87 Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Fri, 26 Jan 2024 13:03:21 +0200 Subject: [PATCH 7/9] Remove --file option from mapping_visu.py --- python/oak/mapping_visu.py | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/python/oak/mapping_visu.py b/python/oak/mapping_visu.py index ce7a973..1c7fcd6 100644 --- a/python/oak/mapping_visu.py +++ b/python/oak/mapping_visu.py @@ -10,14 +10,12 @@ import depthai import threading -from spectacularAI.cli.visualization.serialization import input_stream_reader, MockVioOutput, MockMapperOutput from spectacularAI.cli.visualization.visualizer import Visualizer, VisualizerArgs def parseArgs(): import argparse p = argparse.ArgumentParser(__doc__) p.add_argument("--dataFolder", help="Instead of running live mapping session, replay session from this folder") - p.add_argument('--file', type=argparse.FileType('rb'), help='Read data from file or pipe, using this with mapping_visu C++ example', default=None) p.add_argument("--recordingFolder", help="Record live mapping session for replay") p.add_argument("--resolution", help="Window resolution", default="1280x720") p.add_argument("--fullScreen", help="Start in full screen mode", action="store_true") @@ -50,6 +48,7 @@ def parseArgs(): visArgs.recordPath = args.recordWindow visArgs.pointCloudVoxelSize = args.voxel visArgs.skipPointsWithoutColor = args.color + visArgs.targetFps = 0 if args.dataFolder else 30 visualizer = Visualizer(visArgs) def onMappingOutput(mapperOutput): @@ -59,20 +58,7 @@ def onMappingOutput(mapperOutput): def onVioOutput(vioOutput): visualizer.onVioOutput(vioOutput.getCameraPose(0), status=vioOutput.status) - if args.file: - print("Starting reading input from file or pipe") - def inputStreamLoop(): - vioSource = input_stream_reader(args.file) - for output in vioSource: - if 'cameraPoses' in output: - vioOutput = MockVioOutput(output) - onVioOutput(vioOutput) - else: onMappingOutput(MockMapperOutput(output)) - thread = threading.Thread(target=inputStreamLoop) - thread.start() - visualizer.run() - thread.join() - elif args.dataFolder: + if args.dataFolder: print("Starting replay") replay = spectacularAI.Replay(args.dataFolder, onMappingOutput, configuration=configInternal) replay.setOutputCallback(onVioOutput) From 397d0874f63b9be669e3f5ff1a9f4f329d38dcae Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Fri, 26 Jan 2024 13:04:58 +0200 Subject: [PATCH 8/9] Fix requirements in mapping_visu.py --- python/oak/mapping_visu.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/python/oak/mapping_visu.py b/python/oak/mapping_visu.py index 1c7fcd6..bd4583f 100644 --- a/python/oak/mapping_visu.py +++ b/python/oak/mapping_visu.py @@ -1,9 +1,7 @@ """ Visualize 3D point cloud of the environment in real-time, or playback your recordings and view their 3D point cloud. -Requirements: pygame, numpy, PyOpenGL -Optional: PyOpenGL_accelerate - +Requirements: pip install spectacularAI[full] """ import spectacularAI From 11dea35c5818c5f84511e2df94ed77d09365c923 Mon Sep 17 00:00:00 2001 From: kaatrasa Date: Fri, 26 Jan 2024 16:07:15 +0200 Subject: [PATCH 9/9] Retire RTAB-Map example --- cpp/rtabmap/.gitignore | 1 - cpp/rtabmap/CMakeLists.txt | 108 --- cpp/rtabmap/README.md | 54 -- .../spectacularAI/rtabmap/camera_k4a.h | 48 -- .../spectacularAI/rtabmap/camera_oak.h | 45 -- .../spectacularAI/rtabmap/camera_realsense.h | 43 - .../spectacularAI/rtabmap/camera_replay.h | 36 - .../rtabmap/camera_spectacularai.h | 42 - .../spectacularAI/rtabmap/map_builder.h | 323 -------- .../include/spectacularAI/rtabmap/util.h | 38 - cpp/rtabmap/rtabmap_config.ini | 746 ------------------ cpp/rtabmap/src/camera_k4a.cpp | 118 --- cpp/rtabmap/src/camera_oak.cpp | 126 --- cpp/rtabmap/src/camera_realsense.cpp | 134 ---- cpp/rtabmap/src/camera_replay.cpp | 115 --- cpp/rtabmap/src/camera_spectacularai.cpp | 67 -- cpp/rtabmap/src/main.cpp | 141 ---- cpp/rtabmap/src/util.cpp | 187 ----- 18 files changed, 2372 deletions(-) delete mode 100644 cpp/rtabmap/.gitignore delete mode 100644 cpp/rtabmap/CMakeLists.txt delete mode 100644 cpp/rtabmap/README.md delete mode 100644 cpp/rtabmap/include/spectacularAI/rtabmap/camera_k4a.h delete mode 100644 cpp/rtabmap/include/spectacularAI/rtabmap/camera_oak.h delete mode 100644 cpp/rtabmap/include/spectacularAI/rtabmap/camera_realsense.h delete mode 100644 cpp/rtabmap/include/spectacularAI/rtabmap/camera_replay.h delete mode 100644 cpp/rtabmap/include/spectacularAI/rtabmap/camera_spectacularai.h delete mode 100644 cpp/rtabmap/include/spectacularAI/rtabmap/map_builder.h delete mode 100644 cpp/rtabmap/include/spectacularAI/rtabmap/util.h delete mode 100644 cpp/rtabmap/rtabmap_config.ini delete mode 100644 cpp/rtabmap/src/camera_k4a.cpp delete mode 100644 cpp/rtabmap/src/camera_oak.cpp delete mode 100644 cpp/rtabmap/src/camera_realsense.cpp delete mode 100644 cpp/rtabmap/src/camera_replay.cpp delete mode 100644 cpp/rtabmap/src/camera_spectacularai.cpp delete mode 100644 cpp/rtabmap/src/main.cpp delete mode 100644 cpp/rtabmap/src/util.cpp diff --git a/cpp/rtabmap/.gitignore b/cpp/rtabmap/.gitignore deleted file mode 100644 index 2fa89ef..0000000 --- a/cpp/rtabmap/.gitignore +++ /dev/null @@ -1 +0,0 @@ -3rdparty* diff --git a/cpp/rtabmap/CMakeLists.txt b/cpp/rtabmap/CMakeLists.txt deleted file mode 100644 index a2726b7..0000000 --- a/cpp/rtabmap/CMakeLists.txt +++ /dev/null @@ -1,108 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rtabmap_mapper) -set(CMAKE_CXX_STANDARD 20) - -option(BUILD_REPLAY "Replay support" OFF) -option(BUILD_K4A "Azure Kinect support" OFF) -option(BUILD_REALSENSE "Realsense support" OFF) -option(BUILD_OAK "OAK-D support" OFF) - -if (NOT BUILD_REPLAY AND NOT BUILD_K4A AND NOT BUILD_REALSENSE AND NOT BUILD_OAK) - message(FATAL_ERROR " \ -You must build with support for at least one data source.\n \ -It can be one or more of: \n \ - -DBUILD_REPLAY=ON # With Core SDK\n \ - -DBUILD_K4A=ON # With Azure Kinect plugin\n \ - -DBUILD_REALSENSE=ON # With RealSense plugin\n \ - -DBUILD_OAK=ON # With OAK-D plugin\n \ -") -endif() - -find_package(RTABMap REQUIRED) -find_package(OpenCV REQUIRED) -find_package(PCL 1.7 REQUIRED) -find_package(Threads REQUIRED) - -SET(SOURCES - src/main.cpp - src/camera_spectacularai.cpp - src/camera_replay.cpp - src/camera_k4a.cpp - src/camera_realsense.cpp - src/camera_oak.cpp - src/util.cpp) - -# Find Qt5 first -find_package(Qt5 COMPONENTS Widgets Core Gui Svg QUIET) -if(NOT Qt5_FOUND) - find_package(Qt4 COMPONENTS QtCore QtGui QtSvg) -endif(NOT Qt5_FOUND) - -if(POLICY CMP0020) - cmake_policy(SET CMP0020 NEW) -endif() - -if(QT4_FOUND) - include(${QT_USE_FILE}) -endif(QT4_FOUND) - -set(INCLUDE_DIRS - ${RTABMap_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} -) - -set(LIBRARIES - ${RTABMap_LIBRARIES} - ${OpenCV_LIBRARIES} - ${QT_LIBRARIES} - ${PCL_LIBRARIES} - Threads::Threads -) - -if (BUILD_REPLAY) - find_package(spectacularAI REQUIRED) - list(APPEND LIBRARIES spectacularAI::spectacularAI) - add_definitions(-DSPECTACULARAI_CORE) -endif() - -if (BUILD_K4A) - find_package(spectacularAI_k4aPlugin REQUIRED) - list(APPEND LIBRARIES spectacularAI::k4aPlugin) - add_definitions(-DSPECTACULARAI_CAMERA_K4A) -endif() - -if (BUILD_REALSENSE) - find_package(realsense2 REQUIRED) - find_package(spectacularAI_realsensePlugin REQUIRED) - list(APPEND LIBRARIES - realsense2::realsense2 - spectacularAI::realsensePlugin) - add_definitions(-DSPECTACULARAI_CAMERA_REALSENSE) -endif() - -if (BUILD_OAK) - find_package(depthai REQUIRED) - find_package(spectacularAI_depthaiPlugin REQUIRED) - list(APPEND LIBRARIES - depthai::core - spectacularAI::depthaiPlugin) - add_definitions(-DSPECTACULARAI_CAMERA_OAK) -endif() - -if(QT4_FOUND) - QT4_WRAP_CPP(moc_srcs include/spectacularAI/rtabmap/map_builder.h) -else() - QT5_WRAP_CPP(moc_srcs include/spectacularAI/rtabmap/map_builder.h) -endif() - -add_executable(rtabmap_mapper ${SOURCES} ${moc_srcs}) -target_link_libraries(rtabmap_mapper ${LIBRARIES}) -target_include_directories(rtabmap_mapper PRIVATE ${INCLUDE_DIRS}) - -if(MSVC) - add_custom_command(TARGET rtabmap_mapper POST_BUILD - COMMAND ${CMAKE_COMMAND} -E copy $ $ - COMMAND_EXPAND_LISTS - ) -endif() diff --git a/cpp/rtabmap/README.md b/cpp/rtabmap/README.md deleted file mode 100644 index 67a085d..0000000 --- a/cpp/rtabmap/README.md +++ /dev/null @@ -1,54 +0,0 @@ -# RTAB-Map RGB-D mapping example - -This example demonstrates how one could implement their own 3D mapping application on top of the SpectacularAI Mapping API. -In this example, the resulting keyframe data (RGB-D images, point clouds) and the estimated poses are input to the popular [RTAB-Map SLAM library](https://github.com/introlab/rtabmap). -RTAB-Map then builds both, a 3D model and a (re-)localization map of the environment in real-time. - -## Dependencies - -For non-commercial use the SDK for Intel RealSense device is available at https://github.com/SpectacularAI/sdk. For commercial use and support for other devices, contact us at https://www.spectacularai.com/#contact. The SDK is available for multiple OSes and CPU architectures. - -Also, RTAB-Map library and its dependencies must be installed (https://github.com/introlab/rtabmap/wiki/Installation). - -For Ubuntu you can do it with: -``` -sudo apt-get update -sudo apt-get install libsqlite3-dev libpcl-dev libopencv-dev git cmake libproj-dev libqt5svg5-dev - -# (Recommended) g2o -git clone https://github.com/RainerKuemmerle/g2o.git 3rdparty/g2o -cd 3rdparty/g2o -mkdir build && cd build -cmake -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF .. -make -j4 -sudo make install - -git clone https://github.com/introlab/rtabmap.git 3rdparty/rtabmap -cd 3rdparty/rtabmap/build -cmake .. -make -j4 -sudo make install -``` - -## Build - -To build with RealSense and OAK-D support once you've downloaded and set up the [SDK](https://github.com/SpectacularAI/sdk). -``` -mkdir target && cd target -cmake -DBUILD_REALSENSE=ON -DBUILD_OAK=ON .. && make -``` - -or with Azure Kinect and replay support: -``` -cmake -DBUILD_K4A=ON -DBUILD_REALSENSE=ON -DBUILD_REPLAY=ON .. && make -``` - -## Running - -With an existing dataset, run: -``` -rtabmap_mapper driver # (driver options: replay, k4a, realsense, oak) -``` - -Optionally, you can also give the program a RTAB-Map config file with argument `--config path/to/rtabmap_config.ini`. -A config file, mainly optimized for Azure Kinect, is also provided here as `rtabmap_config.ini`. diff --git a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_k4a.h b/cpp/rtabmap/include/spectacularAI/rtabmap/camera_k4a.h deleted file mode 100644 index a481ce5..0000000 --- a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_k4a.h +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef SPECTACULAR_AI_CAMERA_K4A_HPP -#define SPECTACULAR_AI_CAMERA_K4A_HPP - -#include "camera_spectacularai.h" - -#ifdef SPECTACULARAI_CAMERA_K4A -#include -#endif - -namespace rtabmap { - -class CameraK4A : public CameraSpectacularAI { -public: - static bool available(); - -public: - CameraK4A( - const std::string &recordingFolder, - float imageRate = 0, - const Transform &localTransform = Transform::getIdentity()); - virtual ~CameraK4A(); - - virtual bool init(const std::string &calibrationFolder = ".", const std::string &cameraName = ""); - virtual bool isCalibrated() const; - virtual std::string getSerial() const; - -protected: - virtual SensorData captureImage(CameraInfo *info = 0); - -private: - void postPoseEvent(); - -#ifdef SPECTACULARAI_CAMERA_K4A - std::unique_ptr session; - std::unique_ptr vioPipeline; - - const std::string recordingFolder; - - // K4A device configuration - const std::string colorResolution = "720p"; - const int depthMode = K4A_DEPTH_MODE_WFOV_2X2BINNED; - const int frameRate = 30; -#endif -}; - -} // namespace rtabmap - -#endif // SPECTACULAR_AI_CAMERA_K4A_HPP diff --git a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_oak.h b/cpp/rtabmap/include/spectacularAI/rtabmap/camera_oak.h deleted file mode 100644 index 16a0d71..0000000 --- a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_oak.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef SPECTACULAR_AI_CAMERA_OAK_HPP -#define SPECTACULAR_AI_CAMERA_OAK_HPP - -#include "camera_spectacularai.h" - -#ifdef SPECTACULARAI_CAMERA_OAK -#include -#include -#endif - -namespace rtabmap { - -class CameraOAK : public CameraSpectacularAI { -public: - static bool available(); - -public: - CameraOAK( - const std::string &recordingFolder, - float imageRate = 0, - const Transform &localTransform = Transform::getIdentity()); - virtual ~CameraOAK(); - - virtual bool init(const std::string &calibrationFolder = ".", const std::string &cameraName = ""); - virtual bool isCalibrated() const; - virtual std::string getSerial() const; - -protected: - virtual SensorData captureImage(CameraInfo *info = 0); - -private: - void postPoseEvent(); - -#ifdef SPECTACULARAI_CAMERA_OAK - std::shared_ptr device; - std::unique_ptr session; - std::unique_ptr vioPipeline; - - const std::string recordingFolder; -#endif -}; - -} // namespace rtabmap - -#endif // SPECTACULAR_AI_CAMERA_OAK_HPP diff --git a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_realsense.h b/cpp/rtabmap/include/spectacularAI/rtabmap/camera_realsense.h deleted file mode 100644 index acf3bb6..0000000 --- a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_realsense.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef SPECTACULAR_AI_CAMERA_REALSENSE_HPP -#define SPECTACULAR_AI_CAMERA_REALSENSE_HPP - -#include "camera_spectacularai.h" - -#ifdef SPECTACULARAI_CAMERA_REALSENSE -#include -#endif - -namespace rtabmap { - -class CameraRealsense : public CameraSpectacularAI { -public: - static bool available(); - -public: - CameraRealsense( - const std::string &recordingFolder, - float imageRate = 0, - const Transform &localTransform = Transform::getIdentity()); - virtual ~CameraRealsense(); - - virtual bool init(const std::string &calibrationFolder = ".", const std::string &cameraName = ""); - virtual bool isCalibrated() const; - virtual std::string getSerial() const; - -protected: - virtual SensorData captureImage(CameraInfo *info = 0); - -private: - void postPoseEvent(); - -#ifdef SPECTACULARAI_CAMERA_REALSENSE - std::unique_ptr vioPipeline; - std::unique_ptr session; - - const std::string recordingFolder; -#endif -}; - -} // namespace rtabmap - -#endif // SPECTACULAR_AI_CAMERA_REALSENSE_HPP diff --git a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_replay.h b/cpp/rtabmap/include/spectacularAI/rtabmap/camera_replay.h deleted file mode 100644 index 08962e4..0000000 --- a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_replay.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef SPECTACULAR_AI_CAMERA_REPLAY_HPP -#define SPECTACULAR_AI_CAMERA_REPLAY_HPP - -#include "camera_spectacularai.h" - -namespace rtabmap { - -class CameraReplay : public CameraSpectacularAI { -public: - static bool available(); - -public: - CameraReplay( - const std::string &dataFolder, - float imageRate = 0, - const Transform &localTransform = Transform::getIdentity()); - virtual ~CameraReplay(); - - virtual bool init(const std::string &calibrationFolder = ".", const std::string &cameraName = ""); - virtual bool isCalibrated() const; - virtual std::string getSerial() const; - -protected: - virtual SensorData captureImage(CameraInfo *info = 0); - -private: - void startReplay(); - - const std::string dataFolder; - - std::thread replayThread; -}; - -} // namespace rtabmap - -#endif // SPECTACULAR_AI_CAMERA_REPLAY_HPP diff --git a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_spectacularai.h b/cpp/rtabmap/include/spectacularAI/rtabmap/camera_spectacularai.h deleted file mode 100644 index dfe298a..0000000 --- a/cpp/rtabmap/include/spectacularAI/rtabmap/camera_spectacularai.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef SPECTACULAR_AI_CAMERA_SPECTACULARAI_HPP -#define SPECTACULAR_AI_CAMERA_SPECTACULARAI_HPP - -#include -#include -#include -#include -#include - -#include - -namespace rtabmap { - -class CameraSpectacularAI : public Camera, public UEventsSender { -public: - static bool available(); - -public: - CameraSpectacularAI( - float imageRate = 0, - const Transform &localTransform = Transform::getIdentity()); - virtual ~CameraSpectacularAI(); - - virtual bool init(const std::string &calibrationFolder = ".", const std::string &cameraName = "") = 0; - virtual bool isCalibrated() const = 0; - virtual std::string getSerial() const = 0; - -protected: - virtual void mappingApiCallback(std::shared_ptr output); - - CameraModel model; - - bool shouldQuit = false; - uint64_t latestProcessedKeyFrameId = 0; - SensorData keyFrameData; - Transform keyFramePose; - std::mutex dataMutex; -}; - -} // namespace rtabmap - -#endif // SPECTACULAR_AI_CAMERA_SPECTACULARAI_HPP diff --git a/cpp/rtabmap/include/spectacularAI/rtabmap/map_builder.h b/cpp/rtabmap/include/spectacularAI/rtabmap/map_builder.h deleted file mode 100644 index 36ce4c1..0000000 --- a/cpp/rtabmap/include/spectacularAI/rtabmap/map_builder.h +++ /dev/null @@ -1,323 +0,0 @@ -/* -Modified from https://github.com/introlab/rtabmap/wiki/Cplusplus-RGBD-Mapping -Date: 25.05.2022 - -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke -All rights reserved. -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the Universite de Sherbrooke nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY -DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef MAPBUILDER_H_ -#define MAPBUILDER_H_ - -#include -#include -#include -#include - -#ifndef Q_MOC_RUN // Mac OS X issue -#include "rtabmap/gui/CloudViewer.h" -#include "rtabmap/core/util3d.h" -#include "rtabmap/core/util3d_filtering.h" -#include "rtabmap/core/util3d_transforms.h" -#include "rtabmap/core/RtabmapEvent.h" -#endif -#include "rtabmap/utilite/UStl.h" -#include "rtabmap/utilite/UConversion.h" -#include "rtabmap/utilite/UEventsHandler.h" -#include "rtabmap/utilite/ULogger.h" -#include "rtabmap/core/CameraEvent.h" -#include "rtabmap/core/CameraThread.h" -#include "util.h" - -using namespace rtabmap; - -// This class receives RtabmapEvent and construct/update a 3D Map -class MapBuilder : public QWidget, public UEventsHandler -{ - Q_OBJECT -public: - // Camera ownership is not transferred! - MapBuilder(CameraThread *camera = 0) : - camera_(camera), - odometryCorrection_(Transform::getIdentity()), - processingStatistics_(false), - lastOdometryProcessed_(true) - { - this->setWindowTitle(tr("3D Map")); - this->setMinimumWidth(800); - this->setMinimumHeight(600); - - cloudViewer_ = new CloudViewer(this); - - QVBoxLayout *layout = new QVBoxLayout(); - layout->setSpacing(0); - layout->setMargin(0); - layout->addWidget(cloudViewer_); - this->setLayout(layout); - - qRegisterMetaType("rtabmap::CameraEvent"); - qRegisterMetaType("rtabmap::Statistics"); - qRegisterMetaType("rtabmap::Transform"); - - QAction *pause = new QAction(this); - this->addAction(pause); - pause->setShortcut(Qt::Key_Space); - connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection())); - - QAction *fullScreen = new QAction(this); - this->addAction(fullScreen); - fullScreen->setShortcut(Qt::Key_F); - connect(fullScreen, SIGNAL(triggered()), this, SLOT(toggleFullScreen())); - } - - virtual ~MapBuilder() - { - this->unregisterFromEventsManager(); - delete cloudViewer_; - } - -protected Q_SLOTS: - virtual void pauseDetection() - { - UWARN(""); - if (camera_) - { - if (camera_->isCapturing()) - { - camera_->join(true); - } - else - { - camera_->start(); - } - } - } - - virtual void toggleFullScreen() - { - UWARN(""); - - isFullScreen_ = !isFullScreen_; - if(isFullScreen_) { - showFullScreen(); - } else { - showNormal(); - } - } - - virtual void processOdometry(const rtabmap::CameraEvent &cameraEvent) - { - if (!this->isVisible()) - { - return; - } - - Transform pose = cameraEvent.info().odomPose; - if (!pose.isNull()) - { - // 3d cloud - if (cameraEvent.data().depthOrRightRaw().cols == cameraEvent.data().imageRaw().cols && - cameraEvent.data().depthOrRightRaw().rows == cameraEvent.data().imageRaw().rows && - !cameraEvent.data().depthOrRightRaw().empty() && - (cameraEvent.data().stereoCameraModel().isValidForProjection() || cameraEvent.data().cameraModels().size())) - { - pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( - cameraEvent.data(), - cloudDecimation_, - cloudMaxDepth_); - if (cloud->size()) - { - if (!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_ * pose)) - { - UERROR("Adding cloudOdom to viewer failed!"); - } - } - else - { - cloudViewer_->setCloudVisibility("cloudOdom", false); - UWARN("Empty cloudOdom!"); - } - } - } - cloudViewer_->update(); - - lastOdometryProcessed_ = true; - } - - virtual void processStatistics(const rtabmap::Statistics &stats) - { - processingStatistics_ = true; - - //============================ - // Add RGB-D clouds - //============================ - const std::map &poses = stats.poses(); - QMap clouds = cloudViewer_->getAddedClouds(); - for (std::map::const_iterator iter = poses.lower_bound(1); iter != poses.end(); ++iter) - { - if (!iter->second.isNull()) - { - std::string cloudName = uFormat("cloud%d", iter->first); - - // 3d point cloud - if (clouds.contains(cloudName)) - { - // Update only if the pose has changed - Transform tCloud; - cloudViewer_->getPose(cloudName, tCloud); - if (tCloud.isNull() || iter->second != tCloud) - { - if (!cloudViewer_->updateCloudPose(cloudName, iter->second)) - { - UERROR("Updating pose cloud %d failed!", iter->first); - } - } - cloudViewer_->setCloudVisibility(cloudName, true); - } - else if (iter->first == stats.getLastSignatureData().id()) - { - Signature s = stats.getLastSignatureData(); - s.sensorData().uncompressData(); // make sure data is uncompressed - // Add the new cloud - pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( - s.sensorData(), - cloudDecimation_, - cloudMaxDepth_); - if (cloud->size()) - { - if (!cloudViewer_->addCloud(cloudName, cloud, iter->second)) - { - UERROR("Adding cloud %d to viewer failed!", iter->first); - } - } - else - { - UWARN("Empty cloud %d!", iter->first); - } - } - } - else - { - UWARN("Null pose for %d ?!?", iter->first); - } - } - - //============================ - // Add 3D graph (show all poses) - //============================ - cloudViewer_->removeAllGraphs(); - cloudViewer_->removeCloud("graph_nodes"); - if (poses.size()) - { - // Set graph - pcl::PointCloud::Ptr graph(new pcl::PointCloud); - pcl::PointCloud::Ptr graphNodes(new pcl::PointCloud); - for (std::map::const_iterator iter = poses.lower_bound(1); iter != poses.end(); ++iter) - { - graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z())); - } - *graphNodes = *graph; - - // add graph - cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray); - cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green); - cloudViewer_->setCloudPointSize("graph_nodes", 5); - } - - odometryCorrection_ = stats.mapCorrection(); - cloudViewer_->update(); - processingStatistics_ = false; - } - - virtual void processPose(const rtabmap::Transform &pose) - { - if (!this->isVisible()) - { - return; - } - - if (pose.isNull()) - { - // Odometry lost - cloudViewer_->setBackgroundColor(Qt::darkRed); - } - else - { - cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor()); - - // update camera position - cloudViewer_->updateCameraTargetPosition(odometryCorrection_ * pose * opticalRotation_); - } - } - - virtual bool handleEvent(UEvent *event) - { - if (event->getClassName().compare("RtabmapEvent") == 0) - { - RtabmapEvent *rtabmapEvent = (RtabmapEvent *)event; - const Statistics &stats = rtabmapEvent->getStats(); - // Statistics must be processed in the Qt thread - if (this->isVisible()) - { - QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats)); - } - } - else if (event->getClassName().compare("CameraEvent") == 0) - { - CameraEvent *cameraEvent = (CameraEvent *)event; - // Odometry must be processed in the Qt thread - if (this->isVisible() && - lastOdometryProcessed_ && - !processingStatistics_) - { - lastOdometryProcessed_ = false; // if we receive too many camera events! - QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::CameraEvent, *cameraEvent)); - } - } - else if (event->getClassName().compare("PoseEvent") == 0) - { - PoseEvent *poseEvent = (PoseEvent *)event; - const Transform &pose = poseEvent->pose(); - - if (this->isVisible()) - { - QMetaObject::invokeMethod(this, "processPose", Q_ARG(rtabmap::Transform, pose)); - } - } - return false; - } - -protected: - CloudViewer *cloudViewer_; - CameraThread *camera_; - Transform odometryCorrection_; - bool processingStatistics_; - bool lastOdometryProcessed_; - - bool isFullScreen_ = false; - float cloudMaxDepth_ = 10; - float cloudDecimation_ = 8; - Transform opticalRotation_ = Transform(0, 0, 0, 0, -M_PI/2, 0); -}; - -#endif /* MAPBUILDER_H_ */ diff --git a/cpp/rtabmap/include/spectacularAI/rtabmap/util.h b/cpp/rtabmap/include/spectacularAI/rtabmap/util.h deleted file mode 100644 index 1cdddb7..0000000 --- a/cpp/rtabmap/include/spectacularAI/rtabmap/util.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef SPECTACULAR_AI_UTIL3D_HPP -#define SPECTACULAR_AI_UTIL3D_HPP - -#include -#include -#include -#include -#include - -#include -#include - -namespace rtabmap { - -class PoseEvent: public UEvent { -public: - PoseEvent(const Transform & pose) : pose_(pose) {} - virtual std::string getClassName() const { return "PoseEvent"; } - const Transform & pose() const { return pose_; } - -private: - Transform pose_; -}; - -namespace util { - -LaserScan laserScanFromPointCloud(const std::shared_ptr &cloud); -int colorFormatToOpenCVType(spectacularAI::ColorFormat colorFormat); -cv::Mat convert(const spectacularAI::Matrix3d &m); -Transform convert(const spectacularAI::Pose &pose); -CameraModel convert(const std::shared_ptr &camera, int width, int height); -cv::Mat convertColor(const std::shared_ptr &image); -cv::Mat convertDepth(const std::shared_ptr &image); - -} // namespace util -} // namespace rtabmap - -#endif // SPECTACULAR_AI_UTIL3D_HPP diff --git a/cpp/rtabmap/rtabmap_config.ini b/cpp/rtabmap/rtabmap_config.ini deleted file mode 100644 index 5a2c0cb..0000000 --- a/cpp/rtabmap/rtabmap_config.ini +++ /dev/null @@ -1,746 +0,0 @@ -[CalibrationDialog] -board_height = 6 -board_square_size = 0.033 -board_width = 8 -geometry = @ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\0\0\0\0\0/\0\0\x5!\0\0\x3}\0\0\0\0\0\0\0/\0\0\x5!\0\0\x3}\0\0\0\0\0\0\0\0\a\x80\0\0\0\0\0\0\0/\0\0\x5!\0\0\x3}) -max_scale = 1 - - -[Camera] -Database\cameraIndex = -1 -Database\ignoreGoalDelay = true -Database\ignoreGoals = true -Database\ignoreLandmarks = false -Database\ignoreOdometry = false -Database\path = -Database\startId = 0 -Database\stopId = 0 -Database\useDatabaseStamps = true -DepthAI\confidence = 200 -DepthAI\depth = false -DepthAI\resolution = 1 -DepthFromScan\depthFromScan = false -DepthFromScan\depthFromScanFillBorders = false -DepthFromScan\depthFromScanFillHoles = true -DepthFromScan\depthFromScanHorizontal = false -DepthFromScan\depthFromScanVertical = true -Freenect2\bilateralFiltering = true -Freenect2\edgeAwareFiltering = true -Freenect2\format = 1 -Freenect2\maxDepth = 12 -Freenect2\minDepth = 0.3 -Freenect2\noiseFiltering = true -Freenect2\pipeline = -IMU\base_frame_conversion = true -IMU\complementary_adaptive_gain = true -IMU\complementary_biais_estimation = true -IMU\complementary_bias_alpha = 0.01 -IMU\complementary_gain_acc = 0.01 -IMU\madgwick_gain = 0.1 -IMU\madgwick_zeta = 0 -IMU\publish_inter_imu = false -IMU\strategy = 2 -Images\bayerMode = 0 -Images\config_each_frame = false -Images\filenames_as_stamps = false -Images\gt_format = 0 -Images\gt_path = -Images\imu_local_transform = 0 0 1 0 -1 0 1 0 0 -Images\imu_path = -Images\imu_rate = 0 -Images\maxFrames = 0 -Images\max_pose_time_diff = 0.02 -Images\odom_format = 0 -Images\odom_path = -Images\path = -Images\path_scans = -Images\scan_max_pts = 0 -Images\scan_transform = 0 0 0 0 0 0 -Images\stamps = -Images\startPos = 0 -Images\sync_stamps = true -K4A\depth_resolution = 2 -K4A\framerate = 2 -K4A\ir = false -K4A\mkvPath = -K4A\rgb_resolution = 0 -K4A\useMkvStamps = true -K4W2\format = 1 -MyntEye\auto_exposure = true -MyntEye\brightness = 120 -MyntEye\contrast = 116 -MyntEye\depth = false -MyntEye\gain = 24 -MyntEye\ir_control = 0 -MyntEye\rectify = false -OdomSensor\odom_sensor = 0 -OdomSensor\odom_sensor_calibration_path = -OdomSensor\odom_sensor_device = -OdomSensor\odom_sensor_extrinsics = -0.000622602 0.0303752 0.031389 -0.00272485 0.00749254 0.0 -OdomSensor\odom_sensor_odom_as_gt = false -OdomSensor\odom_sensor_offset_time = 0 -OdomSensor\odom_sensor_scale_factor = 1 -Openni\oniPath = -Openni2\autoExposure = true -Openni2\autoWhiteBalance = true -Openni2\depthDecimation = 1 -Openni2\exposure = 0 -Openni2\gain = 100 -Openni2\hshift = 0 -Openni2\mirroring = false -Openni2\oniPath = -Openni2\stampsIdsUsed = false -Openni2\vshift = 0 -RGBDImages\max_frames = 0 -RGBDImages\path_depth = -RGBDImages\path_rgb = -RGBDImages\scale = 1 -RGBDImages\start_index = 0 -RealSense\depthScaled = false -RealSense\odom = false -RealSense\presetDepth = 2 -RealSense\presetRGB = 0 -RealSense\rgbSource = 0 -RealSense2\emitter = true -RealSense2\global_time_sync = true -RealSense2\height = 480 -RealSense2\height_depth = 480 -RealSense2\ir = false -RealSense2\irdepth = true -RealSense2\json_preset = -RealSense2\rate = 60 -RealSense2\rate_depth = 30 -RealSense2\width = 848 -RealSense2\width_depth = 640 -Scan\downsampleStep = 1 -Scan\fromDepth = false -Scan\normalsK = 0 -Scan\normalsRadius = 0 -Scan\normalsUpF = 0 -Scan\rangeMax = 0 -Scan\rangeMin = 0 -Scan\voxelSize = 0 -StereoImages\max_frames = 0 -StereoImages\path_left = -StereoImages\path_right = -StereoImages\rectify = false -StereoImages\start_index = 0 -StereoVideo\device2 = -1 -StereoVideo\height = 0 -StereoVideo\path = -StereoVideo\path2 = -StereoVideo\width = 0 -StereoZed\confidence_thr = 100 -StereoZed\quality = 1 -StereoZed\resolution = 2 -StereoZed\self_calibration = true -StereoZed\sensing_mode = 0 -StereoZed\svo_path = -StereoZed\textureness_confidence_thr = 90 -UsbCam\height = 0 -UsbCam\width = 0 -Video\path = -calibrationName = -device = -imageDecimation = 1 -imgRate = 0 -localTransform = 0 0 0 0 0 0 -mirroring = false -rgb\driver = 0 -rgb\rectify = false -rgbd\bilateral = false -rgbd\bilateral_sigma_r = 0.1 -rgbd\bilateral_sigma_s = 10 -rgbd\distortion_model = -rgbd\driver = 4 -rgbd\rgbdColorOnly = false -stereo\depthGenerated = false -stereo\driver = 2 -stereo\exposureCompensation = false -type = 0 - - -[Core] -BRIEF\Bytes = 32 -BRISK\Octaves = 3 -BRISK\PatternScale = 1 -BRISK\Thresh = 30 -Bayes\FullPredictionUpdate = false -Bayes\PredictionLC = 0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23 -Bayes\VirtualPlacePriorThr = 0.9 -Db\TargetVersion = -DbSqlite3\CacheSize = 10000 -DbSqlite3\InMemory = false -DbSqlite3\JournalMode = 3 -DbSqlite3\Synchronous = 0 -DbSqlite3\TempStore = 2 -FAST\CV = 0 -FAST\Gpu = false -FAST\GpuKeypointsRatio = 0.05 -FAST\GridCols = 0 -FAST\GridRows = 0 -FAST\MaxThreshold = 200 -FAST\MinThreshold = 7 -FAST\NonmaxSuppression = true -FAST\Threshold = 20 -FREAK\NOctaves = 4 -FREAK\OrientationNormalized = true -FREAK\PatternScale = 22 -FREAK\ScaleNormalized = true -GFTT\BlockSize = 3 -GFTT\K = 0.04 -GFTT\MinDistance = 7 -GFTT\QualityLevel = 0.001 -GFTT\UseHarrisDetector = false -GMS\ThresholdFactor = 6 -GMS\WithRotation = false -GMS\WithScale = false -GTSAM\Optimizer = 1 -Grid\3D = false -Grid\CellSize = 0.05 -Grid\ClusterRadius = 0.1 -Grid\DepthDecimation = 4 -Grid\DepthRoiRatios = 0.0 0.0 0.0 0.0 -Grid\FlatObstacleDetected = true -Grid\FootprintHeight = 0.0 -Grid\FootprintLength = 0.0 -Grid\FootprintWidth = 0.0 -Grid\GroundIsObstacle = false -Grid\MapFrameProjection = false -Grid\MaxGroundAngle = 45 -Grid\MaxGroundHeight = 0.0 -Grid\MaxObstacleHeight = 0.0 -Grid\MinClusterSize = 10 -Grid\MinGroundHeight = 0 -Grid\NoiseFilteringMinNeighbors = 5 -Grid\NoiseFilteringRadius = 0.0 -Grid\NormalK = 20 -Grid\NormalsSegmentation = true -Grid\PreVoxelFiltering = true -Grid\RangeMax = 5 -Grid\RangeMin = 0.0 -Grid\RayTracing = false -Grid\Scan2dUnknownSpaceFilled = false -Grid\ScanDecimation = 1 -Grid\Sensor = 1 -GridGlobal\AltitudeDelta = 0 -GridGlobal\Eroded = false -GridGlobal\FloodFillDepth = 0 -GridGlobal\FootprintRadius = 0.0 -GridGlobal\FullUpdate = true -GridGlobal\MaxNodes = 0 -GridGlobal\MinSize = 0.0 -GridGlobal\OccupancyThr = 0.5 -GridGlobal\ProbClampingMax = 0.971 -GridGlobal\ProbClampingMin = 0.1192 -GridGlobal\ProbHit = 0.7 -GridGlobal\ProbMiss = 0.4 -GridGlobal\UpdateError = 0.01 -Icp\CCFilterOutFarthestPoints = false -Icp\CCMaxFinalRMS = 0.2 -Icp\CCSamplingLimit = 50000 -Icp\CorrespondenceRatio = 0.1 -Icp\DebugExportFormat = -Icp\DownsamplingStep = 1 -Icp\Epsilon = 0 -Icp\Force4DoF = false -Icp\Iterations = 30 -Icp\MaxCorrespondenceDistance = 0.05 -Icp\MaxRotation = 0.78 -Icp\MaxTranslation = 0.2 -Icp\OutlierRatio = 0.85 -Icp\PMConfig = -Icp\PMMatcherEpsilon = 0.0 -Icp\PMMatcherIntensity = false -Icp\PMMatcherKnn = 1 -Icp\PointToPlane = true -Icp\PointToPlaneGroundNormalsUp = 0.0 -Icp\PointToPlaneK = 5 -Icp\PointToPlaneLowComplexityStrategy = 1 -Icp\PointToPlaneMinComplexity = 0.02 -Icp\PointToPlaneRadius = 0.0 -Icp\RangeMax = 0 -Icp\RangeMin = 0 -Icp\Strategy = 0 -Icp\VoxelSize = 0 -ImuFilter\ComplementaryBiasAlpha = 0.01 -ImuFilter\ComplementaryDoAdpativeGain = true -ImuFilter\ComplementaryDoBiasEstimation = true -ImuFilter\ComplementaryGainAcc = 0.01 -ImuFilter\MadgwickGain = 0.1 -ImuFilter\MadgwickZeta = 0.0 -KAZE\Diffusivity = 1 -KAZE\Extended = false -KAZE\NOctaveLayers = 4 -KAZE\NOctaves = 4 -KAZE\Threshold = 0.001 -KAZE\Upright = false -Kp\BadSignRatio = 0.5 -Kp\ByteToFloat = false -Kp\DetectorStrategy = 1 -Kp\DictionaryPath = -Kp\FlannRebalancingFactor = 2.0 -Kp\GridCols = 1 -Kp\GridRows = 1 -Kp\IncrementalDictionary = true -Kp\IncrementalFlann = true -Kp\MaxDepth = 0 -Kp\MaxFeatures = 500 -Kp\MinDepth = 0 -Kp\NNStrategy = 1 -Kp\NewWordsComparedTogether = true -Kp\NndrRatio = 0.8 -Kp\Parallelized = true -Kp\RoiRatios = 0.0 0.0 0.0 0.0 -Kp\SubPixEps = 0.02 -Kp\SubPixIterations = 0 -Kp\SubPixWinSize = 3 -Kp\TfIdfLikelihoodUsed = true -Marker\CornerRefinementMethod = 0 -Marker\Dictionary = 0 -Marker\Length = 0 -Marker\MaxDepthError = 0.01 -Marker\MaxRange = 0.0 -Marker\MinRange = 0.0 -Marker\Priors = -Marker\PriorsVarianceAngular = 0.001 -Marker\PriorsVarianceLinear = 0.001 -Marker\VarianceAngular = 0.01 -Marker\VarianceLinear = 0.001 -Mem\BadSignaturesIgnored = false -Mem\BinDataKept = true -Mem\CompressionParallelized = true -Mem\CovOffDiagIgnored = true -Mem\DepthAsMask = true -Mem\GenerateIds = true -Mem\ImageCompressionFormat = .jpg -Mem\ImageKept = false -Mem\ImagePostDecimation = 1 -Mem\ImagePreDecimation = 1 -Mem\IncrementalMemory = true -Mem\InitWMWithAllNodes = false -Mem\IntermediateNodeDataKept = false -Mem\LaserScanDownsampleStepSize = 1 -Mem\LaserScanNormalK = 0 -Mem\LaserScanNormalRadius = 0.0 -Mem\LaserScanVoxelSize = 0.0 -Mem\LocalizationDataSaved = false -Mem\MapLabelsAdded = true -Mem\NotLinkedNodesKept = true -Mem\RawDescriptorsKept = true -Mem\RecentWmRatio = 0.2 -Mem\ReduceGraph = false -Mem\RehearsalIdUpdatedToNewOne = false -Mem\RehearsalSimilarity = 0.6 -Mem\RehearsalWeightIgnoredWhileMoving = false -Mem\STMSize = 10 -Mem\SaveDepth16Format = false -Mem\StereoFromMotion = false -Mem\TransferSortingByWeightId = false -Mem\UseOdomFeatures = true -Mem\UseOdomGravity = true -ORB\EdgeThreshold = 19 -ORB\FirstLevel = 0 -ORB\Gpu = false -ORB\NLevels = 3 -ORB\PatchSize = 31 -ORB\ScaleFactor = 2 -ORB\ScoreType = 0 -ORB\WTA_K = 2 -Odom\AlignWithGround = false -Odom\FillInfoData = true -Odom\FilteringStrategy = 0 -Odom\GuessMotion = true -Odom\GuessSmoothingDelay = 0 -Odom\Holonomic = true -Odom\ImageBufferSize = 1 -Odom\ImageDecimation = 1 -Odom\KalmanMeasurementNoise = 0.01 -Odom\KalmanProcessNoise = 0.001 -Odom\KeyFrameThr = 0.3 -Odom\ParticleLambdaR = 100 -Odom\ParticleLambdaT = 100 -Odom\ParticleNoiseR = 0.002 -Odom\ParticleNoiseT = 0.002 -Odom\ParticleSize = 400 -Odom\ResetCountdown = 0 -Odom\ScanKeyFrameThr = 0.9 -Odom\Strategy = 0 -Odom\VisKeyFrameThr = 150 -OdomF2M\BundleAdjustment = 1 -OdomF2M\BundleAdjustmentMaxFrames = 10 -OdomF2M\MaxNewFeatures = 0 -OdomF2M\MaxSize = 2000 -OdomF2M\ScanMaxSize = 2000 -OdomF2M\ScanRange = 0 -OdomF2M\ScanSubtractAngle = 45 -OdomF2M\ScanSubtractRadius = 0.05 -OdomF2M\ValidDepthRatio = 0.75 -OdomFovis\BucketHeight = 80 -OdomFovis\BucketWidth = 80 -OdomFovis\CliqueInlierThreshold = 0.1 -OdomFovis\FastThreshold = 20 -OdomFovis\FastThresholdAdaptiveGain = 0.005 -OdomFovis\FeatureSearchWindow = 25 -OdomFovis\FeatureWindowSize = 9 -OdomFovis\InlierMaxReprojectionError = 1.5 -OdomFovis\MaxKeypointsPerBucket = 25 -OdomFovis\MaxMeanReprojectionError = 10.0 -OdomFovis\MaxPyramidLevel = 3 -OdomFovis\MinFeaturesForEstimate = 20 -OdomFovis\MinPyramidLevel = 0 -OdomFovis\StereoMaxDisparity = 128 -OdomFovis\StereoMaxDistEpipolarLine = 1.5 -OdomFovis\StereoMaxRefinementDisplacement = 1.0 -OdomFovis\StereoRequireMutualMatch = true -OdomFovis\TargetPixelsPerFeature = 250 -OdomFovis\UpdateTargetFeaturesWithRefined = false -OdomFovis\UseAdaptiveThreshold = true -OdomFovis\UseBucketing = true -OdomFovis\UseHomographyInitialization = true -OdomFovis\UseImageNormalization = false -OdomFovis\UseSubpixelRefinement = true -OdomLOAM\AngVar = 0.01 -OdomLOAM\LinVar = 0.01 -OdomLOAM\LocalMapping = true -OdomLOAM\Resolution = 0.2 -OdomLOAM\ScanPeriod = 0.1 -OdomLOAM\Sensor = 2 -OdomMSCKF\FastThreshold = 10 -OdomMSCKF\GridCol = 5 -OdomMSCKF\GridMaxFeatureNum = 4 -OdomMSCKF\GridMinFeatureNum = 3 -OdomMSCKF\GridRow = 4 -OdomMSCKF\InitCovAccBias = 0.01 -OdomMSCKF\InitCovExRot = 0.00030462 -OdomMSCKF\InitCovExTrans = 2.5e-05 -OdomMSCKF\InitCovGyroBias = 0.01 -OdomMSCKF\InitCovVel = 0.25 -OdomMSCKF\MaxCamStateSize = 20 -OdomMSCKF\MaxIteration = 30 -OdomMSCKF\NoiseAcc = 0.05 -OdomMSCKF\NoiseAccBias = 0.01 -OdomMSCKF\NoiseFeature = 0.035 -OdomMSCKF\NoiseGyro = 0.005 -OdomMSCKF\NoiseGyroBias = 0.001 -OdomMSCKF\OptTranslationThreshold = 0 -OdomMSCKF\PatchSize = 15 -OdomMSCKF\PositionStdThreshold = 8 -OdomMSCKF\PyramidLevels = 3 -OdomMSCKF\RansacThreshold = 3 -OdomMSCKF\RotationThreshold = 0.2618 -OdomMSCKF\StereoThreshold = 5 -OdomMSCKF\TrackPrecision = 0.01 -OdomMSCKF\TrackingRateThreshold = 0.5 -OdomMSCKF\TranslationThreshold = 0.4 -OdomMono\InitMinFlow = 100 -OdomMono\InitMinTranslation = 0.1 -OdomMono\MaxVariance = 0.01 -OdomMono\MinTranslation = 0.02 -OdomOKVIS\ConfigPath = -OdomORBSLAM\Bf = 0.076 -OdomORBSLAM\Fps = 0.0 -OdomORBSLAM\MapSize = 3000 -OdomORBSLAM\MaxFeatures = 1000 -OdomORBSLAM\ThDepth = 40.0 -OdomORBSLAM\VocPath = -OdomOpen3D\MaxDepth = 3.0 -OdomOpen3D\Method = 0 -OdomVINS\ConfigPath = -OdomViso2\BucketHeight = 50 -OdomViso2\BucketMaxFeatures = 2 -OdomViso2\BucketWidth = 50 -OdomViso2\InlierThreshold = 2.0 -OdomViso2\MatchBinsize = 50 -OdomViso2\MatchDispTolerance = 2 -OdomViso2\MatchHalfResolution = true -OdomViso2\MatchMultiStage = true -OdomViso2\MatchNmsN = 3 -OdomViso2\MatchNmsTau = 50 -OdomViso2\MatchOutlierDispTolerance = 5 -OdomViso2\MatchOutlierFlowTolerance = 5 -OdomViso2\MatchRadius = 200 -OdomViso2\MatchRefinement = 1 -OdomViso2\RansacIters = 200 -OdomViso2\Reweighting = true -Optimizer\Epsilon = 0 -Optimizer\GravitySigma = 0.3 -Optimizer\Iterations = 20 -Optimizer\LandmarksIgnored = false -Optimizer\PriorsIgnored = true -Optimizer\Robust = false -Optimizer\Strategy = 1 -Optimizer\VarianceIgnored = false -PyDetector\Cuda = true -PyDetector\Path = -PyMatcher\Cuda = true -PyMatcher\Iterations = 20 -PyMatcher\Model = indoor -PyMatcher\Path = -PyMatcher\Threshold = 0.2 -RGBD\AngularSpeedUpdate = 0.0 -RGBD\AngularUpdate = 0.1 -RGBD\CreateOccupancyGrid = false -RGBD\Enabled = true -RGBD\GoalReachedRadius = 0.5 -RGBD\GoalsSavedInUserData = false -RGBD\LinearSpeedUpdate = 0.0 -RGBD\LinearUpdate = 0.1 -RGBD\LocalBundleOnLoopClosure = false -RGBD\LocalImmunizationRatio = 0.25 -RGBD\LocalRadius = 10 -RGBD\LoopClosureIdentityGuess = false -RGBD\LoopClosureReextractFeatures = false -RGBD\LoopCovLimited = false -RGBD\MarkerDetection = false -RGBD\MaxLocalRetrieved = 2 -RGBD\MaxLoopClosureDistance = 0 -RGBD\MaxOdomCacheSize = 10 -RGBD\NeighborLinkRefining = true -RGBD\NewMapOdomChangeDistance = 0 -RGBD\OptimizeFromGraphEnd = false -RGBD\OptimizeMaxError = 3 -RGBD\PlanAngularVelocity = 0 -RGBD\PlanLinearVelocity = 0 -RGBD\PlanStuckIterations = 0 -RGBD\ProximityAngle = 45 -RGBD\ProximityBySpace = true -RGBD\ProximityByTime = false -RGBD\ProximityGlobalScanMap = false -RGBD\ProximityMaxGraphDepth = 50 -RGBD\ProximityMaxPaths = 3 -RGBD\ProximityMergedScanCovFactor = 100.0 -RGBD\ProximityOdomGuess = false -RGBD\ProximityPathFilteringRadius = 1 -RGBD\ProximityPathMaxNeighbors = 0 -RGBD\ProximityPathRawPosesUsed = true -RGBD\ScanMatchingIdsSavedInLinks = true -RGBD\StartAtOrigin = false -Reg\Force3DoF = false -Reg\RepeatOnce = true -Reg\Strategy = 2 -Rtabmap\ComputeRMSE = true -Rtabmap\CreateIntermediateNodes = false -Rtabmap\DetectionRate = 1 -Rtabmap\ImageBufferSize = 1 -Rtabmap\ImagesAlreadyRectified = true -Rtabmap\LoopGPS = true -Rtabmap\LoopRatio = 0 -Rtabmap\LoopThr = 0.11 -Rtabmap\MaxRetrieved = 2 -Rtabmap\MemoryThr = 0 -Rtabmap\PublishLastSignature = true -Rtabmap\PublishLikelihood = true -Rtabmap\PublishPdf = true -Rtabmap\PublishRAMUsage = false -Rtabmap\PublishStats = true -Rtabmap\RectifyOnlyFeatures = false -Rtabmap\SaveWMState = false -Rtabmap\StartNewMapOnGoodSignature = false -Rtabmap\StartNewMapOnLoopClosure = false -Rtabmap\StatisticLogged = false -Rtabmap\StatisticLoggedHeaders = true -Rtabmap\StatisticLogsBufferedInRAM = true -Rtabmap\TimeThr = 900 -Rtabmap\WorkingDirectory = /home/kaatr/Documents/RTAB-Map -SIFT\ContrastThreshold = 0.04 -SIFT\EdgeThreshold = 10 -SIFT\NFeatures = 0 -SIFT\NOctaveLayers = 3 -SIFT\RootSIFT = false -SIFT\Sigma = 1.6 -SURF\Extended = false -SURF\GpuKeypointsRatio = 0.01 -SURF\GpuVersion = false -SURF\HessianThreshold = 500 -SURF\OctaveLayers = 2 -SURF\Octaves = 4 -SURF\Upright = false -Stereo\DenseStrategy = 0 -Stereo\Eps = 0.01 -Stereo\Iterations = 30 -Stereo\MaxDisparity = 128 -Stereo\MaxLevel = 5 -Stereo\MinDisparity = 0.5 -Stereo\OpticalFlow = true -Stereo\SSD = true -Stereo\WinHeight = 3 -Stereo\WinWidth = 15 -StereoBM\BlockSize = 15 -StereoBM\Disp12MaxDiff = -1 -StereoBM\MinDisparity = 0 -StereoBM\NumDisparities = 128 -StereoBM\PreFilterCap = 31 -StereoBM\PreFilterSize = 9 -StereoBM\SpeckleRange = 4 -StereoBM\SpeckleWindowSize = 100 -StereoBM\TextureThreshold = 10 -StereoBM\UniquenessRatio = 15 -StereoSGBM\BlockSize = 15 -StereoSGBM\Disp12MaxDiff = 1 -StereoSGBM\MinDisparity = 0 -StereoSGBM\Mode = 2 -StereoSGBM\NumDisparities = 128 -StereoSGBM\P1 = 2 -StereoSGBM\P2 = 5 -StereoSGBM\PreFilterCap = 31 -StereoSGBM\SpeckleRange = 4 -StereoSGBM\SpeckleWindowSize = 100 -StereoSGBM\UniquenessRatio = 20 -SuperPoint\Cuda = true -SuperPoint\ModelPath = -SuperPoint\NMS = true -SuperPoint\NMSRadius = 4 -SuperPoint\Threshold = 0.01 -Version = 0.20.19 -VhEp\Enabled = false -VhEp\MatchCountMin = 8 -VhEp\RansacParam1 = 3 -VhEp\RansacParam2 = 0.99 -Vis\BundleAdjustment = 1 -Vis\CorFlowEps = 0.01 -Vis\CorFlowIterations = 30 -Vis\CorFlowMaxLevel = 3 -Vis\CorFlowWinSize = 16 -Vis\CorGuessMatchToProjection = false -Vis\CorGuessWinSize = 40 -Vis\CorNNDR = 0.8 -Vis\CorNNType = 1 -Vis\CorType = 0 -Vis\DepthAsMask = true -Vis\EpipolarGeometryVar = 0.1 -Vis\EstimationType = 1 -Vis\FeatureType = 1 -Vis\ForwardEstOnly = true -Vis\GridCols = 1 -Vis\GridRows = 1 -Vis\InlierDistance = 0.1 -Vis\Iterations = 300 -Vis\MaxDepth = 0 -Vis\MaxFeatures = 1000 -Vis\MeanInliersDistance = 0.0 -Vis\MinDepth = 0 -Vis\MinInliers = 20 -Vis\MinInliersDistribution = 0.0 -Vis\PnPFlags = 0 -Vis\PnPRefineIterations = 0 -Vis\PnPReprojError = 2 -Vis\RefineIterations = 5 -Vis\RoiRatios = 0.0 0.0 0.0 0.0 -Vis\SubPixEps = 0.02 -Vis\SubPixIterations = 0 -Vis\SubPixWinSize = 3 -g2o\Baseline = 0.075 -g2o\Optimizer = 0 -g2o\PixelVariance = 1 -g2o\RobustKernelDelta = 8 -g2o\Solver = 0 - - -[Gui] -General\beep = false -General\cloudCeilingHeight = 0 -General\cloudFiltering = false -General\cloudFilteringAngle = 30 -General\cloudFilteringRadius = 0.1 -General\cloudFloorHeight = 0 -General\cloudNoiseMinNeighbors = 5 -General\cloudNoiseRadius = 0 -General\cloudVoxel = 0 -General\cloudsKept = true -General\colorScheme0 = 0 -General\colorScheme1 = 0 -General\colorSchemeScan0 = 0 -General\colorSchemeScan1 = 0 -General\decimation0 = 4 -General\decimation1 = 4 -General\downsamplingScan0 = 1 -General\downsamplingScan1 = 1 -General\figure_cache = true -General\figure_time = true -General\gravityLength0 = 1 -General\gravityLength1 = 1 -General\gravityShown0 = false -General\gravityShown1 = false -General\gridMapOpacity = 0.75 -General\gridMapShown = false -General\gtAlign = true -General\imageHighestHypShown = false -General\imageRejectedShown = true -General\imagesKept = true -General\landmarkSize = 0 -General\localizationsGraphView = false -General\loggerEventLevel = 3 -General\loggerLevel = 2 -General\loggerPauseLevel = 3 -General\loggerPrintThreadId = false -General\loggerPrintTime = true -General\loggerType = 1 -General\maxDepth0 = 0 -General\maxDepth1 = 0 -General\maxRange0 = 0 -General\maxRange1 = 0 -General\meshing = false -General\meshing_angle = 15 -General\meshing_quad = true -General\meshing_texture = false -General\meshing_triangle_size = 2 -General\minDepth0 = 0 -General\minDepth1 = 0 -General\minRange0 = 0 -General\minRange1 = 0 -General\noFiltering = true -General\nochangeGraphView = false -General\normalKSearch = 10 -General\normalRadiusSearch = 0 -General\notifyNewGlobalPath = false -General\octomap = false -General\octomap_2dgrid = true -General\octomap_3dmap = true -General\octomap_depth = 16 -General\octomap_point_size = 5 -General\octomap_rendering_type = 0 -General\odomDisabled = false -General\odomF2MGravitySigma = -1 -General\odomOnlyInliersShown = false -General\odomQualityThr = 50 -General\odomRegistration = 3 -General\opacity0 = 1 -General\opacity1 = 0.75 -General\opacityScan0 = 1 -General\opacityScan1 = 0.5 -General\posteriorGraphView = true -General\ptSize0 = 1 -General\ptSize1 = 2 -General\ptSizeFeatures0 = 3 -General\ptSizeFeatures1 = 3 -General\ptSizeScan0 = 1 -General\ptSizeScan1 = 2 -General\roiRatios0 = 0.0 0.0 0.0 0.0 -General\roiRatios1 = 0.0 0.0 0.0 0.0 -General\scanCeilingHeight = 0 -General\scanFloorHeight = 0 -General\scanNormalKSearch = 0 -General\scanNormalRadiusSearch = 0 -General\showClouds0 = true -General\showClouds1 = true -General\showFeatures0 = false -General\showFeatures1 = true -General\showFrames = false -General\showFrustums0 = false -General\showFrustums1 = false -General\showGraphs = true -General\showIMUAcc = false -General\showIMUGravity = false -General\showLabels = false -General\showLandmarks = true -General\showScans0 = false -General\showScans1 = false -General\subtractFiltering = false -General\subtractFilteringAngle = 0 -General\subtractFilteringMinPts = 5 -General\subtractFilteringRadius = 0.02 -General\verticalLayoutUsed = true -General\voxelSizeScan0 = 0 -General\voxelSizeScan1 = 0 -General\wordsGraphView = false diff --git a/cpp/rtabmap/src/camera_k4a.cpp b/cpp/rtabmap/src/camera_k4a.cpp deleted file mode 100644 index 72b2653..0000000 --- a/cpp/rtabmap/src/camera_k4a.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "../include/spectacularAI/rtabmap/camera_k4a.h" -#include "../include/spectacularAI/rtabmap/util.h" - -#include - -namespace rtabmap { - -bool CameraK4A::available() { -#ifdef SPECTACULARAI_CAMERA_K4A - return true; -#else - return false; -#endif -} - -CameraK4A::CameraK4A( - const std::string &recordingFolder, - float imageRate, - const rtabmap::Transform &localTransform) : - CameraSpectacularAI(imageRate, localTransform) -#ifdef SPECTACULARAI_CAMERA_K4A - , - recordingFolder(recordingFolder) -#endif - {} - -CameraK4A::~CameraK4A() { -#ifdef SPECTACULARAI_CAMERA_K4A - shouldQuit = true; - session = nullptr; -#endif -} - -bool CameraK4A::init(const std::string &calibrationFolder, const std::string &cameraName) { - (void)calibrationFolder, (void)cameraName; - -#ifdef SPECTACULARAI_CAMERA_K4A - // Mapping API callback function to receive new, updated and deleted keyframes. - auto mapperFn = [&](std::shared_ptr output) { - this->mappingApiCallback(output); - }; - - // Create config for VIO and k4a device. - spectacularAI::k4aPlugin::Configuration config; - std::map internalParameters; - internalParameters.insert(std::make_pair("applyLoopClosures", "False")); // Let RTAB-Map handle loop closures - internalParameters.insert(std::make_pair("skipFirstNCandidates", "10")); // Skip couple first keyframes to ensure gravity estimate is accurate. - config.internalParameters = internalParameters; - config.k4aConfig = spectacularAI::k4aPlugin::getK4AConfiguration(colorResolution, depthMode, frameRate); - config.recordingFolder = recordingFolder; - - // Create vio pipeline using the config, and then start k4a device and VIO. - vioPipeline = std::make_unique(config); - vioPipeline->setMapperCallback(mapperFn); - session = vioPipeline->startSession(); - - return true; -#else - return false; -#endif -} - -bool CameraK4A::isCalibrated() const { -#ifdef SPECTACULARAI_CAMERA_K4A - return model.isValidForProjection(); -#else - return false; -#endif -} - -std::string CameraK4A::getSerial() const { - return ""; -} - -SensorData CameraK4A::captureImage(CameraInfo *info) { - SensorData data; - -#ifdef SPECTACULARAI_CAMERA_K4A - // Wait until new keyframe is available from mapping API. - while (!shouldQuit && !keyFrameData.isValid()) { - postPoseEvent(); - uSleep(1); - } - - std::lock_guard guard(dataMutex); - if (keyFrameData.isValid()) { - // Send keyframe data to RTAB-Map. - data = keyFrameData; - info->odomPose = keyFramePose; - - // Clear old data - keyFrameData = SensorData(); - } -#endif - - return data; -} - -void CameraK4A::postPoseEvent() { -#ifdef SPECTACULARAI_CAMERA_K4A - // Get the most recent vio pose estimate from the queue. - std::shared_ptr vioOutput; - while (session->hasOutput()) { - vioOutput = session->getOutput(); - } - - if (vioOutput) { - auto &position = vioOutput->pose.position; - auto &rotation = vioOutput->pose.orientation; - Transform pose = Transform(position.x, position.y, position.z, - rotation.x, rotation.y, rotation.z, rotation.w); - - this->post(new PoseEvent(pose)); - } -#endif -} - -} // namespace rtabmap diff --git a/cpp/rtabmap/src/camera_oak.cpp b/cpp/rtabmap/src/camera_oak.cpp deleted file mode 100644 index 45de7bc..0000000 --- a/cpp/rtabmap/src/camera_oak.cpp +++ /dev/null @@ -1,126 +0,0 @@ -#include "../include/spectacularAI/rtabmap/camera_oak.h" -#include "../include/spectacularAI/rtabmap/util.h" - -#include - -namespace rtabmap { - -bool CameraOAK::available() { -#ifdef SPECTACULARAI_CAMERA_OAK - return true; -#else - return false; -#endif -} - -CameraOAK::CameraOAK( - const std::string &recordingFolder, - float imageRate, - const rtabmap::Transform &localTransform) : - CameraSpectacularAI(imageRate, localTransform) -#ifdef SPECTACULARAI_CAMERA_OAK - , - recordingFolder(recordingFolder) -#endif - {} - -CameraOAK::~CameraOAK() { -#ifdef SPECTACULARAI_CAMERA_OAK - shouldQuit = true; - session = nullptr; -#endif -} - -bool CameraOAK::init(const std::string &calibrationFolder, const std::string &cameraName) { - (void)calibrationFolder, (void)cameraName; - -#ifdef SPECTACULARAI_CAMERA_OAK - // Mapping API callback function to receive new, updated and deleted keyframes. - auto mapperFn = [&](std::shared_ptr output) { - this->mappingApiCallback(output); - }; - - // Create Depth AI (OAK-D) pipeline - dai::Pipeline pipeline; - - // Optional configuration - spectacularAI::daiPlugin::Configuration config; - std::map internalParameters; - internalParameters.insert(std::make_pair("applyLoopClosures", "False")); // Let RTAB-Map handle loop closures - internalParameters.insert(std::make_pair("skipFirstNCandidates", "10")); // Skip couple first keyframes to ensure gravity estimate is accurate. - internalParameters.insert(std::make_pair("computeStereoPointCloud", "true")); - internalParameters.insert(std::make_pair("pointCloudNormalsEnabled", "true")); - config.internalParameters = internalParameters; - config.recordingFolder = recordingFolder; - // Example: enable these to support fisheye lenses (SDK 0.16+) - // config.meshRectification = true; - // config.depthScaleCorrection = true; - - vioPipeline = std::make_unique(pipeline, config, mapperFn); - - // Connect to device and start pipeline - device = std::make_shared(pipeline); - session = vioPipeline->startSession(*device); - - return true; -#else - return false; -#endif -} - -bool CameraOAK::isCalibrated() const { -#ifdef SPECTACULARAI_CAMERA_OAK - return model.isValidForProjection(); -#else - return false; -#endif -} - -std::string CameraOAK::getSerial() const { - return ""; -} - -SensorData CameraOAK::captureImage(CameraInfo *info) { - SensorData data; - -#ifdef SPECTACULARAI_CAMERA_OAK - // Wait until new keyframe is available from mapping API. - while (!shouldQuit && !keyFrameData.isValid()) { - postPoseEvent(); - uSleep(1); - } - - std::lock_guard guard(dataMutex); - if (keyFrameData.isValid()) { - // Send keyframe data to RTAB-Map. - data = keyFrameData; - info->odomPose = keyFramePose; - - // Clear old data - keyFrameData = SensorData(); - } -#endif - - return data; -} - -void CameraOAK::postPoseEvent() { -#ifdef SPECTACULARAI_CAMERA_OAK - // Get the most recent vio pose estimate from the queue. - std::shared_ptr vioOutput; - while (session->hasOutput()) { - vioOutput = session->getOutput(); - } - - if (vioOutput) { - auto &position = vioOutput->pose.position; - auto &rotation = vioOutput->pose.orientation; - Transform pose = Transform(position.x, position.y, position.z, - rotation.x, rotation.y, rotation.z, rotation.w); - - this->post(new PoseEvent(pose)); - } -#endif -} - -} // namespace rtabmap diff --git a/cpp/rtabmap/src/camera_realsense.cpp b/cpp/rtabmap/src/camera_realsense.cpp deleted file mode 100644 index 6b0670f..0000000 --- a/cpp/rtabmap/src/camera_realsense.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "../include/spectacularAI/rtabmap/camera_realsense.h" -#include "../include/spectacularAI/rtabmap/util.h" - -#include - -#ifdef SPECTACULARAI_CAMERA_REALSENSE -#include -#endif - -namespace rtabmap { - -bool CameraRealsense::available() { -#ifdef SPECTACULARAI_CAMERA_REALSENSE - return true; -#else - return false; -#endif -} - -CameraRealsense::CameraRealsense( - const std::string &recordingFolder, - float imageRate, - const rtabmap::Transform &localTransform) : - CameraSpectacularAI(imageRate, localTransform) -#ifdef SPECTACULARAI_CAMERA_REALSENSE - , - recordingFolder(recordingFolder) -#endif - {} - -CameraRealsense::~CameraRealsense() { -#ifdef SPECTACULARAI_CAMERA_REALSENSE - shouldQuit = true; - session = nullptr; -#endif -} - -bool CameraRealsense::init(const std::string &calibrationFolder, const std::string &cameraName) { - (void)calibrationFolder, (void)cameraName; -#ifdef SPECTACULARAI_CAMERA_REALSENSE - // Mapping API callback function to receive new, updated and deleted keyframes. - auto mapperFn = [&](std::shared_ptr output) { - this->mappingApiCallback(output); - }; - - spectacularAI::rsPlugin::Configuration config; - config.recordingFolder = recordingFolder; - - std::map internalParameters; - internalParameters.insert(std::make_pair("applyLoopClosures", "False")); // Let RTAB-Map handle loop closures - internalParameters.insert(std::make_pair("skipFirstNCandidates", "10")); // Skip couple first keyframes to ensure gravity estimate is accurate. - config.internalParameters = internalParameters; - - vioPipeline = std::make_unique(config); - { - // Find RealSense device - rs2::context rsContext; - rs2::device_list devices = rsContext.query_devices(); - if (devices.size() != 1) { - UERROR("Connect exactly one RealSense device!"); - return false; - } - rs2::device device = devices.front(); - vioPipeline->configureDevice(device); - } - - // Start pipeline - rs2::config rsConfig; - vioPipeline->configureStreams(rsConfig); - vioPipeline->setMapperCallback(mapperFn); - session = vioPipeline->startSession(rsConfig); - - return true; -#else - return false; -#endif -} - -bool CameraRealsense::isCalibrated() const { -#ifdef SPECTACULARAI_CAMERA_REALSENSE - return model.isValidForProjection(); -#else - return false; -#endif -} - -std::string CameraRealsense::getSerial() const { - return ""; -} - -SensorData CameraRealsense::captureImage(CameraInfo *info) { - SensorData data; - -#ifdef SPECTACULARAI_CAMERA_REALSENSE - // Wait until new keyframe is available from mapping API. - while (!shouldQuit && !keyFrameData.isValid()) { - postPoseEvent(); - uSleep(1); - } - - std::lock_guard guard(dataMutex); - if (keyFrameData.isValid()) { - // Send keyframe data to RTAB-Map. - data = keyFrameData; - info->odomPose = keyFramePose; - - // Clear old data - keyFrameData = SensorData(); - } -#endif - - return data; -} - -void CameraRealsense::postPoseEvent() { -#ifdef SPECTACULARAI_CAMERA_REALSENSE - // Get the most recent vio pose estimate from the queue. - std::shared_ptr vioOutput; - while (session->hasOutput()) { - vioOutput = session->getOutput(); - } - - if (vioOutput) { - auto &position = vioOutput->pose.position; - auto &rotation = vioOutput->pose.orientation; - Transform pose = Transform(position.x, position.y, position.z, - rotation.x, rotation.y, rotation.z, rotation.w); - - this->post(new PoseEvent(pose)); - } -#endif -} - -} // namespace rtabmap diff --git a/cpp/rtabmap/src/camera_replay.cpp b/cpp/rtabmap/src/camera_replay.cpp deleted file mode 100644 index 26ccdec..0000000 --- a/cpp/rtabmap/src/camera_replay.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include "../include/spectacularAI/rtabmap/camera_replay.h" -#include "../include/spectacularAI/rtabmap/util.h" - -#include - -#ifdef SPECTACULARAI_CORE -#include -#include -#endif - -namespace rtabmap { - -bool CameraReplay::available() { -#ifdef SPECTACULARAI_CORE - return true; -#else - return false; -#endif -} - -CameraReplay::CameraReplay( - const std::string& dataFolder, - float imageRate, - const rtabmap::Transform &localTransform) : - CameraSpectacularAI(imageRate, localTransform), - dataFolder(dataFolder) {} - -CameraReplay::~CameraReplay() { - shouldQuit = true; - - if (replayThread.joinable()) { - replayThread.join(); - } -} - -bool CameraReplay::init(const std::string &calibrationFolder, const std::string &cameraName) { - (void)calibrationFolder, (void)cameraName; - - replayThread = std::thread(&CameraReplay::startReplay, this); - - return true; -} - -bool CameraReplay::isCalibrated() const { - return true; -} - -std::string CameraReplay::getSerial() const { - return ""; -} - -SensorData CameraReplay::captureImage(CameraInfo *info) { - SensorData data; - - // Wait until new keyframe is available from mapping API. - while (!shouldQuit && !keyFrameData.isValid()) { - uSleep(1); - } - - std::lock_guard guard(dataMutex); - if (keyFrameData.isValid()) { - // Send keyframe data to RTAB-Map. - data = keyFrameData; - info->odomPose = keyFramePose; - - // Clear old data - keyFrameData = SensorData(); - } - - return data; -} - -void CameraReplay::startReplay() { - #ifdef SPECTACULARAI_CORE - - // Mapping API callback function to receive new, updated and deleted keyframes. - auto mapperFn = [&](std::shared_ptr output) { - this->mappingApiCallback(output); - }; - - // Replay API callback function to capture vio poses for each frame (for smooth visualisation). - auto vioFn = [&](spectacularAI::VioOutputPtr vioOutput) { - if (vioOutput) { - auto &position = vioOutput->pose.position; - auto &rotation = vioOutput->pose.orientation; - Transform pose = Transform(position.x, position.y, position.z, - rotation.x, rotation.y, rotation.z, rotation.w); - - this->post(new PoseEvent(pose)); - } - }; - - std::string configYaml = "applyLoopClosures: False\n" // Disable loop closures & let RTAB-Map handle them. - "skipFirstNCandidates: 10"; // Skip couple first keyframes to ensure gravity estimate is accurate. - - spectacularAI::Vio::Builder vioBuilder = spectacularAI::Vio::builder() - .setConfigurationYAML(configYaml) - .setMapperCallback(mapperFn); - auto replayApi = spectacularAI::Replay::builder(dataFolder, vioBuilder).build(); - replayApi->setOutputCallback(vioFn); - - // Main loop, reads data from replay one line at a time. - bool moreData = true; - while (moreData && !shouldQuit) { - if (!keyFrameData.isValid()) { - moreData = replayApi->replayOneLine(); - } else { - uSleep(1); - } - } - - #endif -} - -} // namespace rtabmap diff --git a/cpp/rtabmap/src/camera_spectacularai.cpp b/cpp/rtabmap/src/camera_spectacularai.cpp deleted file mode 100644 index 0d9a7fd..0000000 --- a/cpp/rtabmap/src/camera_spectacularai.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include "../include/spectacularAI/rtabmap/camera_spectacularai.h" -#include "../include/spectacularAI/rtabmap/util.h" - -#include - -namespace rtabmap { - -bool CameraSpectacularAI::available() { - return true; -} - -CameraSpectacularAI::CameraSpectacularAI( - float imageRate, - const rtabmap::Transform &localTransform) : - Camera(imageRate, localTransform) {} - -CameraSpectacularAI::~CameraSpectacularAI() {} - -void CameraSpectacularAI::mappingApiCallback(std::shared_ptr output) { - if (output->finalMap) shouldQuit = true; - if (output->map->keyFrames.empty()) return; - - // Get the newest keyframe from mapping API. - auto latestKeyFrameIt = output->map->keyFrames.rbegin(); - int64_t latestKeyFrameId = latestKeyFrameIt->first; - auto &frameSet = latestKeyFrameIt->second->frameSet; - - // Ensure the latest key frame is new. - if (latestKeyFrameId <= latestProcessedKeyFrameId) - return; - - UINFO("New keyframe with id=%ld", latestKeyFrameId); - - double timestamp = frameSet->rgbFrame->cameraPose.pose.time; - cv::Mat bgr; - cv::Mat depth; - - if (!frameSet->rgbFrame || !frameSet->depthFrame) { - UWARN("Keyframe %ld has color/depth image! Discarding...", latestKeyFrameId); - return; - } - - std::shared_ptr rgbFrame = frameSet->getUndistortedFrame(frameSet->rgbFrame); - std::shared_ptr depthFrame = frameSet->getAlignedDepthFrame(rgbFrame); - - if (!model.isValidForProjection()) { - model = util::convert(rgbFrame->cameraPose.camera, rgbFrame->image->getWidth(), rgbFrame->image->getHeight()); - assert(model.isValidForProjection()); - } - - bgr = util::convertColor(rgbFrame->image); - depth = util::convertDepth(depthFrame->image); - - SensorData data = SensorData(bgr, depth, model, latestKeyFrameId, timestamp); - - if (latestKeyFrameIt->second->pointCloud) { - LaserScan scan = util::laserScanFromPointCloud(latestKeyFrameIt->second->pointCloud); - data.setLaserScan(scan); - } - - std::lock_guard guard(dataMutex); - latestProcessedKeyFrameId = latestKeyFrameId; - keyFramePose = util::convert(frameSet->rgbFrame->cameraPose.pose); - keyFrameData = data; -} - -} // namespace rtabmap diff --git a/cpp/rtabmap/src/main.cpp b/cpp/rtabmap/src/main.cpp deleted file mode 100644 index dcab0f1..0000000 --- a/cpp/rtabmap/src/main.cpp +++ /dev/null @@ -1,141 +0,0 @@ -#include -#include - -#include -#include -#include -#include -#include - -#include "../include/spectacularAI/rtabmap/map_builder.h" -#include "../include/spectacularAI/rtabmap/camera_replay.h" -#include "../include/spectacularAI/rtabmap/camera_k4a.h" -#include "../include/spectacularAI/rtabmap/camera_realsense.h" -#include "../include/spectacularAI/rtabmap/camera_oak.h" - -void showUsage() { - std::cout << "Usage: rtabmap_mapper driver\n" - << " driver options: replay, k4a, realsense, oak" - << " Optional parameters:\n" - << " --output (-o) path/to/output/database.db [If set, RTAB-Map database is saved to this file]\n" - << " --config (-c) path/to/rtabmap_config.ini [If set, RTAB-Map settings are overriden by this file]\n" - << " --record (-r) path/to/recording [If set, captured data is saved to this location]\n" - << " --input (-i) path/to/dataset [Input dataset path for replay camera driver]\n" - << std::endl; - - exit(EXIT_SUCCESS); -} - -int main(int argc, char *argv[]) { - ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kWarning); - - const std::vector arguments(argv, argv + argc); - std::string driver; - std::string outputPath; - std::string rtabmapConfigFile; - std::string inputDataFolder; - std::string recordingFolder; - float imageRate = 0; // as fast as possible - - if (arguments.size() < 2) { - showUsage(); - return EXIT_SUCCESS; - } - - for (size_t i = 2; i < arguments.size(); ++i) { - const std::string &argument = arguments.at(i); - if (argument == "-i" || argument == "--input") - inputDataFolder = arguments.at(++i); - else if (argument == "-o" || argument == "--output") - outputPath = arguments.at(++i); - else if (argument == "-c" || argument == "--config") - rtabmapConfigFile = arguments.at(++i); - else if (argument == "-r" || argument == "--record") - recordingFolder = arguments.at(++i); - else if (argument == "-h" || argument == "--help") { - showUsage(); - } else { - UERROR("Unknown argument: %s", argument.c_str()); - showUsage(); - } - } - - Camera *camera = 0; - driver = arguments.at(1); - if (driver == "replay") { - if(!CameraReplay::available()) { - UERROR("Not built with CameraReplay support..."); - exit(EXIT_FAILURE); - } - camera = new CameraReplay(inputDataFolder, imageRate); - } else if (driver == "k4a") { - if(!CameraK4A::available()) { - UERROR("Not built with CameraK4A support..."); - exit(EXIT_FAILURE); - } - camera = new CameraK4A(recordingFolder, imageRate); - } else if (driver == "realsense") { - if (!CameraRealsense::available()) { - UERROR("Not built with CameraRealsense support..."); - exit(EXIT_FAILURE); - } - camera = new CameraRealsense(recordingFolder, imageRate); - } else if (driver == "oak") { - if (!CameraOAK::available()) { - UERROR("Not built with CameraOAK support..."); - exit(EXIT_FAILURE); - } - camera = new CameraOAK(recordingFolder, imageRate); - } else { - UERROR("Unknown camera driver: %s", driver.c_str()); - showUsage(); - } - - if(!camera->init()) { - UFATAL("Camera init failed!"); - } - - CameraThread* cameraThread = new CameraThread(camera); - - // GUI stuff, there the handler will receive RtabmapEvent and construct the map - // We give it the camera so the GUI can pause/resume the camera - QApplication app(argc, argv); - MapBuilder mapBuilder(cameraThread); - std::cout << "Press 'F' to toggle full screen, and press +/- to adjust point size!" << std::endl; - - // Create RTAB-Map to process OdometryEvent - Rtabmap *rtabmap = new Rtabmap(); - rtabmap->init(rtabmapConfigFile); - RtabmapThread rtabmapThread(rtabmap); // ownership is transfered - - // Setup handlers - rtabmapThread.registerToEventsManager(); - mapBuilder.registerToEventsManager(); - - // Let's start the threads - rtabmapThread.start(); - cameraThread->start(); - - mapBuilder.show(); - app.exec(); // main loop - - // remove handlers - mapBuilder.unregisterFromEventsManager(); - rtabmapThread.unregisterFromEventsManager(); - - // Kill all threads - cameraThread->join(true); - rtabmapThread.join(true); - delete cameraThread; - - // Close RTAB-Map and optionally save RTAB-Map database - bool save = !outputPath.empty(); - rtabmap->close(save, outputPath); - if (save) { - std::cout << "Saved RTAB-Map database to " + outputPath << std::endl; - } - std::cout << "Finished!" << std::endl; - - return EXIT_SUCCESS; -} diff --git a/cpp/rtabmap/src/util.cpp b/cpp/rtabmap/src/util.cpp deleted file mode 100644 index 30dad19..0000000 --- a/cpp/rtabmap/src/util.cpp +++ /dev/null @@ -1,187 +0,0 @@ -#include "../include/spectacularAI/rtabmap/util.h" - -#include -#include - -namespace rtabmap { -namespace util { -namespace { - -LaserScan laserScanFromPointCloudXYZ(const std::shared_ptr &cloud) { - cv::Mat laserScan = cv::Mat(1, (int)cloud->size(), CV_32FC3); - int oi = 0; - - for (unsigned int i = 0; i < cloud->size(); ++i) { - float *ptr = laserScan.ptr(0, oi++); - const spectacularAI::Vector3f &p = cloud->getPosition(i); - ptr[0] = p.x; - ptr[1] = p.y; - ptr[2] = p.z; - } - - return LaserScan(laserScan(cv::Range::all(), cv::Range(0, oi)), 0, 0.0f, LaserScan::kXYZ); -} - -LaserScan laserScanFromPointCloudXYZRGB(const std::shared_ptr &cloud) { - cv::Mat laserScan = cv::Mat(1, (int)cloud->size(), CV_32FC(4)); - int oi = 0; - - for (unsigned int i = 0; i < cloud->size(); ++i) { - float *ptr = laserScan.ptr(0, oi++); - const spectacularAI::Vector3f &p = cloud->getPosition(i); - const std::array &rgb = cloud->getRGB24(i); - ptr[0] = p.x; - ptr[1] = p.y; - ptr[2] = p.z; - int *ptrInt = (int *)ptr; - ptrInt[3] = int(rgb[2]) | (int(rgb[1]) << 8) | (int(rgb[0]) << 16); - } - - return LaserScan(laserScan(cv::Range::all(), cv::Range(0, oi)), 0, 0.0f, LaserScan::kXYZRGB); -} - -LaserScan laserScanFromPointCloudXYZNormal(const std::shared_ptr &cloud) { - cv::Mat laserScan = cv::Mat(1, (int)cloud->size(), CV_32FC(6)); - int oi = 0; - - for (unsigned int i = 0; i < cloud->size(); ++i) { - float *ptr = laserScan.ptr(0, oi++); - const spectacularAI::Vector3f &p = cloud->getPosition(i); - const spectacularAI::Vector3f &n = cloud->getNormal(i); - ptr[0] = p.x; - ptr[1] = p.y; - ptr[2] = p.z; - ptr[3] = n.x; - ptr[4] = n.y; - ptr[5] = n.z; - } - - return LaserScan(laserScan(cv::Range::all(), cv::Range(0, oi)), 0, 0.0f, LaserScan::kXYZNormal); -} - -LaserScan laserScanFromPointCloudXYZRGBNormal(const std::shared_ptr &cloud) { - cv::Mat laserScan = cv::Mat(1, (int)cloud->size(), CV_32FC(7)); - int oi = 0; - - for (unsigned int i = 0; i < cloud->size(); ++i) { - float *ptr = laserScan.ptr(0, oi++); - const spectacularAI::Vector3f &p = cloud->getPosition(i); - const spectacularAI::Vector3f &n = cloud->getNormal(i); - const std::array &rgb = cloud->getRGB24(i); - ptr[0] = p.x; - ptr[1] = p.y; - ptr[2] = p.z; - ptr[4] = n.x; - ptr[5] = n.y; - ptr[6] = n.z; - int *ptrInt = (int *)ptr; - ptrInt[3] = int(rgb[2]) | (int(rgb[1]) << 8) | (int(rgb[0]) << 16); - } - - return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGBNormal); -} - -} // anonymous namespace - -LaserScan laserScanFromPointCloud(const std::shared_ptr &cloud) { - if (cloud->empty()) - return LaserScan(); - else if (cloud->hasNormals() && cloud->hasColors()) - return laserScanFromPointCloudXYZRGBNormal(cloud); - else if (cloud->hasNormals()) - return laserScanFromPointCloudXYZNormal(cloud); - else if (cloud->hasColors()) - return laserScanFromPointCloudXYZRGB(cloud); - else - return laserScanFromPointCloudXYZ(cloud); -} - -int colorFormatToOpenCVType(spectacularAI::ColorFormat colorFormat) { - switch (colorFormat) { - case spectacularAI::ColorFormat::GRAY: return CV_8UC1; - case spectacularAI::ColorFormat::RGB: return CV_8UC3; - case spectacularAI::ColorFormat::RGBA: return CV_8UC4; - default: UFATAL("Unknown color format!"); return -1; - } -} - -cv::Mat convert(const spectacularAI::Matrix3d &m) { - cv::Mat mat = cv::Mat::zeros(3, 3, CV_64FC1); - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - mat.at(i, j) = m[i][j]; - } - } - - return mat; -} - -Transform convert(const spectacularAI::Pose &pose) { - const spectacularAI::Vector3d& p = pose.position; - const spectacularAI::Quaternion& q = pose.orientation; - - return Transform(p.x, p.y, p.z, q.x, q.y, q.z, q.w); -} - -CameraModel convert( - const std::shared_ptr &camera, - int width, int height -) { - // K is the camera intrinsic 3x3 CV_64FC1 - // D is the distortion coefficients 1x5 CV_64FC1 - // R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity) - // P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]']) - cv::Mat K = convert(camera->getIntrinsicMatrix()); - cv::Mat D = cv::Mat::zeros(1, 5, CV_64FC1); - cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1); - cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1); - P.at(0, 0) = K.at(0, 0); - P.at(1, 1) = K.at(1, 1); - P.at(0, 2) = K.at(0, 2); - P.at(1, 2) = K.at(1, 2); - P.at(2, 3) = 1; - - return CameraModel( - "", - cv::Size(width, height), - K, D, R, P, - Transform::getIdentity()); -} - -cv::Mat convertColor(const std::shared_ptr &image) { - assert(image); - cv::Mat bgr; - const std::uint8_t* data = image->getDataReadOnly(); - int type = colorFormatToOpenCVType(image->getColorFormat()); - - cv::Mat color = cv::Mat( - image->getHeight(), - image->getWidth(), - type, - const_cast(data)).clone(); - - if (type == CV_8UC4) { - cv::cvtColor(color, bgr, cv::COLOR_RGBA2BGR); - } else if (type == CV_8UC3) { - cv::cvtColor(color, bgr, cv::COLOR_RGB2BGR); - } else if (type == CV_8UC1) { - cv::cvtColor(color, bgr, cv::COLOR_GRAY2BGR); - } - - return bgr; -} - -cv::Mat convertDepth(const std::shared_ptr &image) { - assert(image); - assert(image->getColorFormat() == spectacularAI::ColorFormat::GRAY16); - const std::uint8_t* data = image->getDataReadOnly(); - - return cv::Mat( - image->getHeight(), - image->getWidth(), - CV_16UC1, - const_cast(data)).clone(); -} - -} // namespace util -} // namespace rtabmap