feat(slam): add rtabmap_ros

2025-07-14 11:34:38 +08:00
parent 3b6641c1fb
commit 943ce5b06f
1635 changed files with 603092 additions and 0 deletions
@@ -0,0 +1,171 @@
+/*
+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.
+*/
+
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+void showUsage()
+{
+	printf("Usage:\n"
+			"imagesJoiner.exe [option] path\n"
+			"imagesJoiner.exe path_left path_right\n"
+			"  Options:\n"
+			"    -inv       option for copying odd images on the right\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	bool inv = false;
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(strcmp(argv[i], "-inv") == 0)
+		{
+			inv = true;
+			printf(" Inversing option activated...\n");
+			continue;
+		}
+		if(argc > 3)
+		{
+			showUsage();
+			printf(" Not recognized option: \"%s\"\n", argv[i]);
+		}
+	}
+
+	std::string path, pathRight;
+
+	if(argc == 3 && !inv)
+	{
+		//two paths
+		path = argv[1];
+		pathRight = argv[2];
+
+		printf(" Path left = %s\n", path.c_str());
+		printf(" Path right = %s\n", pathRight.c_str());
+	}
+	else
+	{
+		path = argv[argc-1];
+		printf(" Path = %s\n", path.c_str());
+	}
+
+	UDirectory dir(path, "jpg bmp png tiff jpeg");
+	UDirectory dirRight(pathRight, "jpg bmp png tiff jpeg");
+	if(!dir.isValid() || (!pathRight.empty() && !dirRight.isValid()))
+	{
+		printf("Path invalid!\n");
+		exit(-1);
+	}
+
+	std::string targetDirectory = path+"_joined";
+	UDirectory::makeDir(targetDirectory);
+	printf(" Creating directory \"%s\"\n", targetDirectory.c_str());
+
+
+	std::string fileNameA = dir.getNextFilePath();
+	std::string fileNameB;
+	if(dirRight.isValid())
+	{
+		fileNameB = dirRight.getNextFilePath();
+	}
+	else
+	{
+		fileNameB = dir.getNextFilePath();
+	}
+
+	int i=1;
+	while(!fileNameA.empty() && !fileNameB.empty())
+	{
+		if(inv)
+		{
+			std::string tmp = fileNameA;
+			fileNameA = fileNameB;
+			fileNameB = tmp;
+		}
+
+		std::string ext = UFile::getExtension(fileNameA);
+
+		std::string targetFilePath = targetDirectory+UDirectory::separator()+uNumber2Str(i++)+"."+ext;
+
+		cv::Mat imageA = cv::imread(fileNameA.c_str());
+		cv::Mat imageB = cv::imread(fileNameB.c_str());
+
+		fileNameA.clear();
+		fileNameB.clear();
+
+		if(!imageA.empty() && !imageB.empty())
+		{
+			cv::Size sizeA = imageA.size();
+			cv::Size sizeB = imageB.size();
+			cv::Size targetSize(0,0);
+			targetSize.width = sizeA.width + sizeB.width;
+			targetSize.height = sizeA.height > sizeB.height ? sizeA.height : sizeB.height;
+			cv::Mat targetImage(targetSize, imageA.type());
+
+			cv::Mat roiA(targetImage, cv::Rect( 0, 0, sizeA.width, sizeA.height ));
+			imageA.copyTo(roiA);
+			cv::Mat roiB( targetImage, cv::Rect( sizeA.width, 0, sizeB.width, sizeB.height ) );
+			imageB.copyTo(roiB);
+
+			if(!cv::imwrite(targetFilePath.c_str(), targetImage))
+			{
+				printf("Error : saving to \"%s\" goes wrong...\n", targetFilePath.c_str());
+			}
+			else
+			{
+				printf("Saved \"%s\" \n", targetFilePath.c_str());
+			}
+
+			fileNameA = dir.getNextFilePath();
+			if(dirRight.isValid())
+			{
+				fileNameB = dirRight.getNextFilePath();
+			}
+			else
+			{
+				fileNameB = dir.getNextFilePath();
+			}
+		}
+		else
+		{
+			printf("Error: loading images failed!\n");
+		}
+	}
+	printf("%d files processed\n", i-1);
+
+	return 0;
+}