i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged pcl_integration branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-12-11 00:12:44 +00:00
parent 97c70d394e
commit 8b8511e154
124 changed files with 21692 additions and 4458 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.cproject
+547 -449
View File
@@ -1,449 +1,547 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="0.1790260204">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204" moduleId="org.eclipse.cdt.core.settings" name="Unix">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="RTAB-Map" buildProperties="" description="Ubuntu 9.10" id="0.1790260204" name="Unix" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.1790260204." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.379189688" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.379189688.899900990" name=""/>
<builder arguments="-C ${ProjDirPath}/build VERBOSE=true" buildPath="${workspace_loc:/RTAB-Map}" command="make" id="org.eclipse.cdt.build.core.settings.default.builder.1868197384" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.45793018" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.894008781" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.584706788" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1694566822" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.1789919121" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.449413398" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1666253491" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.1172725717" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.508497091" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.230409752" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
</toolChain>
</folderInfo>
<sourceEntries>
<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cconfiguration>
<cconfiguration id="0.1790260204.1906025362">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204.1906025362" moduleId="org.eclipse.cdt.core.settings" name="MinGW">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="RTAB-Map" buildProperties="" description="Windows XP" id="0.1790260204.1906025362" name="MinGW" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.1790260204.1906025362." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1655844623" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.1655844623.223670391" name=""/>
<builder arguments="-C ${ProjDirPath}/build VERBOSE=true" buildPath="${workspace_loc:/RTAB-Map}" command="mingw32-make" id="org.eclipse.cdt.build.core.settings.default.builder.1893174344" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.359841287" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.2036478542" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.562568401" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths"/>
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1681573069" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1080087738" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.2076089335" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1846481663" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.865357815" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.469283430" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1029977695" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1452552582" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
</toolChain>
</folderInfo>
<sourceEntries>
<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="CTAB-Map.null.2089313587" name="CTAB-Map"/>
</storageModule>
<storageModule moduleId="refreshScope" versionNumber="1">
<resource resourceType="PROJECT" workspacePath="/rtabmap"/>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets>
<target name="CMake-MinGW-Debug" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CMake-MinGW-Release" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=ON ../</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CMake-Unix-Debug" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=OFF ../</buildArguments>
<buildTarget/>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CMake-Unix-Release" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=OFF ../</buildArguments>
<buildTarget/>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<scannerConfigBuildInfo instanceId="0.1790260204">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="0.1790260204.1906025362">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
</scannerConfigBuildInfo>
</storageModule>
</cproject>
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="0.1790260204">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204" moduleId="org.eclipse.cdt.core.settings" name="Unix">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="RTAB-Map" buildProperties="" description="Ubuntu/Mac OS X" id="0.1790260204" name="Unix" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.1790260204." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.379189688" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.379189688.899900990" name=""/>
<builder arguments="-C ${ProjDirPath}/build VERBOSE=true" buildPath="${workspace_loc:/RTAB-Map}" command="make" id="org.eclipse.cdt.build.core.settings.default.builder.1868197384" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.45793018" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.894008781" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.584706788" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1694566822" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.1789919121" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.449413398" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1666253491" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.1172725717" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.508497091" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths" valueType="undefIncludePath">
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ts/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/imgproc/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/photo/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include/opencv"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/build"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/objdetect/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/stitching/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/core/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/calib3d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/highgui/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/features2d/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/ml/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/nonfree/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/flann/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/gpu/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/videostab/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/video/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/legacy/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.4.0-beta2/modules/contrib/include"/>
<listOptionValue builtIn="false" value="/Users/MatLab/workspace/OpenCV-2.3.1/modules/objdetect/include"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.230409752" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
</toolChain>
</folderInfo>
<sourceEntries>
<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cconfiguration>
<cconfiguration id="0.1790260204.1906025362">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204.1906025362" moduleId="org.eclipse.cdt.core.settings" name="MinGW">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="RTAB-Map" buildProperties="" description="Windows GCC" id="0.1790260204.1906025362" name="MinGW" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.1790260204.1906025362." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1655844623" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.1655844623.223670391" name=""/>
<builder arguments="-C ${ProjDirPath}/build VERBOSE=true" buildPath="${workspace_loc:/RTAB-Map}" command="nmake" id="org.eclipse.cdt.build.core.settings.default.builder.1893174344" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.359841287" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.2036478542" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.562568401" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths"/>
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1681573069" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1080087738" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.2076089335" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1846481663" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.865357815" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.469283430" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.1029977695" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\utilite\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;M:\opencv-svn\build\install\include&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1452552582" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
</toolChain>
</folderInfo>
<sourceEntries>
<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cconfiguration>
<cconfiguration id="0.1790260204.1906025362.1064002412">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1790260204.1906025362.1064002412" moduleId="org.eclipse.cdt.core.settings" name="NMake">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.PE" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.SOM" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="RTAB-Map" buildProperties="" description="Windows Visual Studio" id="0.1790260204.1906025362.1064002412" name="NMake" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.1790260204.1906025362.1064002412." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.2087892413" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.SOM;org.eclipse.cdt.core.PE;org.eclipse.cdt.core.MachO64" id="org.eclipse.cdt.build.core.prefbase.toolchain.2087892413.1761856661" name=""/>
<builder arguments="-j4" buildPath="${ProjDirPath}/build" command="jom" id="org.eclipse.cdt.build.core.settings.default.builder.826759435" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1653679154" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.1610311642" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.undef.incpaths.1896517824" name="Undefined Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.undef.incpaths"/>
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.700166558" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\opencv\build\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\include\pcl-1.6&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Boost\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Eigen\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\VTK\include\vtk-5.8&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.112516982" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.1962594580" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.2029836145" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\opencv\build\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\include\pcl-1.6&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Boost\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Eigen\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\VTK\include\vtk-5.8&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.2132965672" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.463224635" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<option id="org.eclipse.cdt.build.core.settings.holder.incpaths.119135709" name="Include Paths" superClass="org.eclipse.cdt.build.core.settings.holder.incpaths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;C:\opencv\build\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\Microsoft Visual Studio 10.0\VC\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\include\pcl-1.6&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Boost\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\Eigen\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Qt\4.8.0\include&quot;"/>
<listOptionValue builtIn="false" value="&quot;C:\Program Files (x86)\PCL 1.6.0\3rdParty\VTK\include\vtk-5.8&quot;"/>
</option>
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1193489184" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
</toolChain>
</folderInfo>
<sourceEntries>
<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="CTAB-Map.null.2089313587" name="CTAB-Map"/>
</storageModule>
<storageModule moduleId="refreshScope" versionNumber="2">
<configuration configurationName="Unix">
<resource resourceType="PROJECT" workspacePath="/rtabmap"/>
</configuration>
<configuration configurationName="NMake">
<resource resourceType="PROJECT" workspacePath="/rtabmap"/>
</configuration>
<configuration configurationName="MinGW">
<resource resourceType="PROJECT" workspacePath="/rtabmap"/>
</configuration>
<configuration configurationName="MSVC">
<resource resourceType="PROJECT" workspacePath="/rtabmap"/>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<scannerConfigBuildInfo instanceId="0.1790260204.1906025362.1064002412">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="0.1790260204">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="0.1790260204.1906025362">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets>
<target name="CMake-MinGW-Debug" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CMake-MinGW-Release" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=ON ../</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CMake-Unix-Debug" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=OFF ../</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CMake-Unix-Release" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=OFF ../</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CMake-NMake-Debug" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-E chdir build/ cmake -G "NMake Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="CMake-NMake-Release" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>-G "NMake Makefiles" -D CMAKE_BUILD_TYPE=Release ../</buildArguments>
<buildTarget/>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cproject>
CMakeLists.txt
+87 -20
View File
@@ -5,7 +5,7 @@ IF(APPLE OR WIN32)
ELSE()
cmake_minimum_required(VERSION 2.8.0)
ENDIF()
PROJECT( RTAB-Map )
PROJECT( RTABMap )
SET(PROJECT_PREFIX rtabmap)
####### local cmake modules #######
@@ -14,13 +14,17 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
# VERSION
#######################
SET(PROJECT_VERSION "0.5.0")
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 6)
SET(RTABMAP_PATCH_VERSION 1)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
SET(PROJECT_VERSION "${RTABMAP_VERSION}")
STRING(REGEX MATCHALL "[0-9]" PROJECT_VERSION_PARTS "${PROJECT_VERSION}")
LIST(GET PROJECT_VERSION_PARTS 0 PROJECT_VERSION_MAJOR)
LIST(GET PROJECT_VERSION_PARTS 1 PROJECT_VERSION_MINOR)
LIST(GET PROJECT_VERSION_PARTS 2 PROJECT_VERSION_PATCH)
SET(PROJECT_VERSION_MAJOR ${RTABMAP_MAJOR_VERSION})
SET(PROJECT_VERSION_MINOR ${RTABMAP_MINOR_VERSION})
SET(PROJECT_VERSION_PATCH ${RTABMAP_PATCH_VERSION})
SET(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
@@ -36,10 +40,11 @@ ENDIF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
SET(CMAKE_DEBUG_POSTFIX "d")
ADD_DEFINITIONS( "-Wall" )
IF(WIN32 AND NOT MINGW)
ADD_DEFINITIONS("-wd4100 -wd4512 -wd4548 -wd4619 -wd4625 -wd4626 -wd4668 -wd4710 -wd4820")
ADD_DEFINITIONS("-DNOMINMAX")
ADD_DEFINITIONS("-wd4100 -wd4127 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986")
ELSE ()
ADD_DEFINITIONS( "-Wall" )
ADD_DEFINITIONS("-Wno-unknown-pragmas")
ENDIF()
@@ -110,19 +115,50 @@ SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
####### INSTALL DIR #######
# Offer the user the choice of overriding the installation directories
set(INSTALL_LIB_DIR lib/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH "Installation directory for libraries")
set(INSTALL_BIN_DIR bin CACHE PATH "Installation directory for executables")
set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH
"Installation directory for header files")
if(WIN32 AND NOT CYGWIN)
set(DEF_INSTALL_CMAKE_DIR CMake)
else()
set(DEF_INSTALL_CMAKE_DIR lib/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
endif()
set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
"Installation directory for CMake files")
######## for RTABMapConfig.cmake #########
# Make relative paths absolute (needed later on)
foreach(p LIB BIN INCLUDE CMAKE)
set(var INSTALL_${p}_DIR)
if(NOT IS_ABSOLUTE "${${var}}")
set(${var} "${CMAKE_INSTALL_PREFIX}/${${var}}")
endif()
endforeach()
####### BUILD OPTIONS #######
OPTION(BUILD_LIBS_ONLY "Set to ON to build only the libraries" OFF)
IF(APPLE)
OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
ENDIF(APPLE)
OPTION(DEMO_BUILD "Set to ON to build DEMO version" OFF)
####### DEPENDENCIES #######
FIND_PACKAGE(OpenCV REQUIRED)
FIND_PACKAGE(Sqlite3 REQUIRED)
FIND_PACKAGE(PCL 1.7 REQUIRED)
FIND_PACKAGE(VTK REQUIRED)
FIND_PACKAGE(ZLIB REQUIRED)
# If Qt is here, the GUI will be built
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
IF(DEMO_BUILD)
ADD_DEFINITIONS(-DDEMO_BUILD)
ENDIF(DEMO_BUILD)
####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
IF(APPLE AND BUILD_AS_BUNDLE)
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
@@ -193,6 +229,47 @@ CONFIGURE_FILE(
ADD_CUSTOM_TARGET(uninstall
"${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
####
# Setup RTABMapConfig.cmake
####
# Add all targets to the build-tree export set
IF(BUILD_LIBS_ONLY)
export(TARGETS rtabmap_core rtabmap_gui rtabmap_utilite
FILE "${PROJECT_BINARY_DIR}/RTABMapTargets.cmake")
ELSE()
export(TARGETS rtabmap rtabmap_core rtabmap_gui rtabmap_utilite
FILE "${PROJECT_BINARY_DIR}/RTABMapTargets.cmake")
ENDIF()
# Export the package for use from the build-tree
# (this registers the build-tree with a global CMake-registry)
export(PACKAGE RTABMap)
# Create the RTABMapConfig.cmake and RTABMapConfigVersion files
file(RELATIVE_PATH REL_INCLUDE_DIR "${INSTALL_CMAKE_DIR}"
"${INSTALL_INCLUDE_DIR}")
# ... for the build tree
set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}" "${PROJECT_BINARY_DIR}")
configure_file(RTABMapConfig.cmake.in
"${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY)
# ... for the install tree
set(CONF_INCLUDE_DIRS "\${RTABMap_CMAKE_DIR}/${REL_INCLUDE_DIR}")
configure_file(RTABMapConfig.cmake.in
"${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake" @ONLY)
# ... for both
configure_file(RTABMapConfigVersion.cmake.in
"${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake" @ONLY)
# Install the RTABMapConfig.cmake and RTABMapConfigVersion.cmake
install(FILES
"${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/RTABMapConfig.cmake"
"${PROJECT_BINARY_DIR}/RTABMapConfigVersion.cmake"
DESTINATION "${INSTALL_CMAKE_DIR}" COMPONENT devel)
# Install the export set for use with the install-tree
install(EXPORT RTABMapTargets DESTINATION
"${INSTALL_CMAKE_DIR}" COMPONENT devel)
####
#######################
# CPACK (Packaging)
@@ -223,17 +300,6 @@ set(CPACK_SOURCE_IGNORE_FILES
"\\\\.DS_Store"
)
# Share files
INSTALL(FILES
${PROJECT_SOURCE_DIR}/Matlab/ShowLogs/showlogs.m
${PROJECT_SOURCE_DIR}/Matlab/ShowLogs/getPrecisionRecall.m
${PROJECT_SOURCE_DIR}/Matlab/ShowLogs/importfile.m
DESTINATION share/${PROJECT_PREFIX}
COMPONENT runtime)
INSTALL(FILES ${PROJECT_SOURCE_DIR}/cmake_modules/FindRTABMap.cmake DESTINATION share/${PROJECT_PREFIX} COMPONENT devel)
IF(WIN32)
SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_SOURCE_DIR}/COPYING.txt")
@@ -301,4 +367,5 @@ MESSAGE(STATUS " BUILD_LIBS_ONLY = ${BUILD_LIBS_ONLY}")
IF(APPLE)
MESSAGE(STATUS " BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
ENDIF(APPLE)
MESSAGE(STATUS " DEMO_BUILD = ${DEMO_BUILD}")
MESSAGE(STATUS "--------------------------------------------")
RTABMapConfig.cmake.in
+16
View File
@@ -0,0 +1,16 @@
# - Config file for the RTABMap package
# It defines the following variables
# RTABMap_INCLUDE_DIRS - include directories for RTABMap
# RTABMap_LIBRARIES - libraries to link against
# RTABMap_EXECUTABLE - the bar executable
# Compute paths
get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
# Our library dependencies (contains definitions for IMPORTED targets)
include("${RTABMap_CMAKE_DIR}/RTABMapTargets.cmake")
# These are IMPORTED targets created by RTABMapTargets.cmake
set(RTABMap_LIBRARIES rtabmap_core rtabmap_gui rtabmap_utilite)
set(RTABMap_EXECUTABLE rtabmap)
RTABMapConfigVersion.cmake.in
+11
View File
@@ -0,0 +1,11 @@
set(PACKAGE_VERSION "@RTABMAP_VERSION@")
# Check whether the requested PACKAGE_FIND_VERSION is compatible
if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
set(PACKAGE_VERSION_COMPATIBLE FALSE)
else()
set(PACKAGE_VERSION_COMPATIBLE TRUE)
if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
set(PACKAGE_VERSION_EXACT TRUE)
endif()
endif()
app/src/CMakeLists.txt
+16 -8
View File
@@ -18,6 +18,7 @@ SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/guilib/include
${CMAKE_CURRENT_SOURCE_DIR}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
INCLUDE(${QT_USE_FILE})
@@ -25,8 +26,12 @@ INCLUDE(${QT_USE_FILE})
SET(LIBRARIES
${QT_LIBRARIES}
${OpenCV_LIBS}
${PCL_LIBRARIES}
)
# rc.exe has problems with these defintions... commented!
#add_definitions(${PCL_DEFINITIONS})
# Make sure the compiler can find include files from our library.
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
@@ -55,28 +60,31 @@ ENDIF(WIN32)
# Add binary
IF(APPLE AND BUILD_AS_BUNDLE)
ADD_EXECUTABLE(main_app MACOSX_BUNDLE ${SRC_FILES})
ADD_EXECUTABLE(rtabmap MACOSX_BUNDLE ${SRC_FILES})
ELSEIF(MINGW)
ADD_EXECUTABLE(rtabmap WIN32 ${SRC_FILES})
ELSE()
ADD_EXECUTABLE(main_app WIN32 ${SRC_FILES})
ADD_EXECUTABLE(rtabmap ${SRC_FILES})
ENDIF()
TARGET_LINK_LIBRARIES(main_app rtabmap_corelib rtabmap_guilib rtabmap_utilite ${LIBRARIES})
TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
IF(APPLE AND BUILD_AS_BUNDLE)
SET_TARGET_PROPERTIES(main_app PROPERTIES
SET_TARGET_PROPERTIES(rtabmap PROPERTIES
OUTPUT_NAME ${CMAKE_BUNDLE_NAME})
ELSEIF(WIN32)
SET_TARGET_PROPERTIES(main_app PROPERTIES
SET_TARGET_PROPERTIES(rtabmap PROPERTIES
OUTPUT_NAME ${PROJECT_NAME})
ELSE()
SET_TARGET_PROPERTIES(main_app PROPERTIES
SET_TARGET_PROPERTIES(rtabmap PROPERTIES
OUTPUT_NAME ${PROJECT_PREFIX})
ENDIF()
#---------------------------
# Installation stuff
#---------------------------
INSTALL(TARGETS main_app
RUNTIME DESTINATION bin COMPONENT runtime
INSTALL(TARGETS rtabmap
EXPORT RTABMapTargets
RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
IF(APPLE AND BUILD_AS_BUNDLE)
app/src/RTAB-Map.rc
-1
View File
@@ -1 +0,0 @@
IDI_ICON1 ICON DISCARDABLE "RTAB-Map.ico"
app/src/RTAB-Map.icns → app/src/RTABMap.icns
View File
app/src/RTAB-Map.ico → app/src/RTABMap.ico
View File
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 52 KiB

After

Width:  |  Height:  |  Size: 52 KiB

app/src/RTABMap.rc
+1
View File
@@ -0,0 +1 @@
IDI_ICON1 ICON DISCARDABLE "RTABMap.ico"
app/src/main.cpp
-1
View File
@@ -46,7 +46,6 @@ int main(int argc, char* argv[])
mainWindow->showNormal();
RtabmapThread * rtabmap = new RtabmapThread();
rtabmap->setWorkingDirectory(mainWindow->getWorkingDirectory().toStdString());
rtabmap->start(); // start it not initialized... will be initialized by event from the gui
UEventsManager::addHandler(rtabmap);
cmake_modules/FindRTABMap.cmake
-65
View File
@@ -1,65 +0,0 @@
# - Find RTABMap
# This module finds an installed RTABMap package.
#
# It sets the following variables:
# RTABMap_FOUND - Set to false, or undefined, if RTABMap isn't found.
# RTABMap_INCLUDE_DIRS - The RTABMap include directory.
# RTABMap_LIBRARIES - The RTABMap library to link against.
#
# Look up for rtabmap ros package first, if not found search system wide.
#
SET(RTABMap_ROOT)
# Add ROS RTABMap directory if ROS is installed
FIND_PROGRAM(ROSPACK_EXEC NAME rospack PATHS)
IF(ROSPACK_EXEC)
EXECUTE_PROCESS(COMMAND ${ROSPACK_EXEC} find rtabmap_lib
OUTPUT_VARIABLE RTABMap_ROS_PATH
OUTPUT_STRIP_TRAILING_WHITESPACE
WORKING_DIRECTORY "./"
)
IF(RTABMap_ROS_PATH)
MESSAGE(STATUS "Found RTABMap ROS pkg : ${RTABMap_ROS_PATH}")
SET(RTABMap_ROOT
${RTABMap_ROS_PATH}/rtabmap
${RTABMap_ROOT}
)
ENDIF(RTABMap_ROS_PATH)
ENDIF(ROSPACK_EXEC)
IF(WIN32)
FIND_PATH(RTABMap_INCLUDE_DIRS
rtabmap/core/Rtabmap.h
PATH_SUFFIXES "../include")
FIND_LIBRARY(RTABMap_LIBRARIES
NAMES rtabmap_core
PATH_SUFFIXES "../lib")
ELSE()
FIND_PATH(RTABMap_INCLUDE_DIRS
rtabmap/core/Rtabmap.h
PATHS ${RTABMap_ROOT}/include)
FIND_LIBRARY(RTABMap_LIBRARIES
NAMES rtabmap_core
PATHS ${RTABMap_ROOT}/lib)
ENDIF()
IF (RTABMap_INCLUDE_DIRS AND RTABMap_LIBRARIES)
SET(RTABMap_FOUND TRUE)
ENDIF (RTABMap_INCLUDE_DIRS AND RTABMap_LIBRARIES)
IF (RTABMap_FOUND)
# show which RTABMap was found only if not quiet
IF (NOT RTABMap_FIND_QUIETLY)
MESSAGE(STATUS "Found RTABMap ${RTABMap_VERSION}")
ENDIF (NOT RTABMap_FIND_QUIETLY)
ELSE ()
# fatal error if RTABMap is required but not found
IF (RTABMap_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find RTABMap. Verify your PATH if it is already installed or download it at http://rtabmap.googlecode.com")
ENDIF (RTABMap_FIND_REQUIRED)
ENDIF ()
corelib/include/rtabmap/core/Camera.h
+7 -9
View File
@@ -66,7 +66,6 @@ public:
virtual void parseParameters(const ParametersMap & parameters);
int id() const {return _id;}
protected:
/**
@@ -74,13 +73,15 @@ protected:
*
* @param imageRate : image/second , 0 for fast as the camera can
*/
Camera(float imageRate = 0, unsigned int imageWidth = 0, unsigned int imageHeight = 0, unsigned int framesDropped = 0, int id = 0);
Camera(float imageRate = 0,
unsigned int imageWidth = 0,
unsigned int imageHeight = 0,
unsigned int framesDropped = 0);
virtual cv::Mat captureImage() = 0;
private:
float _imageRate;
int _id;
unsigned int _imageWidth;
unsigned int _imageHeight;
unsigned int _framesDropped;
@@ -105,8 +106,7 @@ public:
float imageRate = 0,
unsigned int imageWidth = 0,
unsigned int imageHeight = 0,
unsigned int framesDropped = 0,
int id = 0);
unsigned int framesDropped = 0);
virtual ~CameraImages();
virtual bool init();
@@ -143,14 +143,12 @@ public:
float imageRate = 0,
unsigned int imageWidth = 0,
unsigned int imageHeight = 0,
unsigned int framesDropped = 0,
int id = 0);
unsigned int framesDropped = 0);
CameraVideo(const std::string & filePath,
float imageRate = 0,
unsigned int imageWidth = 0,
unsigned int imageHeight = 0,
unsigned int framesDropped = 0,
int id = 0);
unsigned int framesDropped = 0);
virtual ~CameraVideo();
virtual bool init();
corelib/include/rtabmap/core/CameraEvent.h
+17 -12
View File
@@ -34,29 +34,35 @@ public:
enum Code {
kCodeFeatures,
kCodeImage,
kCodeImageDepth,
kCodeNoMoreImages
};
public:
CameraEvent(const cv::Mat & image, int cameraId = 0) :
CameraEvent(const cv::Mat & image, int seq=0) :
UEvent(kCodeImage),
_cameraId(cameraId),
_image(image)
_image(image, seq)
{
}
CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int cameraId = 0) :
CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int seq=0) :
UEvent(kCodeFeatures),
_cameraId(cameraId),
_image(image, 0, descriptors, keypoints)
_image(image, seq, descriptors, keypoints)
{
}
CameraEvent(int cameraId = 0) :
UEvent(kCodeNoMoreImages),
_cameraId(cameraId)
CameraEvent() :
UEvent(kCodeNoMoreImages)
{
}
CameraEvent(const cv::Mat & image, const cv::Mat & depth, float depthConstant, const Transform & localTransform, int seq=0) :
UEvent(kCodeImageDepth),
_image(image, depth, depthConstant, Transform(), localTransform, seq)
{
}
CameraEvent(const cv::Mat & image, const cv::Mat & depth, const cv::Mat & depth2d, float depthConstant, const Transform & localTransform, int seq=0) :
UEvent(kCodeImageDepth),
_image(image, depth, depth2d, depthConstant, Transform(), localTransform, seq)
{
}
int cameraId() const {return _cameraId;}
// Image or descriptors
const Image & image() const {return _image;}
@@ -65,7 +71,6 @@ public:
virtual std::string getClassName() const {return std::string("CameraEvent");}
private:
int _cameraId;
Image _image;
};
corelib/include/rtabmap/core/CameraOpenni.h
+63
View File
@@ -0,0 +1,63 @@
/*
* CameraOpenni.h
*
* Created on: 2013-08-22
* Author: Mathieu
*/
#ifndef CAMERAOPENNI_H_
#define CAMERAOPENNI_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/io/openni_camera/openni_depth_image.h>
#include <pcl/io/openni_camera/openni_image.h>
#include <boost/signals2/connection.hpp>
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/UEventsSender.h>
class UTimer;
namespace pcl
{
class Grabber;
}
namespace rtabmap {
class RTABMAP_EXP CameraOpenni : public UEventsSender
{
public:
// default local transform z in, x right, y down));
CameraOpenni(const std::string & deviceId="",
float rate=0,
const Transform & localTRansform = Transform::getIdentity());
virtual ~CameraOpenni();
void image_cb (
const boost::shared_ptr<openni_wrapper::Image>& rgb,
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant);
bool init();
void start();
void pause();
void kill();
bool isRunning();
void setFrameRate(float rate);
private:
pcl::Grabber* interface_;
std::string deviceId_;
float rate_;
UTimer * frameRateTimer_;
Transform localTransform_; // transform from camera_optical_link to base_link
int seq_;
boost::signals2::connection connection_;
};
} /* namespace rtabmap */
#endif /* CAMERAOPENNI_H_ */
corelib/include/rtabmap/core/CameraThread.h
+4 -1
View File
@@ -49,11 +49,13 @@ public:
CameraThread(Camera * camera, bool autoRestart = false);
virtual ~CameraThread();
bool init(); // call camera->init()
//getters
bool isPaused() const {return !this->isRunning();}
bool isCapturing() const {return this->isRunning();}
void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
void setImageRate(float imageRate);
Camera * getCamera() {return _camera;}
protected:
virtual void handleEvent(UEvent* anEvent);
@@ -69,6 +71,7 @@ private:
std::stack<State> _state;
std::stack<ParametersMap> _stateParam;
bool _autoRestart;
int _seq;
};
} // namespace rtabmap
corelib/include/rtabmap/core/DBDriver.h
+19 -12
View File
@@ -31,6 +31,8 @@
#include "rtabmap/utilite/UThreadNode.h"
#include "rtabmap/core/Parameters.h"
#include <rtabmap/core/Transform.h>
namespace rtabmap {
class Signature;
@@ -62,11 +64,9 @@ public:
void asyncSave(VisualWord * vw); //ownership transferred
void emptyTrashes(bool async = false);
double getEmptyTrashesTime() const {return _emptyTrashesTime;}
bool isImagesCompressed() const {return _imagesCompressed;}
public:
void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const;
void addStatisticsAfterRunSurf(int dictionarySize) const;
void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
public:
// Mutex-protected methods of abstract versions below
@@ -82,20 +82,24 @@ public:
// Load objects
void load(VWDictionary * dictionary) const;
void load(std::map<int, std::map<int, Transform> > & mapTransforms) const;
void loadLastNodes(std::list<Signature *> & signatures) const;
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
// Specific queries...
void getImage(int id, cv::Mat & image) const;
void getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
void loadNeighbors(int signatureId, std::set<int> & neighbors) const;
void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
void getNodeData(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & depthConstant, Transform & localTransform) const;
void getNodeData(int signatureId, std::vector<unsigned char> & image) const;
void getPose(int signatureId, Transform & pose, int & mapId) const;
void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const;
void getWeight(int signatureId, int & weight) const;
void getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
void getAllNodeIds(std::set<int> & ids) const;
void getLastNodeId(int & id) const;
void getLastWordId(int & id) const;
void getInvertedIndexNi(int signatureId, int & ni) const;
void save(const std::map<int, std::map<int, Transform> > & mapTransforms) const;
protected:
DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -108,24 +112,28 @@ private:
virtual void executeNoResultQuery(const std::string & sql) const = 0;
virtual void getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const = 0;
virtual void getWeightQuery(int signatureId, int & weight) const = 0;
virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const = 0;
virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
virtual void updateQuery(const std::list<VisualWord *> & words) const = 0;
virtual void saveQuery(const std::map<int, std::map<int, Transform> > & mapTransforms) const = 0;
// Load objects
virtual void loadQuery(VWDictionary * dictionary) const = 0;
virtual void loadQuery(std::map<int, std::map<int, Transform> > & mapTransforms) const = 0;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadNeighborsQuery(int signatureId, std::set<int> & neighbors) const = 0;
virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const = 0;
virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0;
virtual void getImageQuery(int id, cv::Mat & rawData) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & depthConstant, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const = 0;
virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
@@ -145,7 +153,6 @@ private:
UMutex _trashesMutex;
UMutex _dbSafeAccessMutex;
USemaphore _addSem;
bool _imagesCompressed;
double _emptyTrashesTime;
std::string _url;
};
corelib/include/rtabmap/core/DBReader.h
+7 -3
View File
@@ -12,6 +12,8 @@
#include <rtabmap/utilite/UThreadNode.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UEventsSender.h>
#include <rtabmap/core/Transform.h>
#include <opencv2/core/core.hpp>
@@ -21,15 +23,16 @@ namespace rtabmap {
class DBDriver;
class RTABMAP_EXP DBReader : public UThreadNode {
class RTABMAP_EXP DBReader : public UThreadNode, public UEventsSender {
public:
DBReader(const std::string & databasePath,
float frameRate = 0.0f);
float frameRate = 0.0f,
bool odometryIgnored = false);
virtual ~DBReader();
bool init(int startIndex=0);
void setFrameRate(float frameRate);
void getNextImage(cv::Mat & sensors);
void getNextImage(cv::Mat & image, cv::Mat & depth, cv::Mat & depth2d, float & depthConstant, Transform & localTransform, Transform & pose);
protected:
virtual void mainLoopBegin();
@@ -38,6 +41,7 @@ protected:
private:
std::string _path;
float _frameRate;
bool _odometryIgnored;
DBDriver * _dbDriver;
UTimer _timer;
corelib/include/rtabmap/core/Features2d.h
+12 -6
View File
@@ -30,6 +30,10 @@
namespace rtabmap {
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
/////////////////////
// KeypointDescriptor
/////////////////////
@@ -91,20 +95,22 @@ class RTABMAP_EXP KeypointDetector
public:
enum DetectorType {kDetectorSurf, kDetectorSift, kDetectorUndef};
public:
static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
public:
virtual ~KeypointDetector() {}
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image);
std::vector<cv::KeyPoint> generateKeypoints(
const cv::Mat & image,
int maxKeypoints = 0,
const cv::Rect & roi = cv::Rect());
virtual void parseParameters(const ParametersMap & parameters);
unsigned int getWordsPerImageTarget() const {return _wordsPerImageTarget;}
void setRoi(const std::string & roi);
cv::Rect computeRoi(const cv::Mat & image) const;
protected:
KeypointDetector(const ParametersMap & parameters = ParametersMap());
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const = 0;
private:
unsigned int _wordsPerImageTarget;
std::vector<float> _roiRatios; // size 4
};
//SURFDetector
corelib/include/rtabmap/core/Image.h
+58 -3
View File
@@ -8,11 +8,11 @@
#ifndef IMAGE_H_
#define IMAGE_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <rtabmap/core/Transform.h>
namespace rtabmap
{
@@ -29,21 +29,76 @@ public:
_image(image),
_id(id),
_descriptors(descriptors),
_keypoints(keypoints)
_keypoints(keypoints),
_depthConstant(0.0f),
_localTransform(Transform::getIdentity())
{
}
// Metric constructor
Image(const cv::Mat & image,
const cv::Mat & depth,
float depthConstant,
const Transform & pose,
const Transform & localTransform,
int id = 0) :
_image(image),
_id(id),
_depth(depth),
_depthConstant(depthConstant),
_pose(pose),
_localTransform(localTransform)
{
}
// Metric constructor + 2d depth
Image(const cv::Mat & image,
const cv::Mat & depth,
const cv::Mat & depth2d,
float depthConstant,
const Transform & pose,
const Transform & localTransform,
int id = 0) :
_image(image),
_id(id),
_depth(depth),
_depth2d(depth2d),
_depthConstant(depthConstant),
_pose(pose),
_localTransform(localTransform)
{
}
virtual ~Image() {}
bool empty() const {return _image.empty() && _descriptors.empty() && _keypoints.size() == 0;}
const cv::Mat & image() const {return _image;}
int id() const {return _id;};
const cv::Mat & descriptors() const {return _descriptors;}
const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
void setDescriptors(const cv::Mat & descriptors) {_descriptors = descriptors;}
void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
bool isMetric() const {return !_depth.empty() || _depthConstant != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose) {_pose = pose;}
const cv::Mat & depth() const {return _depth;}
const cv::Mat & depth2d() const {return _depth2d;}
float depthConstant() const {return _depthConstant;}
const Transform & pose() const {return _pose;}
const Transform & localTransform() const {return _localTransform;}
private:
cv::Mat _image;
int _id;
cv::Mat _descriptors;
std::vector<cv::KeyPoint> _keypoints;
// Metric stuff
cv::Mat _depth;
cv::Mat _depth2d;
float _depthConstant;
Transform _pose;
Transform _localTransform;
};
}
corelib/include/rtabmap/core/Memory.h
+75 -27
View File
@@ -42,6 +42,7 @@ class VWDictionary;
class VisualWord;
class KeypointDetector;
class KeypointDescriptor;
class Statistics;
class RTABMAP_EXP Memory
{
@@ -55,64 +56,73 @@ public:
virtual ~Memory();
virtual void parseParameters(const ParametersMap & parameters);
bool update(const Image & image, std::map<std::string, float> & stats);
bool update(const Image & image, Statistics * stats = 0);
bool init(const std::string & dbUrl,
bool dbOverwritten = false,
const ParametersMap & parameters = ParametersMap());
const ParametersMap & parameters = ParametersMap(),
bool postInitEvents = true);
std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids);
int incrementMapId();
int forget(const std::set<int> & ignoredIds = std::set<int>());
std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
int cleanup(const std::list<int> & ignoredIds = std::list<int>());
std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
void emptyTrash();
void joinTrashThread();
bool addLoopClosureLink(int oldId, int newId);
bool addLoopClosureLink(int oldId, int newId, const Transform & transform);
void updateNeighborLink(int fromId, int toId, const Transform & transform);
std::map<int, int> getNeighborsId(int signatureId,
unsigned int margin,
int maxCheckedInDatabase = -1,
bool incrementMarginOnLoop = false,
bool ignoreLoopIds = false,
double * dbAccessTime = 0) const;
void deleteLastLocation();
void deleteLocation(int locationId);
void rejectLastLoopClosure();
void rejectLoopClosure(int oldId, int newId);
//getters
const std::set<int> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
int getMaxStMemSize() const {return _maxStMemSize;}
std::set<int> getNeighborLinks(int signatureId,
Transform getMapTransform(int sourceMapId, int targetMapId) const;
void removeMapTransform(int soureId, int targetId);
void getPose(int locationId,
int targetMapId,
Transform & pose,
bool lookInDatabase = false) const;
std::map<int, Transform> getNeighborLinks(int signatureId,
bool ignoreNeighborByLoopClosure = false,
bool lookInDatabase = false) const;
void getLoopClosureIds(int signatureId,
std::set<int> & loopClosureIds,
std::set<int> & childLoopClosureIds,
std::map<int, Transform> & loopClosureIds,
std::map<int, Transform> & childLoopClosureIds,
bool lookInDatabase = false) const;
bool isRawDataKept() const {return _rawDataKept;}
float getSimilarityThreshold() const {return _similarityThreshold;}
std::map<int, int> getWeights() const;
float getSimilarityOnlyWithLast() const {return _rehearsalOnlyWithLast;}
int getLastSignatureId() const;
const Signature * getLastWorkingSignature() const;
int getDatabaseMemoryUsed() const; // in bytes
double getDbSavingTime() const;
cv::Mat getImage(int signatureId) const;
std::vector<unsigned char> getImage(int signatureId) const;
void getImageDepth(
int locationId, std::vector<unsigned char> & rgb,
std::vector<unsigned char> & depth,
std::vector<unsigned char> & depth2d,
float & depthConstant,
Transform & localTransform) const;
std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
bool isIncremental() const {return _incrementalMemory;}
const Signature * getSignature(int id) const;
bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
bool isIDsGenerated() const {return _generateIds;}
//setters
void setSimilarityThreshold(float similarity);
void setSimilarityOnlyLast(int rehearsalOnlyWithLast) {_rehearsalOnlyWithLast = rehearsalOnlyWithLast;}
void setOldSignatureRatio(float oldSignatureRatio);
void setMaxStMemSize(unsigned int maxStMemSize);
void setRecentWmRatio(float recentWmRatio);
void setRawDataKept(bool rawDataKept) {_rawDataKept = rawDataKept;}
void setRoi(const std::string & roi);
void dumpMemoryTree(const char * fileNameTree) const;
virtual void dumpMemory(std::string directory) const;
@@ -127,8 +137,27 @@ public:
//keypoint stuff
int getVWDictionarySize() const;
std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
void extractKeypointsAndDescriptors(
const cv::Mat & image,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors);
protected:
void getMetricConstraints(
const std::vector<int> & ids,
int targetMapId,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & links,
bool lookInDatabase = false);
Transform computeVisualTransform(int oldId, int newId) const;
Transform computeVisualTransform(const Signature & oldS, const Signature & newS) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess) const;
Transform computeScanMatchingTransform(
int newId,
int oldId,
const std::map<int, Transform> & poses);
private:
void preUpdate();
void addSignatureToStm(Signature * signature);
void clear();
@@ -140,11 +169,11 @@ protected:
const std::set<int> & ignoredIds = std::set<int>());
int getNextId();
void initCountId();
void rehearsal(Signature * signature, std::map<std::string, float> & stats);
void rehearsal(Signature * signature, Statistics * stats = 0);
bool rehearsalMerge(int oldId, int newId);
const std::map<int, Signature*> & getSignatures() const {return _signatures;}
private:
void copyData(const Signature * from, Signature * to);
Signature * createSignature(
const Image & image,
@@ -162,34 +191,53 @@ protected:
private:
// parameters
float _similarityThreshold;
bool _rehearsalOnlyWithLast;
bool _rawDataKept;
bool _keepRehearsedNodesInDb;
bool _incrementalMemory;
int _maxStMemSize;
float _recentWmRatio;
bool _oldDataKeptOnRehearsal;
bool _idUpdatedToNewOneRehearsal;
bool _generateIds;
int _idCount;
int _idMapCount;
Signature * _lastSignature;
int _lastLoopClosureId;
bool _memoryChanged; // False by default, become true when Memory::update() is called.
int _signaturesAdded;
std::vector<std::pair<int, int> > _savedLoopClosureInfo; // size 3 or 0
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id
std::set<int> _workingMem; // id,age
std::map<int, std::map<int, Transform> > _mapTransforms; // Transform between maps <fromMapId, <toMapId, transform> >
//Heypoint stuff
//Keypoint stuff
VWDictionary * _vwd;
KeypointDetector * _keypointDetector;
KeypointDescriptor * _keypointDescriptor;
bool _reactivatedWordsComparedToNewWords;
float _badSignRatio;;
bool _tfIdfLikelihoodUsed;
bool _parallelized;
int _wordsPerImageTarget; // <0=none, 0=inf
std::vector<float> _roiRatios; // size 4
// RGBD-SLAM stuff
int _icpType;
int _bowMinInliers;
float _bowInlierDistance;
int _bowIterations;
float _bowMaxDepth;
int _icpDecimation;
float _icpMaxDepth;
float _icpVoxelSize;
int _icpSamples;
float _icpMaxCorrespondenceDistance;
int _icpMaxIterations;
float _icpMaxFitness;
float _icp2MaxCorrespondenceDistance;
int _icp2MaxIterations;
float _icp2MaxFitness;
float _icp2CorrespondenceRatio;
};
} // namespace rtabmap
corelib/include/rtabmap/core/Odometry.h
+203
View File
@@ -0,0 +1,203 @@
/*
* Odometry.h
*
* Created on: 2013-08-23
* Author: Mathieu
*/
#ifndef ODOMETRY_H_
#define ODOMETRY_H_
#include <rtabmap/core/RtabmapExp.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsHandler.h>
#include <rtabmap/utilite/UEvent.h>
#include <rtabmap/utilite/UMutex.h>
#include <rtabmap/utilite/USemaphore.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Image.h>
#include <opencv2/opencv.hpp>
#include <pcl/common/eigen.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
class UTimer;
namespace rtabmap {
class RTABMAP_EXP Odometry
{
public:
virtual ~Odometry() {}
Transform process(Image & image);
virtual void reset();
bool isLargeEnoughTransform(const Transform & transform);
//getters
int getMaxFeatures() const {return _maxFeatures;}
int getMinInliers() const {return _minInliers;}
float getInlierDistance() const {return _inlierDistance;}
int getIterations() const {return _iterations;}
float getMaxDepth() const {return _maxDepth;}
float geLinearUpdate() const {return _linearUpdate;}
float getAngularUpdate() const {return _angularUpdate;}
private:
virtual Transform computeTransform(Image & image) = 0;
private:
int _maxFeatures;
int _minInliers;
float _inlierDistance;
int _iterations;
float _maxDepth;
float _linearUpdate;
float _angularUpdate;
int _resetCountdown;
Transform _pose;
int _resetCurrentCount;
protected:
Odometry(float inlierDistance = Parameters::defaultOdomInlierDistance(),
int maxWords = Parameters::defaultOdomMaxWords(),
int minInliers = Parameters::defaultOdomMinInliers(),
int iterations = Parameters::defaultOdomIterations(),
float maxDepth = Parameters::defaultOdomMaxDepth(),
float linearUpdate = Parameters::defaultOdomLinearUpdate(),
float angularUpdate = Parameters::defaultOdomAngularUpdate(),
int resetCountDown = Parameters::defaultOdomResetCountdown());
Odometry(const rtabmap::ParametersMap & parameters);
};
class RTABMAP_EXP OdometryBinary : public Odometry
{
public:
OdometryBinary(
float inlierDistance = Parameters::defaultOdomInlierDistance(),
int maxWords = Parameters::defaultOdomMaxWords(),
int minInliers = Parameters::defaultOdomMinInliers(),
int iterations = Parameters::defaultOdomIterations(),
float maxDepth = Parameters::defaultOdomMaxDepth(),
float linearUpdate = Parameters::defaultOdomLinearUpdate(),
float angularUpdate = Parameters::defaultOdomAngularUpdate(),
int resetCountdown = Parameters::defaultOdomResetCountdown(),
int briefBytes = Parameters::defaultOdomBinBriefBytes(),
int fastThreshold = Parameters::defaultOdomBinFastThreshold(),
bool fastNonmaxSuppression = Parameters::defaultOdomBinFastNonmaxSuppression(),
bool bruteForceMatching = Parameters::defaultOdomBinBruteForceMatching());
OdometryBinary(const rtabmap::ParametersMap & parameters);
virtual ~OdometryBinary() {}
virtual void reset();
private:
virtual Transform computeTransform(Image & image);
private:
int _briefBytes;
int _fastThreshold;
bool _fastNonmaxSuppression;
bool _bruteForceMatching;
std::vector<cv::KeyPoint> _lastKeypoints;
cv::Mat _lastDescriptors;
cv::Mat _lastDepth;
};
class Memory;
class RTABMAP_EXP OdometryBOW : public Odometry
{
public:
OdometryBOW(
int detectorType = Parameters::defaultKpDetectorStrategy(), // 0=SURF or 1=SIFT
float inlierDistance = Parameters::defaultOdomInlierDistance(),
int maxWords = Parameters::defaultOdomMaxWords(),
int minInliers = Parameters::defaultOdomMinInliers(),
int iterations = Parameters::defaultOdomIterations(),
float maxDepth = Parameters::defaultOdomMaxDepth(),
float linearUpdate = Parameters::defaultOdomLinearUpdate(),
float angularUpdate = Parameters::defaultOdomAngularUpdate(),
int resetCoutdown = Parameters::defaultOdomResetCountdown(),
float surfHessianThreshold = Parameters::defaultSURFHessianThreshold(),
float nndr = Parameters::defaultKpNndrRatio()); // nearest neighbor distance ratio
OdometryBOW(const rtabmap::ParametersMap & parameters);
virtual ~OdometryBOW();
virtual void reset();
private:
virtual Transform computeTransform(Image & image);
private:
Memory * _memory;
};
class RTABMAP_EXP OdometryICP : public Odometry
{
public:
OdometryICP(
int decimation = Parameters::defaultOdomICPDecimation(),
float voxelSize = Parameters::defaultOdomICPVoxelSize(),
float samples = Parameters::defaultOdomICPSamples(),
float maxCorrespondenceDistance = Parameters::defaultOdomICPCorrespondencesDistance(),
int maxIterations = Parameters::defaultOdomICPIterations(),
float maxFitness = Parameters::defaultOdomICPMaxFitness(),
float maxDepth = Parameters::defaultOdomMaxDepth(),
float linearUpdate = Parameters::defaultOdomLinearUpdate(),
float angularUpdate = Parameters::defaultOdomAngularUpdate(),
int resetCoutdown = Parameters::defaultOdomResetCountdown());
OdometryICP(const ParametersMap & parameters);
void reset();
private:
virtual Transform computeTransform(Image & image);
private:
int _decimation;
float _voxelSize;
float _samples;
float _maxCorrespondenceDistance;
int _maxIterations;
float _maxFitness;
pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloud;
};
// return true if odometry is correctly computed
Transform computeTransform(Image & image);
class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
public:
// take ownership of Odometry
OdometryThread(Odometry * odometry);
virtual ~OdometryThread();
protected:
virtual void handleEvent(UEvent * event);
private:
void mainLoopKill();
//============================================================
// MAIN LOOP
//============================================================
void mainLoop();
void addImage(const Image & image);
void getImage(Image & image);
private:
USemaphore _imageAdded;
UMutex _imageMutex;
Image _imageBuffer;
Odometry * _odometry;
bool _resetOdometry;
};
} /* namespace rtabmap */
#endif /* ODOMETRY_H_ */
corelib/include/rtabmap/core/OdometryEvent.h
+43
View File
@@ -0,0 +1,43 @@
/*
* OdometryEvent.h
*
* Created on: 2013-10-15
* Author: Mathieu
*/
#ifndef ODOMETRYEVENT_H_
#define ODOMETRYEVENT_H_
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/core/Image.h"
namespace rtabmap {
class OdometryEvent : public UEvent
{
public:
OdometryEvent(
const Image & data) :
_data(data) {}
virtual ~OdometryEvent() {}
virtual std::string getClassName() const {return "OdometryEvent";}
bool isValid() const {return !_data.pose().isNull();}
const Image & data() const {return _data;}
private:
Image _data;
};
class OdometryResetEvent : public UEvent
{
public:
OdometryResetEvent(){}
virtual ~OdometryResetEvent() {}
virtual std::string getClassName() const {return "OdometryResetEvent";}
};
}
#endif /* ODOMETRYEVENT_H_ */
corelib/include/rtabmap/core/Parameters.h
+143 -69
View File
@@ -29,7 +29,7 @@ namespace rtabmap
{
typedef std::map<std::string, std::string> ParametersMap; // Key, value
typedef std::pair<const std::string, std::string> ParametersPair;
typedef std::pair<std::string, std::string> ParametersPair;
/**
* Macro used to create parameter's key and default value.
@@ -49,14 +49,15 @@ typedef std::pair<const std::string, std::string> ParametersPair;
* DummyVideoImageWidth dummyVideoImageWidth;
* @endcode
*/
#define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE) \
#define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \
public: \
static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
static TYPE default##PREFIX##NAME() {return DEFAULT_VALUE;} \
private: \
class Dummy##PREFIX##NAME { \
public: \
Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE));} \
Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE)); \
descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
}; \
Dummy##PREFIX##NAME dummy##PREFIX##NAME;
// end define PARAM
@@ -80,14 +81,15 @@ typedef std::pair<const std::string, std::string> ParametersPair;
* DummyVideoFileName dummyVideoFileName;
* @endcode
*/
#define RTABMAP_PARAM_STR(PREFIX, NAME, DEFAULT_VALUE) \
#define RTABMAP_PARAM_STR(PREFIX, NAME, DEFAULT_VALUE, DESCRIPTION) \
public: \
static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
static std::string default##PREFIX##NAME() {return DEFAULT_VALUE;} \
private: \
class Dummy##PREFIX##NAME { \
public: \
Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE));} \
Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE)); \
descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
}; \
Dummy##PREFIX##NAME dummy##PREFIX##NAME;
// end define PARAM
@@ -119,85 +121,140 @@ typedef std::pair<const std::string, std::string> ParametersPair;
class RTABMAP_EXP Parameters
{
// Rtabmap parameters
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0); // None 0, Similarity 1, Epipolar 2
RTABMAP_PARAM(Rtabmap, PublishStats, bool, true); // Publishing statistics
RTABMAP_PARAM(Rtabmap, PublishImage, bool, true); // Publishing image
RTABMAP_PARAM(Rtabmap, PublishPdf, bool, true); // Publishing pdf
RTABMAP_PARAM(Rtabmap, PublishLikelihood, bool, true); // Publishing likelihood
RTABMAP_PARAM(Rtabmap, TimeThr, float, 700.0); // Maximum time allowed for the detector (ms) (0 means infinity)
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0); // Maximum signatures in the Working Memory (ms) (0 means infinity)
RTABMAP_PARAM(Rtabmap, ImageBufferSize, int, 0); // Data buffer size (0 min inf)
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory()); // Working directory
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2); // Maximum locations retrieved at the same time from LTM
RTABMAP_PARAM(Rtabmap, LikelihoodNullValuesIgnored, bool, true); // Ignore null values on likelihood normalization
RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true); // Statistic logs buffered in RAM instead of written to hard drive after each iteration.
RTABMAP_PARAM(Rtabmap, StatisticLogged, bool, true); // Logging enabled
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0, "None 0, Similarity 1, Epipolar 2.");
RTABMAP_PARAM(Rtabmap, PublishStats, bool, true, "Publishing statistics.");
RTABMAP_PARAM(Rtabmap, PublishImage, bool, true, "Publishing image.");
RTABMAP_PARAM(Rtabmap, PublishPdf, bool, true, "Publishing pdf.");
RTABMAP_PARAM(Rtabmap, PublishLikelihood, bool, true, "Publishing likelihood.");
RTABMAP_PARAM(Rtabmap, TimeThr, float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity).");
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0, "Maximum signatures in the Working Memory (ms) (0 means infinity).");
RTABMAP_PARAM(Rtabmap, DetectionRate, float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
RTABMAP_PARAM(Rtabmap, ImageBufferSize, int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory(), "Working directory.");
RTABMAP_PARAM_STR(Rtabmap, DatabasePath, Parameters::getDefaultDatabasePath(), "Database path.");
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
RTABMAP_PARAM(Rtabmap, StatisticLogged, bool, false, "Logging enabled.");
// Hypotheses selection
RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.15); // Loop closing threshold
RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0.9); // The loop closure hypothesis must be over LoopRatio x lastHypothesisValue
RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.11, "Loop closing threshold.");
RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0.9, "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
// Memory
RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.2); // Rehearsal mean for each sensor
RTABMAP_PARAM(Mem, RehearsalOnlyWithLast, bool, true); // Only compare to the last signature in STM, otherwise it compares to all signatures in STM
RTABMAP_PARAM(Mem, ImageKept, bool, true); // Keep images in db
RTABMAP_PARAM(Mem, RehearsedNodesKept, bool, true); // Keep rehearsed ndoes in db
RTABMAP_PARAM(Mem, STMSize, unsigned int, 30); // Short-term memory size
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true);
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2); // Ratio of locations after the last loop closure in WM that cannot be transferred
RTABMAP_PARAM(Mem, RehearsalOldDataKept, bool, true); // On merge, keep old data
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, true); // On merge, update to new id
RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 1.0, "Rehearsal similarity.");
RTABMAP_PARAM(Mem, ImageKept, bool, true, "Keep images in db.");
RTABMAP_PARAM(Mem, RehearsedNodesKept, bool, true, "Keep rehearsed ndoes in db.");
RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size.");
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, othwersize it is Localization mode.");
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2, "Ratio of locations after the last loop closure in WM that cannot be transferred.");
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location Ids, False=use input image ids.")
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, PublishKeypoints, bool, true); // Publishing keypoints
RTABMAP_PARAM(Kp, NNStrategy, int, 1); // Naive 0, kdForest 1
RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true);
RTABMAP_PARAM(Kp, WordsPerImage, int, 400);
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.2); //Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)
RTABMAP_PARAM(Kp, MinDistUsed, bool, false); // The nearest neighbor must have a distance < minDist
RTABMAP_PARAM(Kp, MinDist, float, 0.05); // Matching a descriptor with a word (euclidean distance ^ 2)
RTABMAP_PARAM(Kp, NndrUsed, bool, true); // If NNDR ratio is used
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8); // NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)
RTABMAP_PARAM(Kp, MaxLeafs, int, 64); // Maximum number of leafs checked (when using kd-trees)
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0); // Surf detector 0, SIFT detector 1, undef 2
RTABMAP_PARAM(Kp, DescriptorStrategy, int, 0); // kDescriptorSurf=0, kDescriptorSift, kDescriptorUndef
RTABMAP_PARAM(Kp, ReactivatedWordsComparedToNewWords, bool, true); //Reactivated words are compared to the last words added in the dictionary (which are not indexed)
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false); // Use of the td-idf strategy to compute the likelihood
RTABMAP_PARAM(Kp, Parallelized, bool, true); // If the dictionary update and signature creation were parallelized
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0"); // Region of interest ratios [left, right, top, bottom]
RTABMAP_PARAM_STR(Kp, DictionaryPath, ""); // Path of the pre-computed dictionary
RTABMAP_PARAM(Kp, PublishKeypoints, bool, true, "Publishing keypoints.");
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "Naive 0, kdForest 1.");
RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, "");
RTABMAP_PARAM(Kp, WordsPerImage, int, 400, "");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.2, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
RTABMAP_PARAM(Kp, MinDistUsed, bool, false, "The nearest neighbor must have a distance < minDist.");
RTABMAP_PARAM(Kp, MinDist, float, 0.05, "Matching a descriptor with a word (euclidean distance ^ 2)");
RTABMAP_PARAM(Kp, NndrUsed, bool, true, "If NNDR ratio is used.");
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
RTABMAP_PARAM(Kp, MaxLeafs, int, 64, "Maximum number of leafs checked (when using kd-trees).");
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "Surf detector 0, SIFT detector 1, undef 2.");
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false, "Use of the td-idf strategy to compute the likelihood.");
RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized.");
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM_STR(Kp, DictionaryPath, "", "Path of the pre-computed dictionary");
//Database
RTABMAP_PARAM(Db, ImagesCompressed, bool, true); // Images are compressed when saving to database
RTABMAP_PARAM(DbSqlite3, InMemory, bool, false); // Using database in the memory instead of a file on the hard disk
RTABMAP_PARAM(DbSqlite3, CacheSize, unsigned int, 10000); // Sqlite cache size (default is 2000)
RTABMAP_PARAM(DbSqlite3, JournalMode, int, 3); // 0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : "PRAGMA journal_mode")
RTABMAP_PARAM(DbSqlite3, Synchronous, int, 0); // 0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : "PRAGMA synchronous")
RTABMAP_PARAM(DbSqlite3, TempStore, int, 2); // 0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : "PRAGMA temp_store")
RTABMAP_PARAM(DbSqlite3, InMemory, bool, true, "Using database in the memory instead of a file on the hard disk.");
RTABMAP_PARAM(DbSqlite3, CacheSize, unsigned int, 10000, "Sqlite cache size (default is 2000).");
RTABMAP_PARAM(DbSqlite3, JournalMode, int, 3, "0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : \"PRAGMA journal_mode\")");
RTABMAP_PARAM(DbSqlite3, Synchronous, int, 0, "0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : \"PRAGMA synchronous\")");
RTABMAP_PARAM(DbSqlite3, TempStore, int, 2, "0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");
// Keypoints descriptors/detectors
RTABMAP_PARAM(SURF, Extended, bool, false); // true=128, false=64
RTABMAP_PARAM(SURF, HessianThreshold, float, 150.0);
RTABMAP_PARAM(SURF, Octaves, int, 4);
RTABMAP_PARAM(SURF, OctaveLayers, int, 2);
RTABMAP_PARAM(SURF, Upright, bool, false); // U-SURF
RTABMAP_PARAM(SURF, GpuVersion, bool, false);
RTABMAP_PARAM(SURF, Extended, bool, false, "true=128, false=64.");
RTABMAP_PARAM(SURF, HessianThreshold, float, 150.0, "");
RTABMAP_PARAM(SURF, Octaves, int, 4, "");
RTABMAP_PARAM(SURF, OctaveLayers, int, 2, "");
RTABMAP_PARAM(SURF, Upright, bool, false, "U-SURF");
RTABMAP_PARAM(SURF, GpuVersion, bool, false, "");
RTABMAP_PARAM(SIFT, NFeatures, int, 0);
RTABMAP_PARAM(SIFT, NOctaveLayers, int, 3);
RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04);
RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0);
RTABMAP_PARAM(SIFT, Sigma, double, 1.6);
RTABMAP_PARAM(SIFT, NFeatures, int, 0, "");
RTABMAP_PARAM(SIFT, NOctaveLayers, int, 3, "");
RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04, "");
RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0, "");
RTABMAP_PARAM(SIFT, Sigma, double, 1.6, "");
// BayesFilter
RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9); // Virtual place prior
RTABMAP_PARAM_STR(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"); // Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}
RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, true); // Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).
RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9, "Virtual place prior");
RTABMAP_PARAM_STR(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", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, true, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).");
// Verify hypotheses
RTABMAP_PARAM(VhEp, MatchCountMin, int, 8); // Minimum of matching visual words pairs to accept the loop hypothesis
RTABMAP_PARAM(VhEp, RansacParam1, float, 3.0); // Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier
RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99); // Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC
RTABMAP_PARAM(VhEp, MatchCountMin, int, 8, "Minimum of matching visual words pairs to accept the loop hypothesis.");
RTABMAP_PARAM(VhEp, RansacParam1, float, 3.0, "Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
// RGB-D SLAM
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
RTABMAP_PARAM(RGBD, ScanMatchingSize, int, 0, "Laser scan matching history for odometry correction (laser scans are required). Set to 0 to disable odometry correction.");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
// Local loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, true, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius, float, 15, "Maximum radius for space detection.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors, int, 20, "Maximum nearest neighbor.");
// Odometry
RTABMAP_PARAM(Odom, Type, int, 0, "0=BOW 1=Binary.");
RTABMAP_PARAM(Odom, LinearUpdate, float, 0.0, "Min linear displacement to update odometry.");
RTABMAP_PARAM(Odom, AngularUpdate, float, 0.0, "Min angular displacement to update odometry.");
RTABMAP_PARAM(Odom, MaxWords, int, 0, "0 no limits.");
RTABMAP_PARAM(Odom, InlierDistance, float, 0.005, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(Odom, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(Odom, MaxDepth, float, 5.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).")
RTABMAP_PARAM(OdomBin, BriefBytes, int, 32, "");
RTABMAP_PARAM(OdomBin, FastThreshold, int, 30, "");
RTABMAP_PARAM(OdomBin, FastNonmaxSuppression, bool, true, "");
RTABMAP_PARAM(OdomBin, BruteForceMatching, bool, true, "If false, FLANN LSH is used.");
RTABMAP_PARAM(OdomICP, Decimation, int, 4, "");
RTABMAP_PARAM(OdomICP, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(OdomICP, Samples, int, 0, "not used if voxelSize is set.");
RTABMAP_PARAM(OdomICP, CorrespondencesDistance, float, 0.05, "");
RTABMAP_PARAM(OdomICP, Iterations, int, 30, "");
RTABMAP_PARAM(OdomICP, MaxFitness, float, 0.01, "");
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Enabled, bool, false, "Enable ICP");
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=ICP 3D, 1=ICP 2D");
RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.01, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(LccBow, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(LccBow, MaxDepth, float, 5.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(LccIcp3, Decimation, int, 8, "Depth image decimation.");
RTABMAP_PARAM(LccIcp3, MaxDepth, float, 4.0, "Max cloud depth.");
RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set.");
RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "ICP 3D: Max iterations.");
RTABMAP_PARAM(LccIcp3, MaxFitness, float, 1.0, "ICP 3D: Maximum fitness to accept the computed transform.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, "ICP 2D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "ICP 2D: Max iterations.");
RTABMAP_PARAM(LccIcp2, MaxFitness, float, 1.0, "ICP 2D: Maximum fitness to accept the computed transform.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "ICP 2D: Ratio of matching correspondences to accept the transform.");
public:
virtual ~Parameters();
@@ -211,12 +268,29 @@ public:
return parameters_;
}
/**
* Get parameter description
*
*/
static std::string getDescription(const std::string & paramKey);
static void parse(const ParametersMap & parameters, const std::string & key, bool & value);
static void parse(const ParametersMap & parameters, const std::string & key, int & value);
static void parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
static void parse(const ParametersMap & parameters, const std::string & key, float & value);
static void parse(const ParametersMap & parameters, const std::string & key, double & value);
static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
static std::string getDefaultDatabaseName();
private:
Parameters();
static std::string getDefaultWorkingDirectory();
static std::string getDefaultDatabasePath();
private:
static ParametersMap parameters_;
static ParametersMap descriptions_;
static Parameters instance_;
};
corelib/include/rtabmap/core/Rtabmap.h
+33 -6
View File
@@ -37,12 +37,12 @@ namespace rtabmap
class EpipolarGeometry;
class Memory;
class BayesFilter;
class Signature;
class RTABMAP_EXP Rtabmap
{
public:
enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
static const char * kDefaultDatabaseName;
public:
static std::string getVersion();
@@ -53,8 +53,8 @@ public:
Rtabmap();
virtual ~Rtabmap();
void process(const cv::Mat & image, int id=0, std::multimap<int, cv::KeyPoint> * words = 0); // for convenience, an id is automatically generated if id=0
void process(const Image & image, std::multimap<int, cv::KeyPoint> * words = 0); // for convenience
bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
bool process(const Image & image); // for convenience
void init(const ParametersMap & param, bool deleteMemory = true);
void init(const std::string & configFile = "", bool deleteMemory = true);
@@ -62,6 +62,7 @@ public:
void close();
const std::string & getWorkingDir() const {return _wDir;}
std::string getDatabasePath() const;
int getLoopClosureId() const;
int getRetrievedId() const;
int getLastLocationId() const;
@@ -76,25 +77,39 @@ public:
std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;// <Id,Margin> including nodeId
bool isInSTM(int locationId) const;
bool isIDsGenerated() const;
const Statistics & getStatistics() const;
//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
Transform getPose(int locationId) const;
Transform getMapCorrection() const {return _mapCorrection;}
void setTimeThreshold(float maxTimeAllowed); // in ms
void triggerNewMap();
void generateGraph(const std::string & path, int id=0, int margin=5);
void resetMemory(bool dbOverwritten = false);
void dumpPrediction() const;
void dumpData() const;
void parseParameters(const ParametersMap & parameters);
void setWorkingDirectory(std::string path);
void deleteLastLocation();
void setDatabasePath(const std::string & path);
void deleteLocation(int locationId); // Only nodes in STM can be deleted
void rejectLastLoopClosure();
void rejectLoopClosure(int oldId, int newId);
void get3DMap(std::map<int, std::vector<unsigned char> > & images,
std::map<int, std::vector<unsigned char> > & depths,
std::map<int, std::vector<unsigned char> > & depths2d,
std::map<int, float> & depthConstants,
std::map<int, Transform> & localTransforms,
std::map<int, Transform> & poses,
Transform & mapCorrection) const;
std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int & nearestId) const;
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood) const;
private:
void optimizeCurrentMap(int id, bool lookInDatabase, std::map<int, Transform> & optimizedPoses, Transform & mapCorrection) const;
void setupLogFiles(bool overwrite = false);
void flushStatisticLogs();
@@ -110,9 +125,18 @@ private:
float _loopThr;
float _loopRatio;
unsigned int _maxRetrieved;
bool _likelihoodNullValuesIgnored;
bool _statisticLogsBufferedInRAM;
bool _statisticLogged;
bool _rgbdSlamMode;
float _rgbdLinearUpdate;
float _rgbdAngularUpdate;
int _scanMatchingSize;
bool _localLoopClosureDetectionTime;
bool _localLoopClosureDetectionSpace;
float _localDetectRadius;
float _localDetectMaxNeighbors;
bool _icpEnabled;
std::string _databasePath;
int _lcHypothesisId;
float _lcHypothesisValue;
@@ -134,6 +158,9 @@ private:
Statistics statistics_;
std::string _wDir;
std::map<int, Transform> _optimizedPoses;
Transform _mapCorrection;
};
#endif /* RTABMAP_H_ */
corelib/include/rtabmap/core/RtabmapEvent.h
+71 -5
View File
@@ -55,21 +55,42 @@ public:
kCmdGenerateGraph,
kCmdGenerateLocalGraph,
kCmdDeleteMemory,
kCmdCleanSensorsBuffer};
kCmdCleanDataBuffer,
kCmdPublish3DMap,
kCmdTriggerNewMap,
kCmdPause};
public:
RtabmapEventCmd(Cmd cmd) :
UEvent(0),
_cmd(cmd) {}
_cmd(cmd),
_strValue(""),
_intValue(0){}
RtabmapEventCmd(Cmd cmd, int value) :
UEvent(0),
_cmd(cmd),
_strValue(""),
_intValue(value){}
RtabmapEventCmd(Cmd cmd, const std::string & value) :
UEvent(0),
_cmd(cmd),
_strValue(value),
_intValue(0){}
virtual ~RtabmapEventCmd() {}
Cmd getCmd() const {return _cmd;}
void setStr(const std::string & str) {_str = str;}
const std::string & getStr() const {return _str;}
void setStr(const std::string & str) {_strValue = str;}
const std::string & getStr() const {return _strValue;}
void setInt(int v) {_intValue = v;}
int getInt() const {return _intValue;}
virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
private:
Cmd _cmd;
std::string _str;
std::string _strValue;
int _intValue;
};
class RtabmapEventInit : public UEvent
@@ -107,6 +128,51 @@ private:
std::string _info; // "Loading signatures", "Loading words" ...
};
class RtabmapEvent3DMap : public UEvent
{
public:
RtabmapEvent3DMap(int codeError = 0):
UEvent(codeError){}
RtabmapEvent3DMap(
const std::map<int, std::vector<unsigned char> > & images,
const std::map<int, std::vector<unsigned char> > & depths,
const std::map<int, std::vector<unsigned char> > & depths2d,
const std::map<int, float> & depthConstants,
const std::map<int, Transform> & localTransforms,
const std::map<int, Transform> & poses,
const Transform & mapCorrection) :
UEvent(0),
_images(images),
_depths(depths),
_depths2d(depths2d),
_depthConstants(depthConstants),
_localTransforms(localTransforms),
_poses(poses),
_mapCorrection(mapCorrection)
{}
virtual ~RtabmapEvent3DMap() {}
const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
const std::map<int, std::vector<unsigned char> > & getDepths2d() const {return _depths2d;}
const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
const std::map<int, Transform> & getPoses() const {return _poses;}
const Transform & getMapCorrection() const {return _mapCorrection;}
virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
private:
std::map<int, std::vector<unsigned char> > _images;
std::map<int, std::vector<unsigned char> > _depths;
std::map<int, std::vector<unsigned char> > _depths2d;
std::map<int, float> _depthConstants;
std::map<int, Transform> _localTransforms;
std::map<int, Transform> _poses;
Transform _mapCorrection;
};
} // namespace rtabmap
#endif /* RTABMAPEVENT_H_ */
corelib/include/rtabmap/core/RtabmapExp.h
+1 -1
View File
@@ -21,7 +21,7 @@
#define RTABMAPEXP_H
#if defined(_WIN32)
#if defined(rtabmap_corelib_EXPORTS) || defined(rtabmap_guilib_EXPORTS)
#if defined(rtabmap_core_EXPORTS)
#define RTABMAP_EXP __declspec( dllexport )
#else
#define RTABMAP_EXP __declspec( dllimport )
corelib/include/rtabmap/core/RtabmapThread.h
+10 -3
View File
@@ -33,6 +33,8 @@
#include <stack>
class UTimer;
namespace rtabmap {
class Rtabmap;
@@ -52,15 +54,16 @@ public:
kStateGeneratingGraph,
kStateGeneratingLocalGraph,
kStateDeletingMemory,
kStateCleanSensorsBuffer
kStateCleanDataBuffer,
kStatePublishingMap,
kStateTriggeringMap
};
public:
RtabmapThread();
virtual ~RtabmapThread();
void setWorkingDirectory(const std::string & path);
void clearBufferedSensors();
void clearBufferedData();
protected:
virtual void handleEvent(UEvent * anEvent);
@@ -73,6 +76,7 @@ private:
void getImage(Image & image);
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void setDataBufferSize(int size);
void publishMap() const;
private:
UMutex _stateMutex;
@@ -83,8 +87,11 @@ private:
UMutex _imageMutex;
USemaphore _imageAdded;
int _imageBufferMaxSize;
float _rate;
UTimer * _frameRateTimer;
Rtabmap * _rtabmap;
bool _paused;
};
} /* namespace rtabmap */
corelib/src/Signature.h → corelib/include/rtabmap/core/Signature.h
+56 -18
View File
@@ -21,6 +21,8 @@
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
@@ -29,6 +31,8 @@
#include <vector>
#include <set>
#include <rtabmap/core/Transform.h>
namespace rtabmap
{
@@ -36,10 +40,18 @@ class Memory;
class RTABMAP_EXP Signature
{
public:
Signature(int id,
int mapId,
const std::multimap<int, cv::KeyPoint> & words,
const cv::Mat & image = cv::Mat());
const std::multimap<int, pcl::PointXYZ> & words3,
const Transform & pose = Transform(),
const std::vector<unsigned char> & depth2D = std::vector<unsigned char>(),
const std::vector<unsigned char> & image = std::vector<unsigned char>(),
const std::vector<unsigned char> & depth = std::vector<unsigned char>(),
float depthConstant = 0.0f,
const Transform & localTransform =Transform::getIdentity());
virtual ~Signature();
/**
@@ -49,28 +61,33 @@ public:
bool isBadSignature() const;
int id() const {return _id;}
int mapId() const {return _mapId;}
void addNeighbors(const std::set<int> & neighbors);
void addNeighbor(int neighbor);
void addNeighbors(const std::map<int, Transform> & neighbors);
void addNeighbor(int neighbor, const Transform & transform = Transform());
void removeNeighbor(int neighborId);
void removeNeighbors();
bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_neighborsModified=true;}
void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_neighborsModified=true;}
void setLoopClosureIds(const std::map<int, Transform> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
void addLoopClosureId(int loopClosureId, const Transform & transform = Transform());
void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
void changeLoopClosureId(int idFrom, int idTo);
void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
void setChildLoopClosureIds(const std::map<int, Transform> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
void addChildLoopClosureId(int childLoopClosureId, const Transform & transform = Transform());
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
void changeNeighborIds(int idFrom, int idTo);
const std::set<int> & getNeighbors() const {return _neighbors;}
const std::map<int, Transform> & getNeighbors() const {return _neighbors;}
int getWeight() const {return _weight;}
const std::set<int> & getLoopClosureIds() const {return _loopClosureIds;}
const std::set<int> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
const std::map<int, Transform> & getLoopClosureIds() const {return _loopClosureIds;}
const std::map<int, Transform> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
bool isSaved() const {return _saved;}
bool isModified() const {return _modified || _neighborsModified;}
bool isNeighborsModified() const {return _neighborsModified;}
@@ -84,15 +101,29 @@ public:
void setEnabled(bool enabled) {_enabled = enabled;}
const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
void setImage(const cv::Mat & image) {_image = image;}
const cv::Mat & getImage() const {return _image;}
void setImage(const std::vector<unsigned char> & image) {_image = image;}
const std::vector<unsigned char> & getImage() const {return _image;}
//metric stuff
void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
void setDepth(const std::vector<unsigned char> & depth, float depthConstant);
void setDepth2D(const std::vector<unsigned char> & depth2D) {_depth2D = depth2D;}
void setLocalTransform(const Transform & t) {_localTransform = t;}
void setPose(const Transform & pose) {_pose = pose;}
const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
const std::vector<unsigned char> & getDepth() const {return _depth;}
const std::vector<unsigned char> & getDepth2D() const {return _depth2D;}
float getDepthConstant() const {return _depthConstant;}
const Transform & getPose() const {return _pose;}
const Transform & getLocalTransform() const {return _localTransform;}
private:
int _id;
std::set<int> _neighbors; // id
int _mapId;
std::map<int, Transform> _neighbors; // id, transform
int _weight;
std::set<int> _loopClosureIds;
std::set<int> _childLoopClosureIds;
std::map<int, Transform> _loopClosureIds; // id, transform
std::map<int, Transform> _childLoopClosureIds; // id, transform
bool _saved; // If it's saved to bd
bool _modified;
bool _neighborsModified; // Optimization when updating signatures in database
@@ -103,7 +134,14 @@ private:
std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
std::map<int, int> _wordsChanged; // <oldId, newId>
bool _enabled;
cv::Mat _image;
std::vector<unsigned char> _image; //compressed image CV_8UC1 or CV_8UC3
std::vector<unsigned char> _depth; // compressed image CV_16UC1
std::vector<unsigned char> _depth2D; // compressed data CV_32FC2
float _depthConstant;
Transform _pose;
Transform _localTransform; // camera_link -> base_link
std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
};
} // namespace rtabmap
corelib/include/rtabmap/core/Statistics.h
+73 -9
View File
@@ -27,6 +27,7 @@
#include <opencv2/imgproc/imgproc.hpp>
#include <list>
#include <vector>
#include <rtabmap/core/Transform.h>
namespace rtabmap {
@@ -48,17 +49,30 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Vp_hypothesis,);
RTABMAP_STATS(Loop, ReactivateId,);
RTABMAP_STATS(Loop, Hypothesis_ratio,);
RTABMAP_STATS(Loop, Hypothesis_reactivated,);
RTABMAP_STATS(LocalLoop, Scan_matching_success,);
RTABMAP_STATS(LocalLoop, Time_closures,);
RTABMAP_STATS(LocalLoop, Space_closure_id,);
RTABMAP_STATS(LocalLoop, Space_neighbors,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);
RTABMAP_STATS(Memory, Signatures_removed,);
RTABMAP_STATS(Memory, Signatures_retrieved,);
RTABMAP_STATS(Memory, Images_buffered,);
RTABMAP_STATS(Memory, Rehearsal_sim,);
RTABMAP_STATS(Memory, Rehearsal_merged,);
RTABMAP_STATS(Memory, Last_loop_closure,);
RTABMAP_STATS(Timing, Memory_update, ms);
RTABMAP_STATS(Timing, Scan_matching, ms);
RTABMAP_STATS(Timing, Local_detection_TIME, ms);
RTABMAP_STATS(Timing, Local_detection_SPACE, ms);
RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
RTABMAP_STATS(Timing, Reactivation, ms);
RTABMAP_STATS(Timing, Add_loop_closure_link, ms);
RTABMAP_STATS(Timing, Map_optimization, ms);
RTABMAP_STATS(Timing, Likelihood_computation, ms);
RTABMAP_STATS(Timing, Posterior_computation, ms);
RTABMAP_STATS(Timing, Hypotheses_creation, ms);
@@ -70,7 +84,9 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Timing, Joining_trash, ms);
RTABMAP_STATS(Timing, Emptying_trash, ms);
RTABMAP_STATS(, Hypothesis_reactivated,);
RTABMAP_STATS(TimingMem, Pre_update, ms);
RTABMAP_STATS(TimingMem, Signature_creation, ms);
RTABMAP_STATS(TimingMem, Rehearsal, ms);
RTABMAP_STATS(Keypoint, Dictionary_size, words);
RTABMAP_STATS(Keypoint, Response_threshold,);
@@ -88,10 +104,25 @@ public:
// setters
void setExtended(bool extended) {_extended = extended;}
void setRefImageId(int refImageId) {_refImageId = refImageId;}
void setRefImageMapId(int refImageMapId) {_refImageMapId = refImageMapId;}
void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
void setLoopClosureMapId(int loopClosureMapId) {_loopClosureMapId = loopClosureMapId;}
void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
void setRefImage(const cv::Mat & image);
void setLoopImage(const cv::Mat & image);
void setLocalLoopClosureMapId(int localLoopClosureMapId) {_localLoopClosureMapId = localLoopClosureMapId;}
void setRefImage(const std::vector<unsigned char> & image) {_refImage = image;}
void setLoopImage(const std::vector<unsigned char> & image) {_loopImage = image;}
void setRefDepth(const std::vector<unsigned char> & depth) {_refDepth = depth;}
void setRefDepth2D(const std::vector<unsigned char> & depth2d) {_refDepth2d = depth2d;}
void setLoopDepth(const std::vector<unsigned char> & depth) {_loopDepth = depth;}
void setLoopDepth2D(const std::vector<unsigned char> & depth2d) {_loopDepth2d = depth2d;}
void setRefDepthConstant(float depthConstant) {_refDepthConstant = depthConstant;}
void setLoopDepthConstant(float depthConstant) {_loopDepthConstant = depthConstant;}
void setRefLocalTransform(const Transform & localTransform) {_refLocalTransform = localTransform;}
void setLoopLocalTransform(const Transform & localTransform) {_loopLocalTransform = localTransform;}
void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
void setCurrentPose(const Transform & pose) {_currentPose = pose;}
void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
void setWeights(const std::map<int, int> & weights) {_weights = weights;}
void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
@@ -102,10 +133,25 @@ public:
// getters
bool extended() const {return _extended;}
int refImageId() const {return _refImageId;}
int refImageMapId() const {return _refImageMapId;}
int loopClosureId() const {return _loopClosureId;}
int loopClosureMapId() const {return _loopClosureMapId;}
int localLoopClosureId() const {return _localLoopClosureId;}
const cv::Mat & refImage() const {return _refImage;}
const cv::Mat & loopImage() const {return _loopImage;}
int localLoopClosureMapId() const {return _localLoopClosureMapId;}
const std::vector<unsigned char> & refImage() const {return _refImage;}
const std::vector<unsigned char> & loopImage() const {return _loopImage;}
const std::vector<unsigned char> & refDepth() const {return _refDepth;}
const std::vector<unsigned char> & loopDepth() const {return _loopDepth;}
const std::vector<unsigned char> & refDepth2D() const {return _refDepth2d;}
const std::vector<unsigned char> & loopDepth2D() const {return _loopDepth2d;}
float refDepthConstant() const {return _refDepthConstant;}
float loopDepthConstant() const {return _loopDepthConstant;}
const Transform & refLocalTransform() const {return _refLocalTransform;}
const Transform & loopLocalTransform() const {return _loopLocalTransform;}
const std::map<int, Transform> & poses() const {return _poses;}
const Transform & currentPose() const {return _currentPose;}
const Transform & mapCorrection() const {return _mapCorrection;}
const Transform & loopClosureTransform() const {return _loopClosureTransform;}
const std::map<int, int> & weights() const {return _weights;}
const std::map<int, float> & posterior() const {return _posterior;}
const std::map<int, float> & likelihood() const {return _likelihood;}
@@ -116,15 +162,33 @@ public:
const std::map<std::string, float> & data() const {return _data;}
private:
int _extended; // 0 -> only loop closure and last signature ID fields are filled
bool _extended; // 0 -> only loop closure and last signature ID fields are filled
int _refImageId;
int _refImageMapId;
int _loopClosureId;
int _localLoopClosureId; // Note: used by VSLAM
int _loopClosureMapId;
int _localLoopClosureId;
int _localLoopClosureMapId;
// extended data start here...
cv::Mat _refImage;
cv::Mat _loopImage;
std::vector<unsigned char> _refImage;
std::vector<unsigned char> _loopImage;
// Metric data
std::vector<unsigned char> _refDepth;
std::vector<unsigned char> _refDepth2d;
std::vector<unsigned char> _loopDepth;
std::vector<unsigned char> _loopDepth2d;
float _refDepthConstant;
float _loopDepthConstant;
Transform _refLocalTransform;
Transform _loopLocalTransform;
std::map<int, Transform> _poses;
Transform _currentPose;
Transform _mapCorrection;
Transform _loopClosureTransform;
std::map<int, int> _weights;
std::map<int, float> _posterior;
corelib/include/rtabmap/core/Transform.h
+72
View File
@@ -0,0 +1,72 @@
/*
* Transform.h
*
* Created on: 2013-08-30
* Author: Mathieu
*/
#ifndef TRANSFORM_H_
#define TRANSFORM_H_
#include <rtabmap/core/RtabmapExp.h>
#include <vector>
#include <string>
namespace rtabmap {
class RTABMAP_EXP Transform
{
public:
// Zero by default
Transform();
// rotation matrix r## and origin o##
Transform(float r11, float r12, float r13, float o14,
float r21, float r22, float r23, float o24,
float r31, float r32, float r33, float o34);
// x,y,z, roll,pitch,yaw
Transform(float x, float y, float z, float roll, float pitch, float yaw);
float & operator[](int index) {return data_[index];}
const float & operator[](int index) const {return data_[index];}
bool isNull() const;
bool isIdentity() const;
void setNull();
void setIdentity();
const float * data() const {return data_.data();}
float * data() {return data_.data();}
int size() const {return data_.size();}
float & x() {return data_[3];}
float & y() {return data_[7];}
float & z() {return data_[11];}
const float & x() const {return data_[3];}
const float & y() const {return data_[7];}
const float & z() const {return data_[11];}
Transform inverse() const;
Transform rotation() const;
Transform translation() const;
void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
std::string prettyPrint() const;
Transform operator*(const Transform & t) const;
Transform & operator*=(const Transform & t);
bool operator==(const Transform & t) const;
bool operator!=(const Transform & t) const;
static Transform getIdentity();
private:
std::vector<float> data_;
};
RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);
}
#endif /* TRANSFORM_H_ */
corelib/include/rtabmap/core/VWDictionary.h
+4 -5
View File
@@ -25,6 +25,7 @@
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include <set>
#include "rtabmap/core/Parameters.h"
namespace rtabmap
@@ -64,16 +65,13 @@ public:
void setLastWordId(int id) {_lastWordId = id;}
void getCommonWords(unsigned int nbCommonWords, int totalSign, std::list<int> & commonWords) const;
const std::map<int, VisualWord *> & getVisualWords() const {return _visualWords;}
void setMinDist(float d);
float getMinDist() const {return _minDist;}
bool isMinDistUsed() const {return _minDistUsed;}
void setMinDistUsed(bool used) {_minDistUsed = used;}
void setNndrUsed(bool used) {_nndrUsed = used;}
bool isNndrUsed() const {return _nndrUsed;}
void setNndrRatio(float ratio);
float getNndrRatio() {return _nndrRatio;}
unsigned int getNotIndexedWordsCount() const {return _visualWords.size() - _mapIndexId.size();}
unsigned int getLastNewWordsAddedCount() const {return _lastNewWordsAddedCount;}
unsigned int getNotIndexedWordsCount() const {return _notIndexedWords.size();}
int getLastIndexedWordId() const;
int getTotalActiveReferences() const {return _totalActiveReferences;}
void setNNStrategy(NNStrategy strategy, const ParametersMap & parameters = ParametersMap());
@@ -93,7 +91,6 @@ protected:
protected:
std::map<int, VisualWord *> _visualWords; //<id,VisualWord*>
unsigned int _lastNewWordsAddedCount;
int _totalActiveReferences; // keep track of all references for updating the common signature
private:
@@ -110,6 +107,8 @@ private:
cv::Mat _dataTree;
std::map<int ,int> _mapIndexId;
std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
std::set<int> _notIndexedWords; // Words that are not indexed in the dictionary
std::set<int> _removedIndexedWords; // Words not anymore in the dictionary but still indexed in the dictionary
};
} // namespace rtabmap
corelib/include/rtabmap/core/util3d.h
+362
View File
@@ -0,0 +1,362 @@
/*
* Util3D.h
* Author: mathieu
*/
#ifndef UTIL3D_H_
#define UTIL3D_H_
#include "rtabmap/core/RtabmapExp.h"
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include <string>
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/UThread.h>
#include <pcl/common/eigen.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/PolygonMesh.h>
namespace rtabmap
{
class Signature;
namespace util3d
{
/**
* Compress image or data
*
* Example compression:
* cv::Mat image;// an image
* CompressionThread ct(image);
* ct.start();
* ct.join();
* std::vector<unsigned char> bytes = ct.getCompressedData();
*
* Example uncompression
* std::vector<unsigned char> bytes;// a compressed image
* CompressionThread ct(bytes);
* ct.start();
* ct.join();
* cv::Mat image = ct.getUncompressedData();
*/
class RTABMAP_EXP CompressionThread : public UThread
{
public:
// format : ".png" ".jpg" "" (empty is general)
CompressionThread(const cv::Mat & mat, const std::string & format = "");
CompressionThread(const std::vector<unsigned char> & bytes, bool isImage);
const std::vector<unsigned char> & getCompressedData() const {return compressedData_;}
cv::Mat & getUncompressedData() {return uncompressedData_;}
protected:
virtual void mainLoop();
private:
std::vector<unsigned char> compressedData_;
cv::Mat uncompressedData_;
std::string format_;
bool image_;
bool compressMode_;
};
cv::Mat RTABMAP_EXP rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder = true);
cv::Mat RTABMAP_EXP depthFromCloud(
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
float & fx,
float & fy,
bool depth16U = true);
void RTABMAP_EXP rgbdFromCloud(
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
cv::Mat & rgb,
cv::Mat & depth,
float & fx,
float & fy,
bool bgrOrder = true,
bool depth16U = true);
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3(
const std::multimap<int, cv::KeyPoint> & words,
const cv::Mat & depth,
float depthConstant,
const Transform & transform);
std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints);
pcl::PointXYZ RTABMAP_EXP getDepth(const cv::Mat & depthImage,
int x, int y,
float cx, float cy,
float fx, float fy);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int samples);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP sampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int samples);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const std::string & axis,
float min,
float max);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::string & axis,
float min,
float max);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
float depthConstant,
int decimation);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,
float fx, float fy,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
float depthConstant,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
float cx, float cy,
float fx, float fy,
int decimation = 1);
cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);
std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
// remove depth by z axis
void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & cloud1,
pcl::PointCloud<pcl::PointXYZ> & cloud2);
void RTABMAP_EXP extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & cloud1,
pcl::PointCloud<pcl::PointXYZ> & cloud2);
void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
const cv::Mat & depthImage1,
const cv::Mat & depthImage2,
float cx, float cy,
float fx, float fy,
float maxDepth,
pcl::PointCloud<pcl::PointXYZ> & cloud1,
pcl::PointCloud<pcl::PointXYZ> & cloud2);
void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis);
void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis);
int RTABMAP_EXP countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
const std::multimap<int, pcl::PointXYZ> & wordsB);
void RTABMAP_EXP filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
char depthAxis,
bool removeDuplicates);
Transform RTABMAP_EXP transformFromXYZCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
double inlierThreshold = 0.02,
int iterations = 100,
int * inliers = 0);
Transform RTABMAP_EXP icp(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore);
Transform RTABMAP_EXP icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore);
Transform RTABMAP_EXP icp2D(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
double & fitnessScore);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
float maxDistance);
void RTABMAP_EXP findCorrespondences(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
void RTABMAP_EXP findCorrespondences(
const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform = Transform::getIdentity());
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
const cv::Mat & depth,
float depthConstant,
int decimation,
double maxDepth,
float voxel,
int samples,
const Transform & transform = Transform::getIdentity());
inline Eigen::Matrix4f transformToEigen4f(const Transform & transform)
{
Eigen::Matrix4f m;
m << transform[0], transform[1], transform[2], transform[3],
transform[4], transform[5], transform[6], transform[7],
transform[8], transform[9], transform[10], transform[11],
0,0,0,1;
return m;
}
inline Eigen::Matrix4d transformToEigen4d(const Transform & transform)
{
Eigen::Matrix4d m;
m << transform[0], transform[1], transform[2], transform[3],
transform[4], transform[5], transform[6], transform[7],
transform[8], transform[9], transform[10], transform[11],
0,0,0,1;
return m;
}
inline Eigen::Affine3f transformToEigen3f(const Transform & transform)
{
return Eigen::Affine3f(transformToEigen4f(transform));
}
inline Eigen::Affine3d transformToEigen3d(const Transform & transform)
{
return Eigen::Affine3d(transformToEigen4d(transform));
}
inline Transform transformFromEigen4f(const Eigen::Matrix4f & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
inline Transform transformFromEigen4d(const Eigen::Matrix4d & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
inline Transform transformFromEigen3f(const Eigen::Affine3f & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
inline Transform transformFromEigen3d(const Eigen::Affine3d & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
const cv::Mat & image,
const cv::Mat & imageDepth,
float constant,
int fastThreshold=50,
bool fastNonmaxSuppression=true,
float maxDepth = 5.0f);
pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float maxEdgeLength = 0.025, bool smoothing = true);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, Transform> & poses,
const std::multimap<int, std::pair<int, Transform> > & edgeConstraints,
int toroIterations,
std::map<int, Transform> & optimizedPoses,
Transform & mapCorrection);
bool RTABMAP_EXP saveTOROGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_H_ */
corelib/src/BayesFilter.cpp
+4 -27
View File
@@ -19,7 +19,7 @@
#include "BayesFilter.h"
#include "rtabmap/core/Memory.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Parameters.h"
#include <iostream>
@@ -42,37 +42,14 @@ BayesFilter::~BayesFilter() {
void BayesFilter::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kBayesVirtualPlacePriorThr())) != parameters.end())
{
this->setVirtualPlacePrior(std::atof((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kBayesPredictionLC())) != parameters.end())
{
this->setPredictionLC((*iter).second);
}
if((iter=parameters.find(Parameters::kBayesFullPredictionUpdate())) != parameters.end())
{
_fullPredictionUpdate = uStr2Bool((*iter).second.c_str());
}
Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior);
Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate);
}
void BayesFilter::setVirtualPlacePrior(float virtualPlacePrior)
{
if(virtualPlacePrior < 0)
{
ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
_virtualPlacePrior = 0;
}
else if(virtualPlacePrior > 1)
{
ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
_virtualPlacePrior = 1;
}
else
{
_virtualPlacePrior = virtualPlacePrior;
}
UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f);
}
// format = {Virtual place, Loop closure, level1, level2, l3, l4...}
corelib/src/BayesFilter.h
-1
View File
@@ -43,7 +43,6 @@ public:
void reset();
//setters
void setVirtualPlacePrior(float virtualPlacePrior);
void setPredictionLC(const std::string & prediction);
//getters
corelib/src/CMakeLists.txt
+26 -19
View File
@@ -13,6 +13,7 @@ SET(SRC_FILES
Camera.cpp
CameraThread.cpp
CameraOpenni.cpp
EpipolarGeometry.cpp
VisualWord.cpp
@@ -22,6 +23,14 @@ SET(SRC_FILES
Signature.cpp
Features2d.cpp
NearestNeighbor.cpp
Transform.cpp
util3d.cpp
Odometry.cpp
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp
toro3d/treeoptimizer3.cpp
)
SET(INCLUDE_DIRS
@@ -31,11 +40,9 @@ SET(INCLUDE_DIRS
${CMAKE_CURRENT_BINARY_DIR}
${OpenCV_INCLUDE_DIRS}
${SQLITE3_INCLUDE_DIR}
)
SET(LIBRARIES
${OpenCV_LIBS}
${SQLITE3_LIBRARY}
${PCL_INCLUDE_DIRS}
${ZLIB_INCLUDE_DIRS}
)
####################################
@@ -65,26 +72,26 @@ ADD_CUSTOM_COMMAND(
# Generate resources files END
####################################
add_definitions(${PCL_DEFINITIONS})
# Make sure the compiler can find include files from our library.
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
# Add binary that is built from the source file "main.cpp".
# The extension is automatically found.
ADD_LIBRARY(rtabmap_corelib ${SRC_FILES} ${RESOURCES_HEADERS})
TARGET_LINK_LIBRARIES(rtabmap_corelib rtabmap_utilite ${LIBRARIES})
ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS})
TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${OpenCV_LIBS} ${SQLITE3_LIBRARY} ${PCL_LIBRARIES} ${ZLIB_LIBRARIES})
SET_TARGET_PROPERTIES(
rtabmap_corelib
PROPERTIES
OUTPUT_NAME ${PROJECT_PREFIX}_core
INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
)
INSTALL(TARGETS rtabmap_corelib
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)
INSTALL(TARGETS rtabmap_core
EXPORT RTABMapTargets
RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime
LIBRARY DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel
ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel)
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION include/ COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/
DESTINATION "${INSTALL_INCLUDE_DIR}"
COMPONENT devel
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE)
corelib/src/Camera.cpp
+29 -45
View File
@@ -37,10 +37,8 @@ namespace rtabmap
Camera::Camera(float imageRate,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped,
int id) :
unsigned int framesDropped) :
_imageRate(imageRate),
_id(id),
_imageWidth(imageWidth),
_imageHeight(imageHeight),
_framesDropped(framesDropped),
@@ -74,7 +72,6 @@ void Camera::setFeaturesExtracted(bool featuresExtracted, KeypointDetector::Dete
{
ParametersMap pm;
pm.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str((int)detector)));
pm.insert(ParametersPair(Parameters::kKpDescriptorStrategy(), uNumber2Str((int)detector)));
this->parseParameters(pm);
}
}
@@ -101,12 +98,6 @@ void Camera::parseParameters(const ParametersMap & parameters)
{
detector = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
}
//Keypoint descriptor
KeypointDescriptor::DescriptorType descriptor = KeypointDescriptor::kDescriptorUndef;
if((iter=parameters.find(Parameters::kKpDescriptorStrategy())) != parameters.end())
{
descriptor = (KeypointDescriptor::DescriptorType)std::atoi((*iter).second.c_str());
}
if(detector!=KeypointDetector::kDetectorUndef)
{
@@ -116,44 +107,34 @@ void Camera::parseParameters(const ParametersMap & parameters)
delete _keypointDetector;
_keypointDetector = 0;
}
switch(detector)
{
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
break;
case KeypointDetector::kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
break;
}
}
else if(_keypointDetector)
{
_keypointDetector->parseParameters(parameters);
}
if(descriptor!=KeypointDescriptor::kDescriptorUndef)
{
ULOGGER_DEBUG("new descriptor strategy %d", int(descriptor));
if(_keypointDescriptor)
{
delete _keypointDescriptor;
_keypointDescriptor = 0;
}
switch(descriptor)
switch(detector)
{
case KeypointDescriptor::kDescriptorSift:
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorSurf:
case KeypointDetector::kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
_keypointDescriptor = new SURFDescriptor(parameters);
break;
}
}
else if(_keypointDescriptor)
else
{
_keypointDescriptor->parseParameters(parameters);
if(_keypointDetector)
{
_keypointDetector->parseParameters(parameters);
}
if(_keypointDescriptor)
{
_keypointDescriptor->parseParameters(parameters);
}
}
}
@@ -172,7 +153,7 @@ cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & key
{
descriptors = cv::Mat();
keypoints.clear();
float imageRate = _imageRate;
float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
if(imageRate>0)
{
int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
@@ -245,9 +226,8 @@ CameraImages::CameraImages(const std::string & path,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped,
int id) :
Camera(imageRate, imageWidth, imageHeight, framesDropped, id),
unsigned int framesDropped) :
Camera(imageRate, imageWidth, imageHeight, framesDropped),
_path(path),
_startAt(startAt),
_refreshDir(refreshDir),
@@ -382,9 +362,8 @@ CameraVideo::CameraVideo(int usbDevice,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped,
int id) :
Camera(imageRate, imageWidth, imageHeight, framesDropped, id),
unsigned int framesDropped) :
Camera(imageRate, imageWidth, imageHeight, framesDropped),
_src(kUsbDevice),
_usbDevice(usbDevice)
{
@@ -395,9 +374,8 @@ CameraVideo::CameraVideo(const std::string & filePath,
float imageRate,
unsigned int imageWidth,
unsigned int imageHeight,
unsigned int framesDropped,
int id) :
Camera(imageRate, imageWidth, imageHeight, framesDropped, id),
unsigned int framesDropped) :
Camera(imageRate, imageWidth, imageHeight, framesDropped),
_filePath(filePath),
_src(kVideoFile),
_usbDevice(0)
@@ -454,7 +432,13 @@ cv::Mat CameraVideo::captureImage()
cv::Mat img; // Null image
if(_capture.isOpened())
{
_capture.read(img);
if(!_capture.read(img))
{
if(_usbDevice)
{
UERROR("Camera has been disconnected!");
}
}
}
else
{
corelib/src/CameraOpenni.cpp
+150
View File
@@ -0,0 +1,150 @@
/*
* CameraOpenni.cpp
*
* Created on: 2013-08-22
* Author: Mathieu
*/
#include "rtabmap/core/CameraOpenni.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/utilite/ULogger.h"
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <pcl/io/openni_grabber.h>
namespace rtabmap {
CameraOpenni::CameraOpenni(const std::string & deviceId, float inputRate, const Transform & localTransform) :
interface_(0),
deviceId_(deviceId),
rate_(inputRate),
frameRateTimer_(new UTimer()),
localTransform_(localTransform),
seq_(0)
{
}
CameraOpenni::~CameraOpenni()
{
UDEBUG("");
kill();
delete frameRateTimer_;
if(interface_)
{
uSleep(100); // make sure it is stopped
delete interface_;
interface_ = 0;
}
}
void CameraOpenni::image_cb (
const boost::shared_ptr<openni_wrapper::Image>& rgb,
const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
float constant)
{
if(rate_>0.0f)
{
if(frameRateTimer_->getElapsedTime() < 1.0f/rate_)
{
return;
}
}
frameRateTimer_->start();
UTimer t;
cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
cv::Mat bgrFrame;
cv::cvtColor(rgbFrame, bgrFrame, CV_RGB2BGR);
cv::Mat depthFrame(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depthFrame.data);
this->post(new CameraEvent(bgrFrame, depthFrame, constant, localTransform_, ++seq_));
}
bool CameraOpenni::init()
{
if(interface_ && interface_->isRunning())
{
UERROR("Already started!!!\n");
return false;
}
else if(interface_)
{
delete interface_;
interface_ = 0;
}
seq_ = 0;
try
{
interface_ = new pcl::OpenNIGrabber(deviceId_);
}
catch(const pcl::IOException& ex)
{
UERROR("OpenNI exception: %s", ex.what());
if(interface_)
{
delete interface_;
interface_ = 0;
}
return false;
}
frameRateTimer_->start();
return true;
}
void CameraOpenni::start()
{
if(interface_)
{
if(!connection_.connected())
{
boost::function<void (
const boost::shared_ptr<openni_wrapper::Image>&,
const boost::shared_ptr<openni_wrapper::DepthImage>&,
float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
connection_ = interface_->registerCallback (f);
}
if(!interface_->isRunning())
{
interface_->start ();
}
}
}
void CameraOpenni::pause()
{
if(connection_.connected())
{
connection_.disconnect();
}
}
void CameraOpenni::kill()
{
UDEBUG("");
if(interface_)
{
interface_->stop();
}
}
bool CameraOpenni::isRunning()
{
return (interface_ && interface_->isRunning());
}
void CameraOpenni::setFrameRate(float rate)
{
rate_ = rate;
}
} /* namespace rtabmap */
corelib/src/CameraThread.cpp
+26 -9
View File
@@ -31,7 +31,8 @@ namespace rtabmap
// ownership transferred
CameraThread::CameraThread(Camera * camera, bool autoRestart) :
_camera(camera),
_autoRestart(autoRestart)
_autoRestart(autoRestart),
_seq(0)
{
UASSERT(_camera != 0);
}
@@ -43,6 +44,27 @@ CameraThread::~CameraThread()
delete _camera;
}
bool CameraThread::init()
{
if(!this->isRunning())
{
if(_camera)
{
_seq = 0;
return _camera->init();
}
else
{
UERROR("Cannot initialize the camera because the camera object is null...");
}
}
else
{
UERROR("Cannot initialize the camera because it is already running...");
}
return false;
}
void CameraThread::mainLoop()
{
State state = kStateCapturing;
@@ -82,11 +104,6 @@ void CameraThread::pushNewState(State newState, const ParametersMap & parameters
_stateMutex.unlock();
}
void CameraThread::setImageRate(float imageRate)
{
_camera->setImageRate(imageRate);
}
void CameraThread::handleEvent(UEvent* anEvent)
{
if(anEvent->getClassName().compare("ParamEvent") == 0)
@@ -116,11 +133,11 @@ void CameraThread::process()
{
if(_camera->isFeaturesExtracted())
{
this->post(new CameraEvent(descriptors, keypoints, img, _camera->id()));
this->post(new CameraEvent(descriptors, keypoints, img, ++_seq));
}
else
{
this->post(new CameraEvent(img, _camera->id()));
this->post(new CameraEvent(img, ++_seq));
}
}
else if(!this->isKilled())
@@ -133,7 +150,7 @@ void CameraThread::process()
{
ULOGGER_DEBUG("Camera::process() : no more images...");
this->kill();
this->post(new CameraEvent(_camera->id()));
this->post(new CameraEvent());
}
}
}
corelib/src/DBDriver.cpp
+48 -29
View File
@@ -19,7 +19,7 @@
#include "rtabmap/core/DBDriver.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "VisualWord.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UMath.h"
@@ -30,7 +30,6 @@
namespace rtabmap {
DBDriver::DBDriver(const ParametersMap & parameters) :
_imagesCompressed(Parameters::defaultDbImagesCompressed()),
_emptyTrashesTime(0)
{
this->parseParameters(parameters);
@@ -44,11 +43,6 @@ DBDriver::~DBDriver()
void DBDriver::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kDbImagesCompressed())) != parameters.end())
{
_imagesCompressed = uStr2Bool((*iter).second.c_str());
}
}
void DBDriver::closeConnection()
@@ -286,6 +280,13 @@ void DBDriver::load(VWDictionary * dictionary) const
_dbSafeAccessMutex.unlock();
}
void DBDriver::load(std::map<int, std::map<int, Transform> > & mapTransforms) const
{
_dbSafeAccessMutex.lock();
this->loadQuery(mapTransforms);
_dbSafeAccessMutex.unlock();
}
void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
{
_dbSafeAccessMutex.lock();
@@ -386,23 +387,45 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
}
//TODO Check also in the trash ?
void DBDriver::getImage(int signatureId, cv::Mat & rawData) const
void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const
{
_dbSafeAccessMutex.lock();
this->getImageQuery(signatureId, rawData);
this->loadNodeDataQuery(signatures, loadMetricData);
_dbSafeAccessMutex.unlock();
}
//TODO Check also in the trash ?
void DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions) const
void DBDriver::getNodeData(
int signatureId,
std::vector<unsigned char> & image,
std::vector<unsigned char> & depth,
std::vector<unsigned char> & depth2d,
float & depthConstant,
Transform & localTransform) const
{
_dbSafeAccessMutex.lock();
this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
this->getNodeDataQuery(signatureId, image, depth, depth2d, depthConstant, localTransform);
_dbSafeAccessMutex.unlock();
}
//TODO Check also in the trash ?
void DBDriver::loadNeighbors(int signatureId, std::set<int> & neighbors) const
void DBDriver::getNodeData(int signatureId, std::vector<unsigned char> & image) const
{
_dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, image);
_dbSafeAccessMutex.unlock();
}
//TODO Check also in the trash ?
void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
{
_dbSafeAccessMutex.lock();
this->getPoseQuery(signatureId, pose, mapId);
_dbSafeAccessMutex.unlock();
}
//TODO Check also in the trash ?
void DBDriver::loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const
{
_dbSafeAccessMutex.lock();
this->loadNeighborsQuery(signatureId, neighbors);
@@ -418,10 +441,10 @@ void DBDriver::getWeight(int signatureId, int & weight) const
}
//TODO Check also in the trash ?
void DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const
void DBDriver::loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
{
_dbSafeAccessMutex.lock();
this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
this->loadLoopClosuresQuery(signatureId, loopIds, childIds);
_dbSafeAccessMutex.unlock();
}
@@ -457,29 +480,25 @@ void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
_dbSafeAccessMutex.unlock();
}
void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
void DBDriver::save(const std::map<int, std::map<int, Transform> > & mapTransforms) const
{
_dbSafeAccessMutex.lock();
saveQuery(mapTransforms);
_dbSafeAccessMutex.unlock();
}
void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::stringstream query;
query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used) values("
query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size) values("
<< stMemSize << ","
<< lastSignAdded << ","
<< processMemUsed << ","
<< databaseMemUsed << ");";
this->executeNoResultQuery(query.str());
}
}
void DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::stringstream query;
query << "INSERT INTO StatisticsDictionary(dictionary_size) values(" << dictionarySize << ");";
<< databaseMemUsed << ","
<< dictionarySize << ");";
this->executeNoResultQuery(query.str());
}
corelib/src/DBDriverSqlite3.cpp
+609 -266
View File
File diff suppressed because it is too large Load Diff
corelib/src/DBDriverSqlite3.h
+32 -9
View File
@@ -24,6 +24,7 @@
#include "rtabmap/core/DBDriver.h"
#include <opencv2/features2d/features2d.hpp>
#include <sqlite3.h>
#include <pcl/point_types.h>
namespace rtabmap {
@@ -47,40 +48,62 @@ private:
virtual void executeNoResultQuery(const std::string & sql) const;
virtual void getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
virtual void getWeightQuery(int signatureId, int & weight) const;
virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
virtual void saveQuery(const std::list<Signature *> & signatures) const;
virtual void saveQuery(const std::list<VisualWord *> & words) const;
virtual void updateQuery(const std::list<Signature *> & signatures) const;
virtual void updateQuery(const std::list<VisualWord *> & words) const;
virtual void saveQuery(const std::map<int, std::map<int, Transform> > & mapTransforms) const;
// Load objects
virtual void loadQuery(VWDictionary * dictionary) const;
virtual void loadQuery(std::map<int, std::map<int, Transform> > & mapTransforms) const;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadNeighborsQuery(int signatureId, std::set<int> & neighbors) const;
virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const;
virtual void loadLoopClosuresQuery(
int signatureId,
std::map<int, Transform> & loopIds,
std::map<int, Transform> & childIds) const;
virtual void getImageQuery(int nodeId, cv::Mat & image) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
virtual void getNodeDataQuery(
int signatureId,
std::vector<unsigned char> & image,
std::vector<unsigned char> & depth,
std::vector<unsigned char> & depth2d,
float & depthConstant,
Transform & localTransform) const;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const;
virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
private:
std::string queryStepNode() const;
std::string queryStepNodeToSensor() const;
std::string queryStepImage() const;
std::string queryStepDepth() const;
std::string queryStepLink() const;
std::string queryStepWordsChanged() const;
std::string queryStepKeypoint() const;
void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
void stepNodeToSensor(sqlite3_stmt * ppStmt, int nodeId, int sensorId, int num) const;
void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & image) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type) const;
void stepImage(
sqlite3_stmt * ppStmt,
int id,
const std::vector<unsigned char> & image) const;
void stepDepth(
sqlite3_stmt * ppStmt,
int id,
const std::vector<unsigned char> & depth,
const std::vector<unsigned char> & depth2d,
float depthConstant,
const Transform & localTransform) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;
corelib/src/DBReader.cpp
+56 -12
View File
@@ -10,17 +10,20 @@
#include "DBDriverSqlite3.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UFile.h>
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/util3d.h"
namespace rtabmap {
DBReader::DBReader(const std::string & databasePath,
float frameRate) :
float frameRate,
bool odometryIgnored) :
_path(databasePath),
_frameRate(frameRate),
_odometryIgnored(odometryIgnored),
_dbDriver(0),
_currentId(_ids.end())
{
@@ -102,22 +105,46 @@ void DBReader::mainLoopBegin()
void DBReader::mainLoop()
{
cv::Mat image;
this->getNextImage(image);
cv::Mat image, depth, depth2d;
float depthConstant;
Transform localTransform, pose;
this->getNextImage(image, depth, depth2d, depthConstant, localTransform, pose);
if(!image.empty())
{
UEventsManager::post(new CameraEvent(image));
if(depth.empty())
{
this->post(new CameraEvent(image));
}
else
{
if(!_odometryIgnored)
{
Image data(image, depth, depth2d, depthConstant, pose, localTransform);
this->post(new OdometryEvent(data));
}
else
{
// without odometry
this->post(new CameraEvent(image, depth, depth2d, depthConstant, localTransform));
}
}
}
else if(!this->isKilled())
{
UDEBUG("no more images...");
UINFO("no more images...");
this->kill();
UEventsManager::post(new CameraEvent());
this->post(new CameraEvent());
}
}
void DBReader::getNextImage(cv::Mat & image)
void DBReader::getNextImage(
cv::Mat & image,
cv::Mat & depth,
cv::Mat & depth2d,
float & depthConstant,
Transform & localTransform,
Transform & pose)
{
if(_dbDriver)
{
@@ -143,13 +170,30 @@ void DBReader::getNextImage(cv::Mat & image)
if(!this->isKilled() && _currentId != _ids.end())
{
//sensors
_dbDriver->getImage(*_currentId, image);
std::vector<unsigned char> imageBytes;
std::vector<unsigned char> depthBytes;
std::vector<unsigned char> depth2dBytes;
int mapId;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, depthConstant, localTransform);
_dbDriver->getPose(*_currentId, pose, mapId);
++_currentId;
if(image.empty())
if(imageBytes.empty())
{
UWARN("No image loaded from the database!");
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctDepth2D(depth2dBytes, false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
ctImage.join();
ctDepth.join();
ctDepth2D.join();
image = ctImage.getUncompressedData();
depth = ctDepth.getUncompressedData();
depth2d = ctDepth2D.getUncompressedData();
}
}
else
corelib/src/EpipolarGeometry.cpp
+3 -2
View File
@@ -18,7 +18,7 @@
*/
#include "rtabmap/core/EpipolarGeometry.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
@@ -427,7 +427,8 @@ int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int EpipolarGeometry::findPairsUnique(const std::multimap<int, cv::KeyPoint> & wordsA,
int EpipolarGeometry::findPairsUnique(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
corelib/src/Features2d.cpp
+96 -208
View File
@@ -35,6 +35,55 @@
namespace rtabmap {
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
{
cv::Mat descriptors;
limitKeypoints(keypoints, descriptors, maxKeypoints);
}
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
{
UASSERT((int)keypoints.size() == descriptors.rows || descriptors.rows == 0);
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
for(unsigned int i = 0; i <keypoints.size(); ++i)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
}
// Remove them from the signature
int removed = hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
cv::Mat descriptorsTmp;
if(descriptors.rows)
{
descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
}
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = keypoints[iter->second];
if(descriptors.rows)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
if(descriptors.rows)
{
descriptors = descriptorsTmp;
}
}
}
/////////////////////
// KeypointDescriptor
@@ -73,35 +122,12 @@ SURFDescriptor::~SURFDescriptor()
void SURFDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
Parameters::parse(parameters, Parameters::kSURFExtended(), _extended);
Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), _hessianThreshold);
Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), _nOctaveLayers);
Parameters::parse(parameters, Parameters::kSURFOctaves(), _nOctaves);
Parameters::parse(parameters, Parameters::kSURFUpright(), _upright);
Parameters::parse(parameters, Parameters::kSURFGpuVersion(), _gpuVersion);
KeypointDescriptor::parseParameters(parameters);
}
@@ -186,26 +212,11 @@ SIFTDescriptor::~SIFTDescriptor()
void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), _contrastThreshold);
Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), _edgeThreshold);
Parameters::parse(parameters, Parameters::kSIFTNFeatures(), _nfeatures);
Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), _nOctaveLayers);
Parameters::parse(parameters, Parameters::kSIFTSigma(), _sigma);
KeypointDescriptor::parseParameters(parameters);
}
@@ -253,90 +264,33 @@ cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<c
/////////////////////
// KeypointDetector
/////////////////////
KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_roiRatios(std::vector<float>(4, 0.0f))
KeypointDetector::KeypointDetector(const ParametersMap & parameters)
{
this->setRoi(Parameters::defaultKpRoiRatios());
this->parseParameters(parameters);
}
void KeypointDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kKpWordsPerImage())) != parameters.end())
{
_wordsPerImageTarget = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
this->setRoi((*iter).second);
}
}
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & image)
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(
const cv::Mat & image,
int maxKeypoints,
const cv::Rect & roi)
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image.empty())
{
UTimer timer;
timer.start();
cv::Rect roi = computeRoi(image);
// Get keypoints
keypoints = this->_generateKeypoints(image, roi);
keypoints = this->_generateKeypoints(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
//clip the number of words... to _wordsPerImageTarget
// Variable hessian threshold
if(_wordsPerImageTarget > 0)
{
if(keypoints.size() > 0)
{
// 10% margin...
if(keypoints.size() > 1.1 * _wordsPerImageTarget)
{
ULOGGER_DEBUG("too much words (%d), removing words under the new hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
limitKeypoints(keypoints, maxKeypoints);
// Sort words by hessian
std::multimap<float, std::vector<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
for(std::vector<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, std::vector<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
}
// Remove them from the signature
int removed = hessianMap.size()-_wordsPerImageTarget;
std::multimap<float, std::vector<cv::KeyPoint>::iterator>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(_wordsPerImageTarget);
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = *iter->second;
// Adjust keypoint position to raw image
kptsTmp[k].pt.x += roi.x;
kptsTmp[k].pt.y += roi.y;
}
keypoints = kptsTmp;
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
}
else if(roi.x || roi.y)
if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
@@ -353,71 +307,40 @@ std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & im
return keypoints;
}
void KeypointDetector::setRoi(const std::string & roi)
cv::Rect KeypointDetector::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
{
std::list<std::string> strValues = uSplit(roi, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = std::atof((*iter).c_str());
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
}
}
}
cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
{
if(!image.empty() && _roiRatios.size() == 4)
if(!image.empty() && roiRatios.size() == 4)
{
float width = image.cols;
float height = image.rows;
cv::Rect roi(0, 0, width, height);
UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
//left roi
if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
{
roi.x = width * _roiRatios[0];
roi.x = width * roiRatios[0];
}
//right roi
roi.width = width - roi.x;
if(_roiRatios[1] > 0 && _roiRatios[1] < 1 - _roiRatios[0])
if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
{
roi.width -= width * _roiRatios[1];
roi.width -= width * roiRatios[1];
}
//top roi
if(_roiRatios[2] > 0 && _roiRatios[2] < 1 - _roiRatios[3])
if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
{
roi.y = height * _roiRatios[2];
roi.y = height * roiRatios[2];
}
//bottom roi
roi.height = height - roi.y;
if(_roiRatios[3] > 0 && _roiRatios[3] < 1 - _roiRatios[2])
if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
{
roi.height -= height * _roiRatios[3];
roi.height -= height * roiRatios[3];
}
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
@@ -425,7 +348,7 @@ cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
}
else
{
UERROR("Image is null or _roiRatios(=%d) != 4", _roiRatios.size());
UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
return cv::Rect();
}
}
@@ -452,35 +375,12 @@ SURFDetector::~SURFDetector()
void SURFDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
Parameters::parse(parameters, Parameters::kSURFExtended(), _extended);
Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), _hessianThreshold);
Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), _nOctaveLayers);
Parameters::parse(parameters, Parameters::kSURFOctaves(), _nOctaves);
Parameters::parse(parameters, Parameters::kSURFUpright(), _upright);
Parameters::parse(parameters, Parameters::kSURFGpuVersion(), _gpuVersion);
KeypointDetector::parseParameters(parameters);
}
@@ -497,6 +397,7 @@ std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
ULOGGER_DEBUG("");
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
@@ -525,14 +426,17 @@ std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image
detector.detect(imgRoi, keypoints);
}
#else*/
ULOGGER_DEBUG("%f %d %d %d %d", _hessianThreshold, _nOctaves, _nOctaveLayers, _extended?1:0, _upright?1:0);
cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
ULOGGER_DEBUG("");
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::imwrite("test.png", imgRoi);
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, cv::Mat(), keypoints);
#endif
//#endif
ULOGGER_DEBUG("");
return keypoints;
}
@@ -556,27 +460,11 @@ SIFTDetector::~SIFTDetector()
void SIFTDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), _contrastThreshold);
Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), _edgeThreshold);
Parameters::parse(parameters, Parameters::kSIFTNFeatures(), _nfeatures);
Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), _nOctaveLayers);
Parameters::parse(parameters, Parameters::kSIFTSigma(), _sigma);
KeypointDetector::parseParameters(parameters);
}
corelib/src/Memory.cpp
+1383 -645
View File
File diff suppressed because it is too large Load Diff
corelib/src/Odometry.cpp
+731
View File
@@ -0,0 +1,731 @@
/*
* Odometry.cpp
*
* Created on: 2013-08-23
* Author: Mathieu
*/
#include "rtabmap/core/Odometry.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/CameraEvent.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Features2d.h>
#include <rtabmap/core/Memory.h>
#include "rtabmap/core/Signature.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#if _MSC_VER
#define ISFINITE(value) _finite(value)
#else
#define ISFINITE(value) std::isfinite(value)
#endif
namespace rtabmap {
Odometry::Odometry(
float inlierDistance,
int maxWords,
int minInliers,
int iterations,
float maxDepth,
float linearUpdate,
float angularUpdate,
int resetCoutdown) :
_maxFeatures(maxWords),
_minInliers(minInliers),
_inlierDistance(inlierDistance),
_iterations(iterations),
_maxDepth(maxDepth),
_linearUpdate(linearUpdate),
_angularUpdate(angularUpdate),
_resetCountdown(resetCoutdown),
_pose(Transform::getIdentity()),
_resetCurrentCount(0)
{
}
Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_maxFeatures(Parameters::defaultOdomMaxWords()),
_minInliers(Parameters::defaultOdomMinInliers()),
_inlierDistance(Parameters::defaultOdomInlierDistance()),
_iterations(Parameters::defaultOdomIterations()),
_maxDepth(Parameters::defaultOdomMaxDepth()),
_linearUpdate(Parameters::defaultOdomLinearUpdate()),
_angularUpdate(Parameters::defaultOdomAngularUpdate()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_pose(Transform::getIdentity()),
_resetCurrentCount(0)
{
Parameters::parse(parameters, Parameters::kOdomLinearUpdate(), _linearUpdate);
Parameters::parse(parameters, Parameters::kOdomAngularUpdate(), _angularUpdate);
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kOdomMaxWords(), _maxFeatures);
}
void Odometry::reset()
{
_resetCurrentCount = 0;
_pose = Transform::getIdentity();
}
bool Odometry::isLargeEnoughTransform(const Transform & transform)
{
return fabs(transform.x()) > _linearUpdate ||
fabs(transform.y()) > _linearUpdate ||
fabs(transform.z()) > _linearUpdate;
}
Transform Odometry::process(Image & image)
{
Transform t = this->computeTransform(image);
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
_pose *= t;
return _pose;
}
else if(_resetCurrentCount > 0)
{
UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
--_resetCurrentCount;
if(_resetCurrentCount == 0)
{
UWARN("Odometry automatically reset!");
this->reset();
}
}
return Transform();
}
OdometryBinary::OdometryBinary(
float inlierDistance,
int maxWords,
int minInliers,
int iterations,
float maxDepth,
float linearUpdate,
float angularUpdate,
int resetCoutdown,
int briefBytes,
int fastThreshold,
bool fastNonmaxSuppression,
bool bruteForceMatching) :
Odometry(inlierDistance, maxWords, minInliers, iterations, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
_briefBytes(briefBytes),
_fastThreshold(fastThreshold),
_fastNonmaxSuppression(fastNonmaxSuppression),
_bruteForceMatching(bruteForceMatching)
{
}
OdometryBinary::OdometryBinary(const ParametersMap & parameters) :
Odometry(parameters),
_briefBytes(Parameters::defaultOdomBinBriefBytes()),
_fastThreshold(Parameters::defaultOdomBinFastThreshold()),
_fastNonmaxSuppression(Parameters::defaultOdomBinFastNonmaxSuppression()),
_bruteForceMatching(Parameters::defaultOdomBinBruteForceMatching())
{
Parameters::parse(parameters, Parameters::kOdomBinBriefBytes(), _briefBytes);
Parameters::parse(parameters, Parameters::kOdomBinFastThreshold(), _fastThreshold);
Parameters::parse(parameters, Parameters::kOdomBinFastNonmaxSuppression(), _fastNonmaxSuppression);
Parameters::parse(parameters, Parameters::kOdomBinBruteForceMatching(), _bruteForceMatching);
}
void OdometryBinary::reset()
{
Odometry::reset();
_lastKeypoints.clear();
_lastDescriptors = cv::Mat();
_lastDepth = cv::Mat();
}
// return true if odometry is correctly computed
Transform OdometryBinary::computeTransform(Image & image)
{
UTimer timer;
cv::Mat imageMono;
Transform output;
// convert to grayscale
if(image.image().channels() > 1)
{
cv::cvtColor(image.image(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = image.image();
}
cv::FastFeatureDetector detector(_fastThreshold, _fastNonmaxSuppression);
std::vector<cv::KeyPoint> newKeypoints;
detector.detect(imageMono, newKeypoints);
limitKeypoints(newKeypoints, this->getMaxFeatures());
cv::BriefDescriptorExtractor extractor(_briefBytes);
cv::Mat newDescriptors;
extractor.compute(imageMono, newKeypoints, newDescriptors);
int inliers = 0;
int correspondences = 0;
if(_lastKeypoints.size())
{
if(newDescriptors.rows)
{
cv::Mat results;
cv::Mat dists;
int k=1; // find the 1 nearest neighbor
std::vector<std::vector<cv::DMatch> > matches;
if(_bruteForceMatching)
{
cv::BFMatcher matcher(cv::NORM_HAMMING);
matcher.knnMatch(newDescriptors, _lastDescriptors, matches, k);
}
else
{
// Create Flann LSH index
cv::flann::Index flannIndex(_lastDescriptors, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
results = cv::Mat(newDescriptors.rows, k, CV_32SC1);
dists = cv::Mat(newDescriptors.rows, k, CV_32FC1);
// search (nearest neighbor)
flannIndex.knnSearch(newDescriptors, results, dists, k, cv::flann::SearchParams() );
}
pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_2(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> indexes_1, indexes_2;
std::vector<uchar> outlier_mask;
// Check if this descriptor matches with those of the objects
mpts_1->resize(newDescriptors.rows);
mpts_2->resize(newDescriptors.rows);
UDEBUG("newDescriptors=%d _lastKeypoints=%d time=%fs", newDescriptors.rows, _lastKeypoints.size(), timer.elapsed());
int oi = 0;
if(_bruteForceMatching)
{
for(unsigned int i=0; i<matches.size(); ++i)
{
pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.x+0.5f),
int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.y+0.5f),
(float)imageMono.cols/2,
(float)imageMono.rows/2,
1.0f/image.depthConstant(),
1.0f/image.depthConstant());
if(matches.at(i).at(0).trainIdx >=0)
{
pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.x+0.5f),
int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.y+0.5f),
(float)imageMono.cols/2,
(float)imageMono.rows/2,
1.0f/image.depthConstant(),
1.0f/image.depthConstant());
if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
(this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
{
mpts_1->at(oi) = pt1;
mpts_2->at(oi) = pt2;
++oi;
}
}
else
{
UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
}
}
}
else
{
for(int i=0; i<newDescriptors.rows; ++i)
{
pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
int(newKeypoints.at(i).pt.x+0.5f),
int(newKeypoints.at(i).pt.y+0.5f),
(float)imageMono.cols/2,
(float)imageMono.rows/2,
1.0f/image.depthConstant(),
1.0f/image.depthConstant());
if(results.at<int>(i,0) >=0)
{
pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
int(_lastKeypoints.at(results.at<int>(i,0)).pt.x+0.5f),
int(_lastKeypoints.at(results.at<int>(i,0)).pt.y+0.5f),
(float)imageMono.cols/2,
(float)imageMono.rows/2,
1.0f/image.depthConstant(),
1.0f/image.depthConstant());
if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
(this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
{
mpts_1->at(oi) = pt1;
mpts_2->at(oi) = pt2;
++oi;
}
}
else
{
UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
}
}
}
mpts_1->resize(oi);
mpts_2->resize(oi);
UDEBUG("Correspondences = %d", oi);
if(oi >= this->getMinInliers())
{
mpts_1 = util3d::transformPointCloud(mpts_1, image.localTransform()); // new
mpts_2 = util3d::transformPointCloud(mpts_2, image.localTransform()); // previous
correspondences = mpts_2->size();
Transform t = util3d::transformFromXYZCorrespondences(
mpts_1,
mpts_2,
this->getInlierDistance(),
this->getIterations(),
&inliers);
float x,y,z, roll,pitch,yaw;
pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(t), x,y,z, roll,pitch,yaw);
// Large transforms may be erroneous computed transforms, so keep under 1 m
if(inliers >= this->getMinInliers())
{
if(isLargeEnoughTransform(t))
{
_lastKeypoints = newKeypoints;
_lastDescriptors = newDescriptors;
_lastDepth = image.depth().clone();
output = t;
}
else
{
output.setIdentity();
}
}
else
{
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
}
else
{
UWARN("Not enough inliers %d < %d", oi, this->getMinInliers());
}
}
else
{
UWARN("No feature extracted!");
}
}
else
{
_lastKeypoints = newKeypoints;
_lastDescriptors = newDescriptors;
_lastDepth = image.depth().clone();
output.setIdentity();
}
UINFO("Odom update time = %fs features=%d inliers=%d/%d",
timer.elapsed(),
newDescriptors.rows,
inliers,
correspondences);
return output;
}
//OdometryBOW
OdometryBOW::OdometryBOW(
int detectorType, // SURF or SIFT
float inlierDistance,
int maxWords,
int minInliers,
int iterations,
float maxDepth,
float linearUpdate,
float angularUpdate,
int resetCoutdown,
float surfHessianThreshold,
float nndr) : // nearest neighbor distance ratio
Odometry(inlierDistance, maxWords, minInliers, iterations, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
_memory(new Memory())
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxWords)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(detectorType)));
customParameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(surfHessianThreshold)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
if(!_memory->init("", false, customParameters, false))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
}
OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
Odometry(parameters),
_memory(new Memory(parameters))
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
if(!_memory->init("", false, customParameters, false))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
}
OdometryBOW::~OdometryBOW()
{
UDEBUG("");
delete _memory;
UDEBUG("");
}
void OdometryBOW::reset()
{
Odometry::reset();
_memory->init("", false, ParametersMap(), false);
}
// return true if odometry is correctly computed
Transform OdometryBOW::computeTransform(Image & image)
{
UTimer timer;
Transform output;
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
_memory->extractKeypointsAndDescriptors(image.image(), keypoints, descriptors);
image.setDescriptors(descriptors);
image.setKeypoints(keypoints);
int inliers = 0;
int correspondences = 0;
const Signature * previousSignature = _memory->getLastWorkingSignature();
if(_memory->update(image))
{
const Signature * newSignature = _memory->getLastWorkingSignature();
if(previousSignature && newSignature)
{
Transform transform;
if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
util3d::findCorrespondences(
previousSignature->getWords3(),
newSignature->getWords3(),
*inliers1,
*inliers2,
this->getMaxDepth());
if((int)inliers1->size() >= this->getMinInliers())
{
correspondences = inliers1->size();
transform = util3d::transformFromXYZCorrespondences(
inliers2,
inliers1,
this->getInlierDistance(),
this->getIterations(),
&inliers);
if(inliers < this->getMinInliers())
{
transform.setNull();
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
}
else
{
UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers());
}
}
if(transform.isNull())
{
_memory->deleteLocation(newSignature->id());
}
else if(!isLargeEnoughTransform(transform))
{
output.setIdentity();
_memory->deleteLocation(newSignature->id());
}
else
{
output = transform;
_memory->deleteLocation(previousSignature->id());
}
}
else if(!previousSignature && newSignature)
{
output.setIdentity();
}
_memory->emptyTrash();
}
UINFO("Odom update time = %fs features=%d inliers=%d/%d",
timer.elapsed(),
descriptors.rows,
inliers,
correspondences);
return output;
}
// OdometryICP
OdometryICP::OdometryICP(
int decimation,
float voxelSize,
float samples,
float maxCorrespondenceDistance,
int maxIterations,
float maxFitness,
float maxDepth,
float linearUpdate,
float angularUpdate,
int resetCoutdown) :
Odometry(0, 0, 0, 0, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
_decimation(decimation),
_voxelSize(voxelSize),
_samples(samples),
_maxCorrespondenceDistance(maxCorrespondenceDistance),
_maxIterations(maxIterations),
_maxFitness(maxFitness),
_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
{
}
OdometryICP::OdometryICP(const ParametersMap & parameters) :
Odometry(parameters),
_decimation(Parameters::defaultOdomICPDecimation()),
_voxelSize(Parameters::defaultOdomICPVoxelSize()),
_samples(Parameters::defaultOdomICPSamples()),
_maxCorrespondenceDistance(Parameters::defaultOdomICPCorrespondencesDistance()),
_maxIterations(Parameters::defaultOdomICPIterations()),
_maxFitness(Parameters::defaultOdomICPMaxFitness()),
_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
{
Parameters::parse(parameters, Parameters::kOdomICPDecimation(), _decimation);
Parameters::parse(parameters, Parameters::kOdomICPVoxelSize(), _voxelSize);
Parameters::parse(parameters, Parameters::kOdomICPSamples(), _samples);
Parameters::parse(parameters, Parameters::kOdomICPCorrespondencesDistance(), _maxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kOdomICPIterations(), _maxIterations);
Parameters::parse(parameters, Parameters::kOdomICPMaxFitness(), _maxFitness);
}
void OdometryICP::reset()
{
Odometry::reset();
_previousCloud.reset(new pcl::PointCloud<pcl::PointNormal>);
}
// return not null if odometry is correctly computed
Transform OdometryICP::computeTransform(Image & image)
{
UTimer timer;
Transform output;
bool hasConverged = false;
double fitness = 0;
unsigned int minPoints = 100;
if(!image.depth().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
image.depth(),
image.depthConstant(),
_decimation,
this->getMaxDepth(),
_voxelSize,
_samples,
image.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
UWARN("removed nan normals...");
}
if(_previousCloud->size() > minPoints && newCloud->size() > minPoints)
{
Transform transform = util3d::icpPointToPlane(newCloud,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
fitness);
//pcl::io::savePCDFile("old.pcd", *_previousCloud);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
{
output = transform;
_previousCloud = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
hasConverged?"true":"false", fitness, _maxFitness);
}
}
else if(newCloud->size() > minPoints)
{
output.setIdentity();
_previousCloud = newCloud;
}
}
else
{
UERROR("Depth is empty?!?");
}
UINFO("Odom update time = %fs hasConverged=%s fitness=%f cloud=%d",
timer.elapsed(),
hasConverged?"true":"false",
fitness,
(int)_previousCloud->size());
return output;
}
// OdometryThread
OdometryThread::OdometryThread(Odometry * odometry) :
_odometry(odometry),
_resetOdometry(false)
{
UASSERT(_odometry != 0);
}
OdometryThread::~OdometryThread()
{
this->unregisterFromEventsManager();
this->join(true);
if(_odometry)
{
delete _odometry;
}
}
void OdometryThread::handleEvent(UEvent * event)
{
if(this->isRunning())
{
if(event->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * cameraEvent = (CameraEvent*)event;
if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth)
{
this->addImage(cameraEvent->image());
}
else if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
{
this->post(new CameraEvent()); // forward the event
}
}
else if(event->getClassName().compare("OdometryResetEvent") == 0)
{
_resetOdometry = true;
}
}
}
void OdometryThread::mainLoopKill()
{
_imageAdded.release();
}
//============================================================
// MAIN LOOP
//============================================================
void OdometryThread::mainLoop()
{
if(_resetOdometry)
{
_odometry->reset();
_resetOdometry = false;
}
Image image;
getImage(image);
if(!image.empty())
{
Transform pose = _odometry->process(image);
image.setPose(pose); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(image));
}
}
void OdometryThread::addImage(const Image & image)
{
if(image.empty() || image.depth().empty() || image.depthConstant() == 0.0f)
{
ULOGGER_ERROR("image empty !?");
return;
}
bool notify = true;
_imageMutex.lock();
{
notify = _imageBuffer.empty();
_imageBuffer = image;
}
_imageMutex.unlock();
if(notify)
{
_imageAdded.release();
}
}
void OdometryThread::getImage(Image & image)
{
_imageAdded.acquire();
_imageMutex.lock();
{
if(!_imageBuffer.empty())
{
image = _imageBuffer;
_imageBuffer = cv::Mat();
}
}
_imageMutex.unlock();
}
} /* namespace rtabmap */
corelib/src/Parameters.cpp
+84 -1
View File
@@ -20,11 +20,15 @@
#include "rtabmap/core/Parameters.h"
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <math.h>
#include <stdlib.h>
namespace rtabmap
{
ParametersMap Parameters::parameters_;
ParametersMap Parameters::descriptions_;
Parameters Parameters::instance_;
Parameters::Parameters()
@@ -37,18 +41,97 @@ Parameters::~Parameters()
std::string Parameters::getDefaultWorkingDirectory()
{
#ifdef DEMO_BUILD
std::string path = "."; // current directory
#else
std::string path = UDirectory::homeDir();
if(!path.empty())
{
UDirectory::makeDir(path += UDirectory::separator() + "Documents");
UDirectory::makeDir(path += UDirectory::separator() + "RTAB-Map");
path += UDirectory::separator(); // add trailing separator
}
else
{
UFATAL("Can't get the HOME variable environment!");
}
#endif
path += UDirectory::separator(); // add trailing separator
return path;
}
std::string Parameters::getDefaultDatabasePath()
{
return getDefaultWorkingDirectory() + getDefaultDatabaseName();
}
std::string Parameters::getDefaultDatabaseName()
{
return "rtabmap.db";
}
std::string Parameters::getDescription(const std::string & paramKey)
{
std::string description;
ParametersMap::iterator iter = descriptions_.find(paramKey);
if(iter != descriptions_.end())
{
description = iter->second;
}
else
{
UERROR("Parameters \"%s\" doesn't exist!", paramKey.c_str());
}
return description;
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, bool & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = uStr2Bool(iter->second.c_str());
}
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, int & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = atoi(iter->second.c_str());
}
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, unsigned int & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = atoi(iter->second.c_str());
}
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, float & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = atof(iter->second.c_str());
}
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, double & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = atof(iter->second.c_str());
}
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, std::string & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = iter->second;
}
}
}
corelib/src/Rtabmap.cpp
+687 -109
View File
File diff suppressed because it is too large Load Diff
corelib/src/RtabmapThread.cpp
+115 -39
View File
@@ -23,16 +23,21 @@
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/ParamEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
namespace rtabmap {
RtabmapThread::RtabmapThread() :
_imageBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
_rtabmap(new Rtabmap())
_rate(Parameters::defaultRtabmapDetectionRate()),
_frameRateTimer(new UTimer()),
_rtabmap(new Rtabmap()),
_paused(false)
{
}
@@ -44,6 +49,7 @@ RtabmapThread::~RtabmapThread()
// Stop the thread first
join(true);
delete _frameRateTimer;
delete _rtabmap;
}
@@ -61,12 +67,7 @@ void RtabmapThread::pushNewState(State newState, const ParametersMap & parameter
_imageAdded.release();
}
void RtabmapThread::setWorkingDirectory(const std::string & path)
{
_rtabmap->setWorkingDirectory(path);
}
void RtabmapThread::clearBufferedSensors()
void RtabmapThread::clearBufferedData()
{
_imageMutex.lock();
{
@@ -75,9 +76,36 @@ void RtabmapThread::clearBufferedSensors()
_imageMutex.unlock();
}
void RtabmapThread::publishMap() const
{
std::map<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depths2d;
std::map<int, float> depthConstants;
std::map<int, Transform> localTransforms;
std::map<int, Transform> poses;
Transform mapCorrection;
_rtabmap->get3DMap(images,
depths,
depths2d,
depthConstants,
localTransforms,
poses,
mapCorrection);
this->post(new RtabmapEvent3DMap(images,
depths,
depths2d,
depthConstants,
localTransforms,
poses,
mapCorrection));
}
void RtabmapThread::mainLoopKill()
{
this->clearBufferedSensors();
this->clearBufferedData();
// this will post the newData semaphore
_imageAdded.release();
@@ -100,22 +128,21 @@ void RtabmapThread::mainLoop()
}
_stateMutex.unlock();
ParametersMap::iterator iter;
switch(state)
{
case kStateDetecting:
this->process();
break;
case kStateChangingParameters:
if((iter=parameters.find(Parameters::kRtabmapImageBufferSize())) != parameters.end())
{
_imageBufferMaxSize = std::atoi(iter->second.c_str());
}
Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _imageBufferMaxSize);
Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
UASSERT(_imageBufferMaxSize >= 0);
UASSERT(_rate >= 0.0f);
_rtabmap->parseParameters(parameters);
break;
case kStateReseting:
_rtabmap->resetMemory();
this->clearBufferedSensors();
this->clearBufferedData();
break;
case kStateDumpingMemory:
_rtabmap->dumpData();
@@ -131,10 +158,16 @@ void RtabmapThread::mainLoop()
break;
case kStateDeletingMemory:
_rtabmap->resetMemory(true);
this->clearBufferedSensors();
this->clearBufferedData();
break;
case kStateCleanSensorsBuffer:
this->clearBufferedSensors();
case kStateCleanDataBuffer:
this->clearBufferedData();
break;
case kStatePublishingMap:
this->publishMap();
break;
case kStateTriggeringMap:
_rtabmap->triggerNewMap();
break;
default:
UFATAL("Invalid state !?!?");
@@ -147,12 +180,24 @@ void RtabmapThread::handleEvent(UEvent* event)
{
if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
{
UDEBUG("CameraEvent");
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeFeatures)
if(e->getCode() == CameraEvent::kCodeImage ||
e->getCode() == CameraEvent::kCodeFeatures ||
e->getCode() == CameraEvent::kCodeImageDepth)
{
this->addImage(e->image());
}
}
else if(event->getClassName().compare("OdometryEvent") == 0)
{
UDEBUG("OdometryEvent");
OdometryEvent * e = (OdometryEvent*)event;
if(e->isValid())
{
this->addImage(e->data());
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
@@ -200,10 +245,29 @@ void RtabmapThread::handleEvent(UEvent* event)
ULOGGER_DEBUG("CMD_DELETE_MEMORY");
pushNewState(kStateDeletingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdCleanSensorsBuffer)
else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_SENSORS_BUFFER");
pushNewState(kStateCleanSensorsBuffer);
ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
pushNewState(kStateCleanDataBuffer);
}
else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
{
ULOGGER_DEBUG("CMD_PUBLISH_MAP");
pushNewState(kStatePublishingMap);
}
else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
{
ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
pushNewState(kStateTriggeringMap);
}
else if(cmd == RtabmapEventCmd::kCmdPause)
{
ULOGGER_DEBUG("CMD_PAUSE");
_paused = !_paused;
}
else
{
UWARN("Cmd %d unknown!", cmd);
}
}
else if(event->getClassName().compare("ParamEvent") == 0)
@@ -233,28 +297,40 @@ void RtabmapThread::process()
void RtabmapThread::addImage(const Image & image)
{
if(image.empty())
if(!_paused)
{
ULOGGER_ERROR("image empty !?");
return;
}
bool notify = true;
_imageMutex.lock();
{
_imageBuffer.push_back(image);
while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
if(image.empty())
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
_imageBuffer.pop_front();
notify = false;
ULOGGER_ERROR("image empty !?");
return;
}
}
_imageMutex.unlock();
if(notify)
{
_imageAdded.release();
if(_rate>0.0f)
{
if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
{
return;
}
}
_frameRateTimer->start();
bool notify = true;
_imageMutex.lock();
{
_imageBuffer.push_back(image);
while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
_imageBuffer.pop_front();
notify = false;
}
}
_imageMutex.unlock();
if(notify)
{
_imageAdded.release();
}
}
}
corelib/src/Signature.cpp
+73 -11
View File
@@ -17,9 +17,10 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/util3d.h"
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/utilite/UtiLite.h>
@@ -34,31 +35,45 @@ Signature::~Signature()
Signature::Signature(
int id,
int mapId,
const std::multimap<int, cv::KeyPoint> & words,
const cv::Mat & image) :
const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose,
const std::vector<unsigned char> & depth2D, // in base_link frame
const std::vector<unsigned char> & image, // in camera_link frame
const std::vector<unsigned char> & depth, // in camera_link frame
float depthConstant,
const Transform & localTransform) :
_id(id),
_mapId(mapId),
_weight(0),
_saved(false),
_modified(true),
_neighborsModified(true),
_words(words),
_enabled(false),
_image(image)
_image(image),
_depth(depth),
_depth2D(depth2D),
_depthConstant(depthConstant),
_pose(pose),
_localTransform(localTransform),
_words3(words3)
{
}
void Signature::addNeighbors(const std::set<int> & neighbors)
void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
{
for(std::set<int>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
{
this->addNeighbor(*i);
this->addNeighbor(i->first, i->second);
}
}
void Signature::addNeighbor(int neighbor)
void Signature::addNeighbor(int neighbor, const Transform & transform)
{
UDEBUG("Add neighbor %d to %d", neighbor, this->id());
_neighbors.insert(neighbor);
_neighbors.insert(std::pair<int, Transform>(neighbor, transform));
_neighborsModified = true;
}
@@ -80,15 +95,47 @@ void Signature::removeNeighbors()
void Signature::changeNeighborIds(int idFrom, int idTo)
{
if(_neighbors.find(idFrom) != _neighbors.end())
std::map<int, Transform>::iterator iter = _neighbors.find(idFrom);
if(iter != _neighbors.end())
{
_neighbors.erase(idFrom);
_neighbors.insert(idTo);
Transform t = iter->second;
_neighbors.erase(iter);
_neighbors.insert(std::pair<int, Transform>(idTo, t));
_neighborsModified = true;
}
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
void Signature::addLoopClosureId(int loopClosureId, const Transform & transform)
{
if(loopClosureId && _loopClosureIds.insert(std::pair<int, Transform>(loopClosureId, transform)).second)
{
_neighborsModified=true;
}
}
void Signature::addChildLoopClosureId(int childLoopClosureId, const Transform & transform)
{
if(childLoopClosureId && _childLoopClosureIds.insert(std::pair<int, Transform>(childLoopClosureId, transform)).second)
{
_neighborsModified=true;
}
}
void Signature::changeLoopClosureId(int idFrom, int idTo)
{
std::map<int, Transform>::iterator iter = _loopClosureIds.find(idFrom);
if(iter != _loopClosureIds.end())
{
Transform t = iter->second;
_loopClosureIds.erase(iter);
_loopClosureIds.insert(std::pair<int, Transform>(idTo, t));
_neighborsModified = true;
}
UDEBUG("(%d) loop closure ids changed from %d to %d", _id, idFrom, idTo);
}
float Signature::compareTo(const Signature * s) const
{
float similarity = 0.0f;
@@ -109,12 +156,18 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
if(kps.size())
{
std::list<pcl::PointXYZ> pts = uValues(_words3, oldWordId);
_words.erase(oldWordId);
_words3.erase(oldWordId);
_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
{
_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
}
for(std::list<pcl::PointXYZ>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
{
_words3.insert(std::pair<int, pcl::PointXYZ>(activeWordId, (*iter)));
}
}
}
@@ -126,11 +179,20 @@ bool Signature::isBadSignature() const
void Signature::removeAllWords()
{
_words.clear();
_words3.clear();
}
void Signature::removeWord(int wordId)
{
_words.erase(wordId);
_words3.erase(wordId);
}
void Signature::setDepth(const std::vector<unsigned char> & depth, float depthConstant)
{
UASSERT_MSG(depth.empty() || (!depth.empty() && depthConstant > 0.0f), uFormat("depthConstant=%f",depthConstant).c_str());
_depth = depth;
_depthConstant=depthConstant;
}
} //namespace rtabmap
corelib/src/Statistics.cpp
+3 -11
View File
@@ -34,7 +34,9 @@ Statistics::Statistics() :
_extended(0),
_refImageId(0),
_loopClosureId(0),
_localLoopClosureId(0)
_localLoopClosureId(0),
_refDepthConstant(0),
_loopDepthConstant(0)
{
_defaultDataInitialized = true;
}
@@ -49,14 +51,4 @@ void Statistics::addStatistic(const std::string & name, float value)
uInsert(_data, std::pair<std::string, float>(name, value));
}
void Statistics::setRefImage(const cv::Mat & image)
{
_refImage = image;
}
void Statistics::setLoopImage(const cv::Mat & image)
{
_loopImage = image;
}
}
corelib/src/Transform.cpp
+190
View File
@@ -0,0 +1,190 @@
/*
* Transform.cpp
*
* Created on: 2013-08-30
* Author: Mathieu
*/
#include <rtabmap/core/Transform.h>
#include <pcl/common/eigen.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <iomanip>
namespace rtabmap {
Transform::Transform() : data_(12)
{
data_[0] = 0.0f;
data_[1] = 0.0f;
data_[2] = 0.0f;
data_[3] = 0.0f;
data_[4] = 0.0f;
data_[5] = 0.0f;
data_[6] = 0.0f;
data_[7] = 0.0f;
data_[8] = 0.0f;
data_[9] = 0.0f;
data_[10] = 0.0f;
data_[11] = 0.0f;
}
// rotation matrix r## and origin o##
Transform::Transform(float r11, float r12, float r13, float o14,
float r21, float r22, float r23, float o24,
float r31, float r32, float r33, float o34) :
data_(12)
{
data_[0] = r11;
data_[1] = r12;
data_[2] = r13;
data_[3] = o14;
data_[4] = r21;
data_[5] = r22;
data_[6] = r23;
data_[7] = o24;
data_[8] = r31;
data_[9] = r32;
data_[10] = r33;
data_[11] = o34;
}
Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
{
Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
*this = util3d::transformFromEigen3f(t);
}
bool Transform::isNull() const
{
return (data_[0] == 0.0f &&
data_[1] == 0.0f &&
data_[2] == 0.0f &&
data_[3] == 0.0f &&
data_[4] == 0.0f &&
data_[5] == 0.0f &&
data_[6] == 0.0f &&
data_[7] == 0.0f &&
data_[8] == 0.0f &&
data_[9] == 0.0f &&
data_[10] == 0.0f &&
data_[11] == 0.0f) ||
uIsNan(data_[0]) ||
uIsNan(data_[1]) ||
uIsNan(data_[2]) ||
uIsNan(data_[3]) ||
uIsNan(data_[4]) ||
uIsNan(data_[5]) ||
uIsNan(data_[6]) ||
uIsNan(data_[7]) ||
uIsNan(data_[8]) ||
uIsNan(data_[9]) ||
uIsNan(data_[10]) ||
uIsNan(data_[11]);
}
bool Transform::isIdentity() const
{
return data_[0] == 1.0f &&
data_[1] == 0.0f &&
data_[2] == 0.0f &&
data_[3] == 0.0f &&
data_[4] == 0.0f &&
data_[5] == 1.0f &&
data_[6] == 0.0f &&
data_[7] == 0.0f &&
data_[8] == 0.0f &&
data_[9] == 0.0f &&
data_[10] == 1.0f &&
data_[11] == 0.0f;
}
void Transform::setNull()
{
*this = Transform();
}
void Transform::setIdentity()
{
*this = getIdentity();
}
Transform Transform::getIdentity()
{
return Transform(1,0,0,0,
0,1,0,0,
0,0,1,0);
}
Transform Transform::inverse() const
{
Eigen::Matrix4f m = util3d::transformToEigen4f(*this);
return util3d::transformFromEigen4f(m.inverse());
}
Transform Transform::rotation() const
{
return Transform(data_[0], data_[1], data_[2], 0,
data_[4], data_[5], data_[6], 0,
data_[8], data_[9], data_[10], 0);
}
Transform Transform::translation() const
{
return Transform(1,0,0, data_[3],
0,1,0, data_[7],
0,0,1, data_[11]);
}
void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
{
pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
}
std::string Transform::prettyPrint() const
{
float x,y,z,roll,pitch,yaw;
getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
return uFormat("xyz=%f,%f,%f rpy=%f,%f,%f", x,y,z, roll,pitch,yaw);
}
Transform Transform::operator*(const Transform & t) const
{
Eigen::Matrix4f m1 = util3d::transformToEigen4f(*this);
Eigen::Matrix4f m2 = util3d::transformToEigen4f(t);
return util3d::transformFromEigen4f(m1*m2);
}
Transform & Transform::operator*=(const Transform & t)
{
*this = *this * t;
return *this;
}
bool Transform::operator==(const Transform & t) const
{
return memcmp(data_.data(), t.data_.data(), data_.size() * sizeof(float)) == 0;
}
bool Transform::operator!=(const Transform & t) const
{
return !(*this == t);
}
std::ostream& operator<<(std::ostream& os, const Transform& s)
{
for(int i = 0; i < 3; ++i)
{
for(int j = 0; j < 4; ++j)
{
std::cout << std::left << std::setw(12) << s.data()[i*4 + j];
}
std::cout << std::endl;
}
return os;
}
}
corelib/src/VWDictionary.cpp
+75 -94
View File
@@ -20,7 +20,7 @@
#include "rtabmap/core/VWDictionary.h"
#include "VisualWord.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "NearestNeighbor.h"
#include "rtabmap/core/Parameters.h"
@@ -37,7 +37,6 @@ const int VWDictionary::ID_START = 1;
const int VWDictionary::ID_INVALID = 0;
VWDictionary::VWDictionary(const ParametersMap & parameters) :
_lastNewWordsAddedCount(0),
_totalActiveReferences(0),
_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
_minDistUsed(Parameters::defaultKpMinDistUsed()),
@@ -66,26 +65,14 @@ VWDictionary::~VWDictionary()
void VWDictionary::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kKpMinDistUsed())) != parameters.end())
{
_minDistUsed = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpMinDist())) != parameters.end())
{
this->setMinDist(std::atof((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kKpNndrUsed())) != parameters.end())
{
_nndrUsed = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpNndrRatio())) != parameters.end())
{
this->setNndrRatio(std::atof((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kKpMaxLeafs())) != parameters.end())
{
_maxLeafs = (unsigned int)std::atoi((*iter).second.c_str());
}
Parameters::parse(parameters, Parameters::kKpMinDistUsed(), _minDistUsed);
Parameters::parse(parameters, Parameters::kKpMinDist(), _minDist);
Parameters::parse(parameters, Parameters::kKpNndrUsed(), _nndrUsed);
Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
Parameters::parse(parameters, Parameters::kKpMaxLeafs(), _maxLeafs);
UASSERT(_minDist >= 0.0f);
UASSERT(_nndrRatio >= 0.0f);
std::string dictionaryPath = _dictionaryPath;
bool incrementalDictionary = _incrementalDictionary;
@@ -183,6 +170,7 @@ void VWDictionary::setIncrementalDictionary(bool incrementalDictionary, const st
// laplacian not used
VisualWord * vw = new VisualWord(id, &(descriptor[0]), dimension, 0);
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
_notIndexedWords.insert(_notIndexedWords.end(), id);
}
else
{
@@ -272,30 +260,6 @@ VWDictionary::NNStrategy VWDictionary::nnStrategy() const
return strategy;
}
void VWDictionary::setMinDist(float d)
{
if(d < 0)
{
ULOGGER_ERROR("Match threshold must be positive (%f)", d);
}
else
{
_minDist = d;
}
}
void VWDictionary::setNndrRatio(float ratio)
{
if(ratio < 0)
{
ULOGGER_ERROR("Ratio must be positive (%f)", ratio);
}
else
{
_nndrRatio = ratio;
}
}
int VWDictionary::getLastIndexedWordId() const
{
if(_mapIndexId.size())
@@ -311,7 +275,7 @@ int VWDictionary::getLastIndexedWordId() const
void VWDictionary::update()
{
ULOGGER_DEBUG("");
if(!_incrementalDictionary && !_dataTree.empty())
if(!_incrementalDictionary && !_notIndexedWords.size())
{
// No need to update the search index if we
// use a fixed dictionary and the index is
@@ -319,59 +283,74 @@ void VWDictionary::update()
return;
}
_mapIndexId.clear();
if(_nn && _visualWords.size())
if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
{
UTimer timer;
timer.start();
_mapIndexId.clear();
_dataTree = cv::Mat();
if(!_dim)
if(_nn && _visualWords.size())
{
_dim = _visualWords.begin()->second->getDim();
}
UTimer timer;
timer.start();
// Create the kd-Tree
_dataTree = cv::Mat(_visualWords.size(), _dim, CV_32F); // SURF descriptors are CV_32F
std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
{
float * rowFl = _dataTree.ptr<float>(i);
if(iter->second->getDim() == _dim)
if(!_dim)
{
memcpy(rowFl, iter->second->getDescriptor(), _dim*sizeof(float));
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
_dim = _visualWords.begin()->second->getDim();
}
else
// Create the kd-Tree
_dataTree = cv::Mat(_visualWords.size(), _dim, CV_32F); // SURF descriptors are CV_32F
std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
{
ULOGGER_WARN("A word is not the same size than the dictionary, ignoring that word...");
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, 0)); // set to INVALID
float * rowFl = _dataTree.ptr<float>(i);
if(iter->second->getDim() == _dim)
{
memcpy(rowFl, iter->second->getDescriptor(), _dim*sizeof(float));
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
}
else
{
ULOGGER_WARN("A word is not the same size than the dictionary, ignoring that word...");
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, 0)); // set to INVALID
}
}
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), _dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
// Update the nearest neighbor algorithm
_nn->setData(_dataTree);
ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
}
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), _dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
// Update the nearest neighbor algorithm
_nn->setData(_dataTree);
ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
}
_lastNewWordsAddedCount = 0;
else
{
UINFO("Dictionary has not changed, so no need to update it!");
}
_notIndexedWords.clear();
_removedIndexedWords.clear();
}
void VWDictionary::clear()
{
ULOGGER_DEBUG("");
if(_visualWords.size() && _incrementalDictionary)
{
UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", _visualWords.size());
UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", (int)_visualWords.size());
}
if(_notIndexedWords.size())
{
UWARN("Not indexed words should be empty here (%d words still not indexed)", (int)_notIndexedWords.size());
}
for(std::map<int, VisualWord *>::iterator i=_visualWords.begin(); i!=_visualWords.end(); ++i)
{
delete (*i).second;
}
_visualWords.clear();
_lastNewWordsAddedCount = 0;
_notIndexedWords.clear();
_removedIndexedWords.clear();
_totalActiveReferences = 0;
_lastWordId = 0;
_dataTree = cv::Mat();
@@ -390,19 +369,12 @@ void VWDictionary::addWordRef(int wordId, int signatureId)
{
VisualWord * vw = 0;
vw = uValue(_visualWords, wordId, vw);
if(!vw)
{
vw = uValue(_unusedWords, wordId, vw);
if(vw)
{
_visualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
_unusedWords.erase(vw->id());
}
}
if(vw)
{
vw->addRef(signatureId);
_totalActiveReferences += 1;
_unusedWords.erase(vw->id());
}
else
{
@@ -447,7 +419,12 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
return wordIds;
}
int newWordsCount= 0;
if(!_incrementalDictionary && _dataTree.empty())
{
UERROR("Dictionary mode is set to fixed but no words are in it!");
return wordIds;
}
int dupWordsCount= 0;
unsigned int k=1; // k nearest neighbors
@@ -537,10 +514,10 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
{
VisualWord * vw = new VisualWord(getNextId(), newPts.ptr<float>(i), _dim, signatureId);
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
newWords.push_back(vw);
wordIds.push_back(vw->id());
UASSERT(vw->id()>0);
++newWordsCount;
}
else
{
@@ -602,9 +579,9 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
if(badDist)
{
++newWordsCount;
VisualWord * vw = new VisualWord(getNextId(), d, _dim, signatureId);
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
wordIds.push_back(vw->id());
UASSERT(vw->id()>0);
}
@@ -628,12 +605,11 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
ULOGGER_DEBUG("Naive search time = %fs", timer.ticks());
}
ULOGGER_DEBUG("%d new words added...", newWordsCount);
ULOGGER_DEBUG("%d new words added...", _notIndexedWords.size());
ULOGGER_DEBUG("%d duplicated words added...", dupWordsCount);
UDEBUG("total time %fs", timer.ticks());
_lastNewWordsAddedCount = newWordsCount;
_totalActiveReferences += newWordsCount;
_totalActiveReferences += _notIndexedWords.size();
return wordIds;
}
@@ -835,9 +811,10 @@ void VWDictionary::addWord(VisualWord * vw)
{
if(vw)
{
_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
_notIndexedWords.insert(vw->id());
if(vw->getReferences().size())
{
_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
_totalActiveReferences += uSum(uValues(vw->getReferences()));
}
else
@@ -1037,6 +1014,10 @@ void VWDictionary::removeWords(const std::vector<VisualWord*> & words)
{
_visualWords.erase(words[i]->id());
_unusedWords.erase(words[i]->id());
if(_notIndexedWords.erase(words[i]->id()) == 0)
{
_removedIndexedWords.insert(words[i]->id());
}
}
}
corelib/src/random_sample.h
+255
View File
@@ -0,0 +1,255 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* 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 copyright holder(s) 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 OWNER 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.
*
* $Id: extract_indices.h 1370 2011-06-19 01:06:01Z jspricke $
*
*/
#ifndef PCL_FILTERS_RANDOM_SUBSAMPLE_H_
#define PCL_FILTERS_RANDOM_SUBSAMPLE_H_
#include <pcl/filters/filter_indices.h>
#include <time.h>
#include <limits.h>
/** \brief @b RandomSample applies a random sampling with uniform probability.
* Based off Algorithm A from the paper "Faster Methods for Random Sampling"
* by Jeffrey Scott Vitter. The algorithm runs in O(N) and results in sorted
* indices
* http://www.ittc.ku.edu/~jsv/Papers/Vit84.sampling.pdf
* \author Justin Rosen
* \ingroup filters
*/
template<typename PointT>
class RandomSample : public pcl::FilterIndices<PointT>
{
using pcl::FilterIndices<PointT>::filter_name_;
using pcl::FilterIndices<PointT>::getClassName;
using pcl::FilterIndices<PointT>::indices_;
using pcl::FilterIndices<PointT>::input_;
using pcl::FilterIndices<PointT>::negative_;
using pcl::FilterIndices<PointT>::keep_organized_;
using pcl::FilterIndices<PointT>::user_filter_value_;
using pcl::FilterIndices<PointT>::extract_removed_indices_;
using pcl::FilterIndices<PointT>::removed_indices_;
typedef typename pcl::FilterIndices<PointT>::PointCloud PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
typedef typename PointCloud::ConstPtr PointCloudConstPtr;
public:
typedef boost::shared_ptr< RandomSample<PointT> > Ptr;
typedef boost::shared_ptr< const RandomSample<PointT> > ConstPtr;
/** \brief Empty constructor. */
RandomSample (bool extract_removed_indices = false) :
pcl::FilterIndices<PointT> (extract_removed_indices),
sample_ (UINT_MAX),
seed_ (static_cast<unsigned int> (time (NULL)))
{
filter_name_ = "RandomSample";
}
/** \brief Set number of indices to be sampled.
* \param sample
*/
inline void
setSample (unsigned int sample)
{
sample_ = sample;
}
/** \brief Get the value of the internal \a sample parameter.
*/
inline unsigned int
getSample ()
{
return (sample_);
}
/** \brief Set seed of random function.
* \param seed
*/
inline void
setSeed (unsigned int seed)
{
seed_ = seed;
}
/** \brief Get the value of the internal \a seed parameter.
*/
inline unsigned int
getSeed ()
{
return (seed_);
}
protected:
/** \brief Number of indices that will be returned. */
unsigned int sample_;
/** \brief Random number seed. */
unsigned int seed_;
/** \brief Sample of point indices into a separate PointCloud
* \param output the resultant point cloud
*/
void
applyFilter (PointCloud &output)
{
std::vector<int> indices;
if (keep_organized_)
{
bool temp = extract_removed_indices_;
extract_removed_indices_ = true;
applyFilter (indices);
extract_removed_indices_ = temp;
copyPointCloud (*input_, output);
// Get X, Y, Z fields
std::vector<sensor_msgs::PointField> fields;
pcl::getFields (*input_, fields);
std::vector<size_t> offsets;
for (size_t i = 0; i < fields.size (); ++i)
{
if (fields[i].name == "x" ||
fields[i].name == "y" ||
fields[i].name == "z")
offsets.push_back (fields[i].offset);
}
// For every "removed" point, set the x,y,z fields to user_filter_value_
const static float user_filter_value = user_filter_value_;
for (size_t rii = 0; rii < removed_indices_->size (); ++rii)
{
uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output[(*removed_indices_)[rii]]);
for (size_t i = 0; i < offsets.size (); ++i)
{
memcpy (pt_data + offsets[i], &user_filter_value, sizeof (float));
}
if (!pcl_isfinite (user_filter_value_))
output.is_dense = false;
}
}
else
{
output.is_dense = true;
applyFilter (indices);
copyPointCloud (*input_, indices, output);
}
}
/** \brief Sample of point indices
* \param indices the resultant point cloud indices
*/
void
applyFilter (std::vector<int> &indices)
{
unsigned N = static_cast<unsigned> (indices_->size ());
unsigned int sample_size = negative_ ? N - sample_ : sample_;
// If sample size is 0 or if the sample size is greater then input cloud size
// then return all indices
if (sample_size >= N)
{
indices = *indices_;
removed_indices_->clear ();
}
else
{
// Resize output indices to sample size
indices.resize (static_cast<size_t> (sample_size));
if (extract_removed_indices_)
removed_indices_->resize (static_cast<size_t> (N - sample_size));
// Set random seed so derived indices are the same each time the filter runs
std::srand (seed_);
// Algorithm A
unsigned top = N - sample_size;
unsigned i = 0;
unsigned index = 0;
std::vector<bool> added;
if (extract_removed_indices_)
added.resize (indices_->size (), false);
for (size_t n = sample_size; n >= 2; n--)
{
float V = unifRand ();
unsigned S = 0;
float quot = static_cast<float> (top) / static_cast<float> (N);
while (quot > V)
{
S++;
top--;
N--;
quot = quot * static_cast<float> (top) / static_cast<float> (N);
}
index += S;
if (extract_removed_indices_)
added[index] = true;
indices[i++] = (*indices_)[index++];
N--;
}
index += N * static_cast<unsigned> (unifRand ());
if (extract_removed_indices_)
added[index] = true;
indices[i++] = (*indices_)[index++];
// Now populate removed_indices_ appropriately
if (extract_removed_indices_)
{
unsigned ri = 0;
for (size_t i = 0; i < added.size (); i++)
{
if (!added[i])
{
(*removed_indices_)[ri++] = (*indices_)[i];
}
}
}
}
}
/** \brief Return a random number fast using a LCG (Linear Congruential Generator) algorithm.
* See http://software.intel.com/en-us/articles/fast-random-number-generator-on-the-intel-pentiumr-4-processor/ for more information.
*/
inline float
unifRand ()
{
return (static_cast<float>(rand () / double (RAND_MAX)));
//return (((214013 * seed_ + 2531011) >> 16) & 0x7FFF);
}
};
#endif //#ifndef PCL_FILTERS_RANDOM_SUBSAMPLE_H_
corelib/src/resources/DatabaseSchema.sql
+24 -10
View File
@@ -15,18 +15,32 @@
-- *******************************************************************
CREATE TABLE Node (
id INTEGER NOT NULL,
map_id INTEGER NOT NULL,
weight INTEGER,
pose BLOB,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE MapLink (
source_map_id INTEGER NOT NULL,
target_map_id INTEGER NOT NULL,
transform BLOB
);
CREATE TABLE Image (
id INTEGER NOT NULL,
raw_width INTEGER NOT NULL,
raw_height INTEGER NOT NULL,
raw_data_type INTEGER NOT NULL,
raw_compressed CHAR NOT NULL,
raw_data BLOB,
data BLOB,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Depth (
id INTEGER NOT NULL,
data BLOB, -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
constant FLOAT,
local_transform BLOB,
data2d BLOB, -- CV_32FC2, Example: Laser scan
time_enter DATE,
PRIMARY KEY (id)
);
@@ -35,6 +49,7 @@ CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
transform BLOB,
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
);
@@ -56,6 +71,9 @@ CREATE TABLE Map_Node_Word (
size INTEGER NOT NULL,
dir FLOAT NOT NULL,
response FLOAT NOT NULL,
depth_x FLOAT,
depth_y FLOAT,
depth_z FLOAT,
FOREIGN KEY (node_id) REFERENCES Node(id),
FOREIGN KEY (word_id) REFERENCES Word(id)
);
@@ -65,14 +83,10 @@ CREATE TABLE Statistics (
last_sign_added INTEGER,
process_mem_used INTEGER,
database_mem_used INTEGER,
dictionary_size INTEGER,
time_enter DATE
);
CREATE TABLE StatisticsDictionary (
dictionary_size INTEGER,
time_enter DATE
);
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
corelib/src/toro3d/dmatrix.hh
+94
View File
@@ -0,0 +1,94 @@
#ifndef DMATRIX_HXX
#define DMATRIX_HXX
#include <iostream>
#include <exception>
class DNotInvertibleMatrixException: public std::exception {};
class DIncompatibleMatrixException: public std::exception {};
class DNotSquareMatrixException: public std::exception {};
template <class X> struct DVector{
public:
DVector(int n=0);
~DVector();
DVector(const DVector&);
DVector& operator=(const DVector&);
X& operator[](int i) {
if ((*shares)>1) detach();
return elems[i];
}
const X& operator[](int i) const { return elems[i]; }
X operator*(const DVector&) const;
DVector operator+(const DVector&) const;
DVector operator-(const DVector&) const;
DVector operator*(const X&) const;
int dim() const { return size; }
void detach();
static DVector<X> I(int);
protected:
X * elems;
int size;
int * shares;
};
template <class X> class DMatrix {
public:
DMatrix(int n=0,int m=0);
~DMatrix();
DMatrix(const DMatrix&);
DMatrix& operator=(const DMatrix&);
X * operator[](int i) {
if ((*shares)>1) detach();
return mrows[i];
}
const X * operator[](int i) const { return mrows[i]; }
const X det() const;
DMatrix inv() const;
DMatrix transpose() const;
DMatrix operator*(const DMatrix&) const;
DMatrix operator+(const DMatrix&) const;
DMatrix operator-(const DMatrix&) const;
DMatrix operator*(const X&) const;
int rows() const { return nrows; }
int columns() const { return ncols; }
void detach();
static DMatrix I(int);
protected:
X * elems;
int nrows,ncols;
X ** mrows;
int * shares;
};
template <class X> DVector<X> operator * (const DMatrix<X> m, const DVector<X> v);
template <class X> DVector<X> operator * (const DVector<X> v, const DMatrix<X> m);
/*************** IMPLEMENTATION ***************/
#include "dmatrix.hxx"
#endif
corelib/src/toro3d/dmatrix.hxx
+289
View File
@@ -0,0 +1,289 @@
template <class X> DVector<X>::DVector(int n) {
if (n<1) n=1;
size=n;
elems=new X[size];
for (int i=0;i<size; i++)
elems[i]=X(0);
shares=new int;
(*shares)=1;
}
template <class X> DVector<X>::~DVector() {
if (--(*shares)) return;
delete [] elems;
delete shares;
}
template <class X> DVector<X>::DVector(const DVector<X>& m) {
shares=m.shares;
elems=m.elems;
size=m.size;
(*shares)++;
}
template <class X> DVector<X>& DVector<X>::operator=(const DVector<X>& m) {
if (shares==m.shares)
return *this;
if (!--(*shares)) {
delete [] elems;
delete shares;
}
shares=m.shares;
elems=m.elems;
size=m.size;
(*shares)++;
return *this;
}
template <class X> X DVector<X>::operator*(const DVector<X>& v) const{
if (size!=v.size) throw DIncompatibleMatrixException();
X p=X(0);
for (int i=0; i<size; i++)
p+=elems[i]*v.elems[i];
return p;
}
template <class X> void DVector<X>::detach() {
DVector<X> aux(size);
for (int i=0;i<size;i++) aux.elems[i]=elems[i];
operator=(aux);
}
template <class X> DVector<X> DVector<X>::operator+(const DVector<X>& v) const{
if (size!=v.size) throw DIncompatibleMatrixException();
DVector<X> r(size);
for (int i=0; i<size; i++){
r.elems[i]=elems[i]+v.elems[i];
}
return r;
}
template <class X> DVector<X> DVector<X>::operator-(const DVector<X>& v) const{
if (size!=v.size) throw DIncompatibleMatrixException();
DVector<X> r(size);
for (int i=0; i<size; i++){
r.elems[i]=elems[i]-v.elems[i];
}
return r;
}
template <class X> DVector<X> DVector<X>::operator*(const X& d) const{
DVector<X> r(size);
for (int i=0; i<size; i++){
r.elems[i]=elems[i]*d;
}
return r;
}
template <class X> DMatrix<X>::DMatrix(int n,int m) {
if (n<1) n=1;
if (m<1) m=1;
nrows=n;
ncols=m;
elems=new X[nrows*ncols];
mrows=new X* [nrows];
for (int i=0;i<nrows;i++) mrows[i]=elems+ncols*i;
for (int i=0;i<nrows*ncols;i++) elems[i]=X(0);
shares=new int;
(*shares)=1;
}
template <class X> DMatrix<X>::~DMatrix() {
if (--(*shares)) return;
delete [] elems;
delete [] mrows;
delete shares;
}
template <class X> DMatrix<X>::DMatrix(const DMatrix& m) {
shares=m.shares;
elems=m.elems;
nrows=m.nrows;
ncols=m.ncols;
mrows=m.mrows;
(*shares)++;
}
template <class X> DMatrix<X>& DMatrix<X>::operator=(const DMatrix& m) {
if (shares==m.shares)
return *this;
if (!--(*shares)) {
delete [] elems;
delete [] mrows;
delete shares;
}
shares=m.shares;
elems=m.elems;
nrows=m.nrows;
ncols=m.ncols;
mrows=m.mrows;
(*shares)++;
return *this;
}
template <class X> DMatrix<X> DMatrix<X>::inv() const {
if (nrows!=ncols) throw DNotInvertibleMatrixException();
DMatrix<X> aux1(*this),aux2(I(nrows));
aux1.detach();
for (int i=0;i<nrows;i++) {
int k=i;
for (;k<nrows&&aux1.mrows[k][i]==X(0);k++){};
if (k>=nrows) throw DNotInvertibleMatrixException();
X val=aux1.mrows[k][i];
for (int j=0;j<nrows;j++) {
aux1.mrows[k][j]=aux1.mrows[k][j]/val;
aux2.mrows[k][j]=aux2.mrows[k][j]/val;
}
if (k!=i) {
for (int j=0;j<nrows;j++) {
X tmp=aux1.mrows[k][j];
aux1.mrows[k][j]=aux1.mrows[i][j];
aux1.mrows[i][j]=tmp;
tmp=aux2.mrows[k][j];
aux2.mrows[k][j]=aux2.mrows[i][j];
aux2.mrows[i][j]=tmp;
}
}
for (int j=0;j<nrows;j++)
if (j!=i) {
X tmp=aux1.mrows[j][i];
for (int l=0;l<nrows;l++) {
aux1.mrows[j][l]=aux1.mrows[j][l]-tmp*aux1.mrows[i][l];
aux2.mrows[j][l]=aux2.mrows[j][l]-tmp*aux2.mrows[i][l];
}
}
}
return aux2;
}
template <class X> const X DMatrix<X>::det() const {
if (nrows!=ncols) throw DNotSquareMatrixException();
DMatrix<X> aux(*this);
X d=X(1);
aux.detach();
for (int i=0;i<nrows;i++) {
int k=i;
for (;k<nrows&&aux.mrows[k][i]==X(0);k++){};
if (k>=nrows) return X(0);
X val=aux.mrows[k][i];
for (int j=0;j<nrows;j++) {
aux.mrows[k][j]/=val;
}
d=d*val;
if (k!=i) {
for (int j=0;j<nrows;j++) {
X tmp=aux.mrows[k][j];
aux.mrows[k][j]=aux.mrows[i][j];
aux.mrows[i][j]=tmp;
}
d=-d;
}
for (int j=i+1;j<nrows;j++){
X tmp=aux.mrows[j][i];
if (!(tmp==X(0)) ){
for (int l=0;l<nrows;l++) {
aux.mrows[j][l]=aux.mrows[j][l]-tmp*aux.mrows[i][l];
}
//d=d*tmp;
}
}
}
return d;
}
template <class X> DMatrix<X> DMatrix<X>::transpose() const {
DMatrix<X> aux(ncols, nrows);
for (int i=0; i<nrows; i++)
for (int j=0; j<ncols; j++)
aux[j][i]=mrows[i][j];
return aux;
}
template <class X> DMatrix<X> DMatrix<X>::operator*(const DMatrix<X>& m) const {
if (ncols!=m.nrows) throw DIncompatibleMatrixException();
DMatrix<X> aux(nrows,m.ncols);
for (int i=0;i<nrows;i++)
for (int j=0;j<m.ncols;j++){
X a=0;
for (int k=0;k<ncols;k++)
a+=mrows[i][k]*m.mrows[k][j];
aux.mrows[i][j]=a;
}
return aux;
}
template <class X> DMatrix<X> DMatrix<X>::operator+(const DMatrix<X>& m) const {
if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
DMatrix<X> aux(nrows,ncols);
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]+m.elems[i];
return aux;
}
template <class X> DMatrix<X> DMatrix<X>::operator-(const DMatrix<X>& m) const {
if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
DMatrix<X> aux(nrows,ncols);
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]-m.elems[i];
return aux;
}
template <class X> DMatrix<X> DMatrix<X>::operator*(const X& e) const {
DMatrix<X> aux(nrows,ncols);
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]*e;
return aux;
}
template <class X> void DMatrix<X>::detach() {
DMatrix<X> aux(nrows,ncols);
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i];
operator=(aux);
}
template <class X> DMatrix<X> DMatrix<X>::I(int n) {
DMatrix<X> aux(n,n);
for (int i=0;i<n;i++) aux[i][i]=X(1);
return aux;
}
template <class X> std::ostream& operator<<(std::ostream& os, const DMatrix<X> &m) {
os << "{";
for (int i=0;i<m.rows();i++) {
if (i>0) os << ",";
os << "{";
for (int j=0;j<m.columns();j++) {
if (j>0) os << ",";
os << m[i][j];
}
os << "}";
}
return os << "}";
}
template <class X> DVector<X> operator * (const DMatrix<X> m, const DVector<X> v){
if (v.dim()!=m.columns()) throw DIncompatibleMatrixException();
DVector<X> r(m.rows());
for (int i=0; i<m.rows(); i++){
X a=X(0);
for (int j=0; j<m.columns(); j++){
a+=m[i][j]*v[j];
}
r[i]=a;
}
return r;
}
template <class X> DVector<X> operator * (const DVector<X> v, const DMatrix<X> m){
if (v.dim()!=m.rows()) throw DIncompatibleMatrixException();
DVector<X> r(m.columns());
for (int i=0; i<m.columns(); i++){
X a=X(0);
for (int j=0; j<m.rows(); j++){
a+=m[j][i]*v[j];
}
r[i]=a;
}
return r;
}
corelib/src/toro3d/posegraph.hh
+274
View File
@@ -0,0 +1,274 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file posegraph.hh
*
* \brief The template class for the node parameters. The graph of
* poses with support to tree construction functionalities.
**/
#ifndef _TREEPOSEGRAPH_HXX_
#define _TREEPOSEGRAPH_HXX_
#include <iostream>
#include <assert.h>
#include <set>
#include <list>
#include <map>
#include <deque>
#include <vector>
#include <limits>
#include <algorithm>
namespace AISNavigation{
/** \brief A comparator class (struct) that compares the level
of two vertices if edges **/
template <class E>
struct EVComparator{
/** Comparison operator for the level **/
enum CompareMode {CompareLevel, CompareLength};
CompareMode mode;
EVComparator(){
mode=CompareLevel;
}
inline bool operator() (const E& e1, const E& e2){
int o1=0, o2=0;
switch (mode){
case CompareLevel:
o1=e1->top->level;
o2=e2->top->level;
break;
case CompareLength:
o1=e1->length;
o2=e2->length;
break;
}
return o1<o2;
}
};
/** \brief The template class for representing an abstract tree
without specifing the dimensionality of the exact parameterization
of the nodes. This definition is passed in via the Operation (Ops)
template class **/
template <class Ops>
struct TreePoseGraph{
typedef typename Ops::BaseType BaseType;
typedef typename Ops::PoseType Pose;
typedef typename Ops::RotationType Rotation;
typedef typename Ops::TranslationType Translation;
typedef typename Ops::TransformationType Transformation;
typedef typename Ops::CovarianceType Covariance;
typedef typename Ops::InformationType Information;
typedef typename Ops::ParametersType Parameters;
struct Vertex;
/** \brief Definition of an edge in the graph based on the template
input from Ops **/
struct Edge{
Vertex* v1; /**< The constraint is defined between v1 and v2 **/
Vertex* v2; /**< The constraint is defined between v1 and v2 **/
Vertex* top; /**< The node with the smallest level in the path **/
int length; /**< Length of the path on the tree (number of vertieces involved) **/
Transformation transformation; /**< Transformation describing the constraint (relative mapping) **/
Information informationMatrix; /**< Uncertainty encoded in the information matrix **/
bool mark;
double learningRate;
};
typedef typename EVComparator<Edge*>::CompareMode EdgeCompareMode;
typedef typename std::list< Edge* > EdgeList;
typedef typename std::map< int, Vertex* > VertexMap;
typedef typename std::set< Vertex* > VertexSet;
typedef typename std::map< Edge*, Edge* > EdgeMap;
typedef typename std::multiset< Edge*, EVComparator<Edge*> > EdgeSet;
/** \brief Definition of a vertex in the graph based on the
template input from Ops **/
struct Vertex {
// Graph-related elements
int id; /**< Id of the vertex in the graph **/
EdgeList edges; /**< The edges related to this vertex **/
// Tree-related elements
int level; /**< level in the tree. It is the distance on the tree to the root **/
Vertex* parent; /**< Parent vertex **/
Edge* parentEdge; /**< Constraint between the parent and the current vertex in the tree **/
EdgeList children; /**< All constraints involving the children of this vertex **/
// Parameterization-related elements
Transformation transformation; /**< redundant representation of the vertex, without gymbal locks **/
Pose pose; /**< The pose of the vertex **/
Parameters parameters; /**< The parameter representation **/
bool mark;
};
/** Returns the vertex with the given id **/
Vertex* vertex(int id);
/** Returns a const pointer to the vertex with the given id **/
const Vertex* vertex (int id) const;
/** Returns the edge between the two vertices **/
Edge* edge(int id1, int id2);
/** Returns a const pointer tothe edge between the two vertices **/
const Edge* edge(int id1, int id2) const;
/** Add a vertex to the graph **/
Vertex* addVertex(int id, const Pose& pose);
/** Remove a vertex from the graph **/
Vertex* removeVertex (int id);
/** Add an edge/constraint to the graph **/
Edge* addEdge(Vertex* v1, Vertex* v2, const Transformation& t, const Information& i);
/** Remove an edge/constraint from the graph **/
Edge* removeEdge(Edge* eq);
/** Adds en edge incrementally to the tree.
It builds a simple tree and initializes the structures for the optimization.
This function is for online processing.
It requires that at least one vertex is already present in the graph.
The vertices are represented by their ids.
Once the edge is introduced in the structure:
- the parent of the node with the higher ID is computed.
- the top node is assigned
- the edge is inserted in the
@returns A pointer to the added edge, if the insertion was succesfull. 0 otherwise.
**/
Edge* addIncrementalEdge(int id1, int id2, const Transformation& t, const Information& i);
/** Returns a set of edges which are accected by the mofification of the vertex v.
The set is ordered according to the level of their top node.
**/
EdgeSet* affectedEdges(Vertex* v);
EdgeSet* affectedEdges(VertexSet& vl);
/** Function to perform a breadth-first visit of the nodes in the tree to carry out a specific action act**/
template <class Action>
void treeBreadthVisit(Action& act);
/** Function to perform a depth-first visit of the nodes in the tree to carry out a specific action act **/
template <class Action>
void treeDepthVisit(Action& act, Vertex *v);
/** Constructs the tree be computing a minimal spanning tree **/
bool buildMST(int id);
/** Constructs the incremental tree according to the input trajectory **/
bool buildSimpleTree();
/** Trun around an edge (used to ensure a certain oder on the vertexes) **/
void revertEdge(Edge* e);
/** Revert edge info. This function needs to be implemented by a subclass **/
virtual void revertEdgeInfo(Edge* e) = 0;
/** Revert edge info. This function needs to be implemented by a subclass **/
virtual void initializeFromParentEdge(Vertex* v) = 0;
/** Delete all edges and vertices **/
void clear();
/**constructor*/
TreePoseGraph(){
sortedEdges=0;
edgeCompareMode=EVComparator<Edge*>::CompareLevel;
}
/** Destructor **/
virtual ~TreePoseGraph();
/** Sort constraints for correct processing order **/
EdgeSet* sortEdges();
/** Determines the length of the longest path in the tree **/
int maxPathLength();
/** Determines the path length of all pathes in the tree **/
int totalPathLength();
/** remove gaps in the indices of the vertex ids **/
void compressIndices();
/** compute the highest index of an vertex **/
int maxIndex();
/** performs a consistency check on the tree and the graph structure.
@returns false on failure.*/
bool sanityCheck();
/** The root node of the tree **/
Vertex* root;
/** All vertices **/
VertexMap vertices;
/** All edges **/
EdgeMap edges;
/** The constraints/edges sorted according to the level in the tree
in order to allow us the efficient update (pose computation) of
the nodes in the tree (see the RSS07 paper for further
details) **/
EdgeSet* sortedEdges;
protected:
void fillEdgeInfo(Edge* e);
void fillEdgesInfo();
EdgeCompareMode edgeCompareMode;
};
//include the template implementation part
#include "posegraph.hxx"
}; //namespace AISNavigation
#endif
corelib/src/toro3d/posegraph.hxx
+693
View File
@@ -0,0 +1,693 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file posegraph.hxx
*
* \brief The implementation of the template class for the node
* parameters.
**/
/*********************** IMPLEMENTATION PART ***********************/
template <typename Ops>
typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::vertex(int id){
typename VertexMap::iterator it=vertices.find(id);
if (it==vertices.end())
return 0;
return it->second;
}
template <typename Ops>
const typename TreePoseGraph<Ops>::Vertex * TreePoseGraph<Ops>::vertex (int id) const{
typename VertexMap::const_iterator it=vertices.find(id);
if (it==edges.end())
return 0;
return it->second;
}
template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::edge(int id1, int id2){
Vertex* v1=vertex(id1);
if (!v1)
return false;
typename EdgeList::iterator it=v1->edges.begin();
while(it!=v1->edges.end()){
if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
return *it;
it++;
}
return 0;
}
template <class Ops>
const typename TreePoseGraph<Ops>::Edge * TreePoseGraph<Ops>::edge(int id1, int id2) const{
const Vertex* v1=vertex(id1);
if (!v1)
return false;
typename EdgeList::const_iterator it=v1->edges.begin();
while(it!=v1->edges.end()){
if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
return *it;
it++;
}
return 0;
}
template <class Ops>
void TreePoseGraph<Ops>::revertEdge(typename TreePoseGraph<Ops>::Edge * e){
revertEdgeInfo(e);
Vertex* ap=e->v2;
e->v2=e->v1;
e->v1=ap;
}
template <class Ops>
typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::addVertex(int id, const typename TreePoseGraph<Ops>::Pose& pose){
Vertex* v=vertex(id);
if (v)
return 0;
v=new Vertex;
v->id=id;
v->pose=pose;
v->parent=0;
v->mark=false;
vertices.insert(std::make_pair(id,v));
return v;
}
template <class Ops>
typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::removeVertex (int id){
typename VertexMap::iterator it=vertices.find(id);
if (it==vertices.end())
return 0;
Vertex* v=it->second;
if (v==0)
return false;
typename TreePoseGraph<Ops>::EdgeList el=v->edges;
for(typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
removeEdge(*it);
}
delete v;
vertices.erase(it);
return v;
}
template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::addEdge(typename TreePoseGraph<Ops>::Vertex* v1, typename TreePoseGraph<Ops>::Vertex* v2,
const typename TreePoseGraph<Ops>::Transformation& t, const typename TreePoseGraph<Ops>::Information& i){
if (v1==v2)
return 0;
Edge* e=edge(v1->id, v2->id);
if (e)
return 0;
e=new Edge;
e->mark=false;
e->v1=v1;
e->v2=v2;
e->top=0;
e->transformation=t;
e->informationMatrix=i;
v1->edges.push_back(e);
v2->edges.push_back(e);
edges.insert(std::make_pair(e,e));
return e;
}
template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::addIncrementalEdge(int id1, int id2,
const typename TreePoseGraph<Ops>::Transformation& t, const typename TreePoseGraph<Ops>::Information& i){
EVComparator<Edge*> comp;
comp.mode=edgeCompareMode;
if (! sortedEdges)
sortedEdges=new EdgeSet(comp);
typename VertexMap::iterator it1=vertices.find(id1);
typename VertexMap::iterator it2=vertices.find(id2);
Vertex* v1, *v2, *addedVertex=0;
if (it1==vertices.end() && it2==vertices.end()){
return 0;
}
if (it1==vertices.end()){
typename TreePoseGraph<Ops>::Pose p;
v1=addedVertex=addVertex(id1,p);
} else {
v1=it1->second;
}
if (it2==vertices.end()){
typename TreePoseGraph<Ops>::Pose p;
v2=addedVertex=addVertex(id2,p);
} else {
v2=it2->second;
}
if (v1->id==v2->id){
assert(0);
}
Edge* e=addEdge(v1,v2,t,i);
if (!e){
return 0;
}
if (v1->id>v2->id)
revertEdge(e);
if (addedVertex){
Vertex* otherVertex= (addedVertex==v1)? v2:v1;
addedVertex->parent=otherVertex;
addedVertex->parentEdge=e;
addedVertex->level=otherVertex->level+1;
otherVertex->children.push_back(e);
}
fillEdgeInfo(e);
sortedEdges->insert(e);
if (addedVertex){
initializeFromParentEdge(addedVertex);
}
return e;
}
template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::removeEdge(typename TreePoseGraph<Ops>::Edge* e){
{
typename EdgeMap::iterator it=edges.find(e);
if (it==edges.end()){
return 0;
}
edges.erase(it);
}
Vertex* v1=e->v1;
Vertex* v2=e->v2;
{
typename EdgeList::iterator it=v1->edges.begin();
while(it!=v1->edges.end()){
if (*it==e){
v1->edges.erase(it);
break;
}
it++;
}
}
{
typename EdgeList::iterator it=v2->edges.begin();
while(it!=v2->edges.end()){
if ((*it)==e){
delete *it;
v2->edges.erase(it);
break;
}
it++;
}
}
return e;
}
template <class Ops>
template <class Action>
void TreePoseGraph<Ops>::treeBreadthVisit(Action& act){
typedef std::deque<Vertex*> VertexDeque;
static VertexDeque q;
q.push_back(root);
while (!q.empty()){
Vertex* current=q.front();
act.perform(current);
q.pop_front();
typename EdgeList::iterator it=current->children.begin();
while(it!=current->children.end()){
typename TreePoseGraph::Edge* e=(*it);
q.push_back(e->v2);
if(e->v2==current){
std::cerr << "error in the link direction v=" << current->id << std::endl;
std::cerr << " v1=" << e->v1->id << " v2=" << e->v2->id << std::endl;
assert(0);
}
it++;
}
}
q.clear();
}
template <class Ops>
template <class Action>
void TreePoseGraph<Ops>::treeDepthVisit(Action& act, Vertex* v){
act.perform(v);
typename EdgeList::iterator it=v->children.begin();
while(it!=v->children.end()){
treeDepthVisit(act, (*it)->v2);
it++;
}
}
template <class Ops>
bool TreePoseGraph<Ops>::buildMST(int id){
typedef std::deque<Vertex*> VertexDeque;
typename VertexMap::iterator it=vertices.begin();
while (it!=vertices.end()){
it->second->parent=0;
it->second->parentEdge=0;
it->second->children.clear();
it++;
}
Vertex* v=vertex(id);
if (!v)
return false;
root=v;
root->level=0;
VertexDeque q;
q.push_back(v);
//std::cerr << "v=" << v->id << std::endl;
while (!q.empty()){
v=q.front();
typename EdgeList::iterator it=v->edges.begin();
while (it!=v->edges.end()){
Edge* e=(*it);
bool invertedEdge=false;
Vertex* other=e->v2;
if (other==v){
other=e->v1;
invertedEdge=true;
}
if (other!=root && other->parent==0){
if (invertedEdge){
revertEdge(e);
}
//std::cerr << "INSERT v=" << v->id<< " " << "e=(" << e->v1->id << "," << e->v2->id << ")" << std::endl;
other->parent=v;
other->parentEdge=e;
other->level=v->level+1;
q.push_back(other);
v->children.push_back(e);
//std::cerr << "v=" << other->id << std::endl;
}
it++;
}
q.pop_front();
}
fillEdgesInfo();
return true;
}
/** \brief A class (struct) to dermine the level of a vertex in the tree **/
template <class TPG>
struct LevelAssigner{
/** Dermines the level of the vertex v in the tree **/
void perform(typename TPG::Vertex* v){
if (v->parent)
v->level=v->parent->level+1;
else
v->level=0;
}
};
template <class Ops>
bool TreePoseGraph<Ops>::buildSimpleTree(){
root=0;
//rectify all the constraints, so that the v1<v2
for (typename EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
Edge* e=it->second;
if (e->v1->id > e->v2->id)
revertEdge(e);
}
//clear the tree data
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->parent=0;
v->parentEdge=0;
v->children.clear();
}
//fill the structure
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
if (v->edges.empty()){
assert(0);
continue;
}
Edge* bestEdge=v->edges.front();
int bestId=std::numeric_limits<int>::max();
bool found=false;
typename EdgeList::iterator li=v->edges.begin();
while(li!=v->edges.end()){
Edge* e =*li;
if (e->v2==v && e->v1->id<bestId){ //consider only the entering edges
bestId=e->v1->id;
bestEdge=e;
found=true;
}
li++;
}
if (found){
v->parentEdge=bestEdge;
v->parent=bestEdge->v1;
v->parent->children.push_back(bestEdge);
} else {
assert(! root);
root=v;
}
}
// std::cerr << "root=" << root << std::endl;
assert(root);
//assign the level
LevelAssigner< TreePoseGraph<Ops> > oa;
treeDepthVisit(oa, root);
fillEdgesInfo();
return true;
}
template <class Ops>
TreePoseGraph<Ops>::~TreePoseGraph(){
clear();
}
template <class Ops>
void TreePoseGraph<Ops>::clear(){
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
delete it->second;
it->second=0;
}
for (typename EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
delete it->second;
it->second=0;
}
vertices.clear();
edges.clear();
if ( sortedEdges )
delete sortedEdges;
sortedEdges=0;
}
template <class Ops>
void TreePoseGraph<Ops>::fillEdgeInfo(Edge* e){
Vertex* v1=e->v1;
Vertex* v2=e->v2;
int length=0;
while (v1!=v2) {
if (v1->level > v2->level){
v1=v1->parent;
length++;
} else if (v2->level > v1->level){
v2=v2->parent;
length++;
} else if (v1->level==v2->level){
v1=v1->parent;
v2=v2->parent;
length+=2;
}
}
e->length=length;
e->top=v1;
}
template <class Ops>
void TreePoseGraph<Ops>::fillEdgesInfo(){
typename TreePoseGraph<Ops>::EdgeMap em=edges;
for(typename EdgeMap::iterator it=em.begin(); it!=em.end(); it++){
fillEdgeInfo(it->second);
}
}
template <class Ops>
typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::sortEdges(){
EVComparator<Edge*> comp;
comp.mode=edgeCompareMode;
EdgeSet * el=new EdgeSet(comp);
typename EdgeMap::iterator it=edges.begin();
while(it!=edges.end()){
el->insert(it->second);
it++;
}
return el;
}
template <class Ops>
typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::affectedEdges(Vertex* v){
EVComparator<Edge*> comp;
comp.mode=edgeCompareMode;
EdgeSet * es=new EdgeSet(comp);
std::deque<Vertex*> frontier;
std::list<Vertex*> markedVertices;
//frontier.push_back(v);
//v->mark=true;
for (typename EdgeList::iterator it=v->children.begin(); it!=v->children.end(); it++){
Edge* e=*it;
Vertex* other=(e->v1==v)?e->v2:e->v1;
frontier.push_back(other);
other->mark=true;
markedVertices.push_back(other);
e->mark=true;
es->insert(e);
}
while (! frontier.empty()){
Vertex* c=frontier.front();
frontier.pop_front();
markedVertices.push_back(c);
EdgeList& el=c->edges;
for (typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
Edge* e=*it;
if (e->mark)
continue;
Vertex* other= (e->v1==c)?e->v2:e->v1;
if (other==c->parent)
continue;
if (other!=e->top && ! e->top->mark){
e->top->mark=true;
frontier.push_back(e->top);
}
e->mark=true;
es->insert(e);
if (!other->mark){
other->mark=true;
frontier.push_back(other);
}
}
}
for (typename std::list<Vertex*>::iterator it=markedVertices.begin(); it!=markedVertices.end(); it++){
(*it)->mark=false;
}
for (typename EdgeSet::iterator it=es->begin(); it!=es->end(); it++){
(*it)->mark=false;
}
return es;
}
template <class Ops>
typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::affectedEdges(typename TreePoseGraph<Ops>::VertexSet& vl){
EVComparator<Edge*> comp;
comp.mode=edgeCompareMode;
EdgeSet * es=new EdgeSet(comp);
std::deque<Vertex*> frontier;
std::list<Vertex*> markedVertices;
// for (typename VertexSet::iterator it=vl.begin(); it!=vl.end(); it++){
// frontier.push_back(*it);
// (*it)->mark=true;
// }
for (typename VertexSet::iterator it=vl.begin(); it!=vl.end(); it++){
Vertex* v=*it;
for (typename EdgeList::iterator it=v->children.begin(); it!=v->children.end(); it++){
Edge* e=*it;
Vertex* other=(e->v1==v)?e->v2:e->v1;
frontier.push_back(other);
other->mark=true;
markedVertices.push_back(other);
e->mark=true;
es->insert(e);
}
}
while (! frontier.empty()){
Vertex* c=frontier.front();
frontier.pop_front();
markedVertices.push_back(c);
EdgeList& el=c->edges;
for (typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
Edge* e=*it;
if (e->mark)
continue;
Vertex* other= (e->v1==c)?e->v2:e->v1;
if (other==c->parent)
continue;
if (other!=e->top && ! e->top->mark){
e->top->mark=true;
frontier.push_back(e->top);
}
e->mark=true;
es->insert(e);
if (!other->mark){
other->mark=true;
frontier.push_back(other);
}
}
}
for (typename std::list<Vertex*>::iterator it=markedVertices.begin(); it!=markedVertices.end(); it++){
(*it)->mark=false;
}
for (typename EdgeSet::iterator it=es->begin(); it!=es->end(); it++){
(*it)->mark=false;
}
return es;
}
template <class Ops>
int TreePoseGraph<Ops>::maxPathLength(){
int max=0;
typename EdgeMap::const_iterator it=edges.begin();
while(it!=edges.end()){
int l=it->second->length;
max=l>max?l:max;
it++;
}
return max;
}
template <class Ops>
int TreePoseGraph<Ops>::totalPathLength(){
int t=0;
typename EdgeMap::const_iterator it=edges.begin();
while(it!=edges.end()){
t+=it->second->length;
it++;
}
return t;
}
template <class Ops>
void TreePoseGraph<Ops>::compressIndices(){
VertexMap vmap;
int i=0;
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->id=i;
vmap.insert(std::make_pair(i,v));
i++;
}
vertices=vmap;
}
template <class Ops>
int TreePoseGraph<Ops>::maxIndex(){
typename VertexMap::reverse_iterator it=vertices.rbegin();
if (it!=vertices.rend())
return it->second->id;
return -1;
}
template <class TPG>
struct LoopChecker{
bool noloops;
void perform(typename TPG::Vertex* v){
if (!noloops)
return;
if (!v->mark)
v->mark=true;
else
noloops=false;
}
};
template <class Ops>
bool TreePoseGraph<Ops>::sanityCheck(){
//check that each node has exactly one parent
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->mark=false;
Vertex* vp=v->parent;
if (! vp){
if (v!=root){
std::cerr << "root not found in the graph" << std::endl;
return false;
}
}
const EdgeList& children=it->second->children;
for (typename EdgeList::const_iterator lt=children.begin(); lt!=children.end(); lt++){
if ((*lt)->v1!=v){
std::cerr << "wrong direction of the edges" << std::cerr;
return false;
}
}
}
//check that there are no loops in the tree
LoopChecker< TreePoseGraph<Ops> > lc;
lc.noloops=true;
treeBreadthVisit(lc);
if (!lc.noloops){
std::cerr << "the tree contains loops" << std::endl;
return false;
}
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->mark=false;
}
return true;
}
corelib/src/toro3d/posegraph3.cpp
+405
View File
@@ -0,0 +1,405 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
* * Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file posegraph3.cpp
*
* \brief Defines the graph of 3D poses, with specific functionalities
* such as loading, saving, merging constraints, and etc.
**/
#include "posegraph3.hh"
#include <fstream>
#include <sstream>
#include <string>
using namespace std;
namespace AISNavigation {
#define LINESIZE 81920
#define DEBUG(i) \
if (verboseLevel>i) cerr
bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){
clear();
ifstream is(filename);
if (!is)
return false;
while(is){
char buf[LINESIZE];
is.getline(buf,LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (twoDimensions){
if (tag=="VERTEX"){
int id;
Pose p(0.,0.,0.,0.,0.,0.);
ls >> id >> p.x() >> p.y() >> p.yaw();
TreePoseGraph3::Vertex* v=addVertex(id,p);
if (v){
v->transformation=Transformation(p);
}
}
} else {
if (tag=="VERTEX3"){
int id;
Pose p;
ls >> id >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
TreePoseGraph3::Vertex* v=addVertex(id,p);
if (v){
v->transformation=Transformation(p);
}
}
}
}
is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg);
bool edgesOk=true;
while(is){
char buf[LINESIZE];
is.getline(buf,LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (twoDimensions){
if (tag=="EDGE"){
int id1, id2;
Pose p(0.,0.,0.,0.,0.,0.);
InformationMatrix m;
ls >> id1 >> id2 >> p.x() >> p.y() >> p.yaw();
m=DMatrix<double>::I(6);
if (! overrideCovariances){
ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[2][2] >> m[0][2] >> m[1][2];
m[2][0]=m[0][2]; m[2][1]=m[1][2]; m[1][0]=m[0][1];
}
TreePoseGraph3::Vertex* v1=vertex(id1);
TreePoseGraph3::Vertex* v2=vertex(id2);
Transformation t(p);
if (!addEdge(v1, v2,t ,m)){
cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
cerr << "edge=" << id1 <<" -> " << id2 << endl;
edgesOk=false;
}
}
} else {
if (tag=="EDGE3"){
int id1, id2;
Pose p;
InformationMatrix m;
ls >> id1 >> id2 >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
m=DMatrix<double>::I(6);
if (! overrideCovariances){
for (int i=0; i<6; i++)
for (int j=i; j<6; j++)
ls >> m[i][j];
}
TreePoseGraph3::Vertex* v1=vertex(id1);
TreePoseGraph3::Vertex* v2=vertex(id2);
Transformation t(p);
if (!addEdge(v1, v2,t ,m)){
cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
cerr << "edge=" << id1 <<" -> " << id2 << endl;
edgesOk=false;
}
}
}
}
return true;
//return edgesOk;
}
bool TreePoseGraph3::loadEquivalences(const char* filename){
ifstream is(filename);
if (!is)
return false;
EdgeList suppressed;
uint equivCount=0;
while (is){
char buf[LINESIZE];
is.getline(buf, LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (tag=="EQUIV"){
int id1, id2;
ls >> id1 >> id2;
Edge* e=edge(id1,id2);
if (!e)
e=edge(id2,id1);
if (e){
suppressed.push_back(e);
equivCount++;
}
}
}
for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
Edge* e=*it;
if (e->v1->id > e->v2->id)
revertEdge(e);
collapseEdge(e);
}
return true;
}
bool TreePoseGraph3::saveGnuplot(const char* filename){
ofstream os(filename);
if (!os)
return false;
for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
TreePoseGraph3::Vertex* v=it->second;
v->pose=v->transformation.toPoseType();
}
for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph3::Edge * e=it->second;
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.z() << " "
<< v1->pose.roll() << " " << v1->pose.pitch() << " " << v1->pose.yaw() <<endl;
os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.z() << " "
<< v2->pose.roll() << " " << v2->pose.pitch() << " " << v2->pose.yaw() <<endl;
os << endl << endl;
}
return true;
}
bool TreePoseGraph3::save(const char* filename){
ofstream os(filename);
if (!os)
return false;
for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
TreePoseGraph3::Vertex* v=it->second;
v->pose=v->transformation.toPoseType();
os << "VERTEX3 "
<< v->id << " "
<< v->pose.x() << " "
<< v->pose.y() << " "
<< v->pose.z() << " "
<< v->pose.roll() << " "
<< v->pose.pitch() << " "
<< v->pose.yaw() << endl;
}
for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph3::Edge * e=it->second;
os << "EDGE3 " << e->v1->id << " " << e->v2->id << " ";
Pose p=e->transformation.toPoseType();
os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << " ";
for (int i=0; i<6; i++)
for (int j=i; j<6; j++)
os << e->informationMatrix[i][j] << " ";
os << endl;
}
return true;
}
/** \brief A class (struct) used to print vertex information to a
stream. Needed for debugging. **/
struct IdPrinter{
IdPrinter(std::ostream& _os):os(_os){}
std::ostream& os;
void perform(TreePoseGraph3::Vertex* v){
std::cout << "(" << v->id << "," << v->level << ")" << endl;
}
};
void TreePoseGraph3::printDepth( std::ostream& os ){
IdPrinter ip(os);
treeDepthVisit(ip, root);
}
void TreePoseGraph3::printWidth( std::ostream& os ){
IdPrinter ip(os);
treeBreadthVisit(ip);
}
/** \brief A class (struct) for realizing the pose update of the
individual nodes. Assumes the correct order of constraint updates
(according to the tree level, see RSS07 paper)**/
struct PosePropagator{
void perform(TreePoseGraph3::Vertex* v){
if (!v->parent)
return;
TreePoseGraph3::Transformation tParent(v->parent->transformation);
TreePoseGraph3::Transformation tNode=tParent*v->parentEdge->transformation;
assert(v->parentEdge->v1==v->parent);
assert(v->parentEdge->v2==v);
v->transformation=tNode;
}
};
void TreePoseGraph3::initializeOnTree(){
PosePropagator pp;
treeDepthVisit(pp, root);
}
void TreePoseGraph3::printEdgesStat(std::ostream& os){
for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph3::Edge * e=it->second;
os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
Pose p=e->transformation.toPoseType();
os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << endl;
os << " top=" << e->top->id << " length=" << e->length << endl;
}
}
void TreePoseGraph3::revertEdgeInfo(Edge* e){
// here we assume uniform covariances, and we neglect the transofrmation
// induced by the Jacobian when reverting the link
e->transformation=e->transformation.inv();
};
void TreePoseGraph3::initializeFromParentEdge(Vertex* v){
Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
v->transformation=tp;
v->pose=tp.toPoseType();
v->parameters=v->parentEdge->transformation;
}
void TreePoseGraph3::collapseEdge(Edge* e){
Vertex* v1=e->v1;
Vertex* v2=e->v2;
// all the edges of v2 become outgoing
for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
if ( (*it)->v1!=v2 )
revertEdge(*it);
}
// all the edges of v1 become outgoing
for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
if ( (*it)->v1!=v1 )
revertEdge(*it);
}
assert(e->v1==v1);
InformationMatrix I12=e->informationMatrix;
CovarianceMatrix C12=I12.inv();
Transformation T12=e->transformation;
Pose p12=T12.toPoseType();
Transformation iT12=T12.inv();
//compute the marginal information of the nodes in the path v1-v2-v*
for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
Edge* e2=*it2;
if (e2->v1==v2){ //edge leaving v2
Transformation T2x=e2->transformation;
Pose p2x=T2x.toPoseType();
InformationMatrix I2x=e2->informationMatrix;
CovarianceMatrix C2x=I2x.inv();
//compute the estimate of the vertex based on the path v1-v2-vx
Transformation tr=iT12*T2x;
CovarianceMatrix CM=C2x;
Transformation T1x_pred=T12*e2->transformation;
Covariance C1x_pred=C12+C2x;
InformationMatrix I1x_pred=C1x_pred.inv();
e2->transformation=T1x_pred;
e2->informationMatrix=I1x_pred;
}
}
//all the edges leaving v1 and leaving v2 and leading to the same point are merged
std::list<Transformation> tList;
std::list<InformationMatrix> iList;
std::list<Vertex*> vList;
//others are transformed and added to v1
for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
Edge* e1x=0;
Edge* e2x=0;
if ( ((*it2)->v1!=v1)){
e2x=*it2;
for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
if ((*it1)->v2==(*it2)->v2)
e1x=*it1;
}
}
// FIXME
// edges leading to the same node are ignored
// should be merged
if (e1x && e2x){
// here goes something for mergin the constraints, according to the information matrices.
// in 3D it is a nightmare, so i postpone this, and i simply ignore the redundant constraints.
// the resultng system is overconfident
}
if (!e1x && e2x){
tList.push_back(e2x->transformation);
iList.push_back(e2x->informationMatrix);
vList.push_back(e2x->v2);
}
}
removeVertex(v2->id);
std::list<Transformation>::iterator t=tList.begin();
std::list<InformationMatrix>::iterator i=iList.begin();
std::list<Vertex*>::iterator v=vList.begin();
while (i!=iList.end()){
addEdge(v1,*v,*t,*i);
i++;
t++;
v++;
}
}
void TreePoseGraph3::recomputeAllTransformations(){
TransformationPropagator tp;
treeDepthVisit(tp,root);
}
}; //namespace AISNavigation
corelib/src/toro3d/posegraph3.hh
+145
View File
@@ -0,0 +1,145 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file posegraph3.hh
*
* \brief Defines the graph of 3D poses, with specific functionalities
* such as loading, saving, merging constraints, and etc.
**/
#ifndef _POSEGRAPH3_HH_
#define _POSEGRAPH3_HH_
#include "posegraph.hh"
#include "transformation3.hh"
#include <iostream>
#include <vector>
typedef unsigned int uint;
#ifndef M_PI
#define M_PI 3.14159265359
#endif
namespace AISNavigation {
/** \brief The class (struct) that contains 2D graph related functions
such as loading, saving, merging, etc. **/
struct TreePoseGraph3: public TreePoseGraph<Operations3D<double> >{
typedef Operations3D<double> Ops;
typedef Ops::PoseType Pose;
typedef Ops::RotationType Rotation;
typedef Ops::TranslationType Translation;
typedef Ops::TransformationType Transformation;
typedef Ops::CovarianceType CovarianceMatrix;
typedef Ops::InformationType InformationMatrix;
/** Load a graph from a file ignoring the equivalence constraints
@param filename the graph file
@param overrideCovariances ignore the covariances from the file, and use identities instead
**/
bool load( const char* filename, bool overrideCovariances=false, bool twoDimensions=false);
/** Load only the equivalence constraints from a graph file (call load before) **/
bool loadEquivalences( const char* filename);
/** Saves the graph in the graph-format**/
bool save( const char* filename);
/** Saved the graph for visualizing it using gnuplot **/
bool saveGnuplot( const char* filename);
/** Debug function **/
void printDepth( std::ostream& os );
/** Debug function **/
void printWidth( std::ostream& os );
/** Debug function **/
void printEdgesStat( std::ostream& os);
/** Initializes the parameters based on the topology of the tree and the actual transformation*/
void initializeOnTree();
/** Recomputes all the transformations based on the parameters and the tree*/
void recomputeAllTransformations();
virtual void initializeFromParentEdge(Vertex* v);
/** Turn around the edge (<i,j> => <j,i>) **/
virtual void revertEdgeInfo(Edge* e);
/** Function to compress a graph. Needed if, for example, equivalence
constraints are used to build a graoh structure with indices
without gaps. **/
virtual void collapseEdge(Edge* e);
/** Specifies the verbose level for debugging **/
int verboseLevel;
protected:
/** \brief A class (struct) to compute the parameterization of the vertex v **/
struct ParameterPropagator{
inline void perform(TreePoseGraph3::Vertex* v){
if (!v->parent){
v->parameters=TreePoseGraph3::Transformation(0.,0.,0.,0.,0.,0.);
return;
}
v->parameters=v->parent->transformation.inv()*v->transformation;
}
};
/** \brief A class (struct) to compute the parameterization of the vertex v **/
struct TransformationPropagator{
inline void perform(TreePoseGraph3::Vertex* v){
if (!v->parent){
return;
}
v->transformation=v->parent->transformation*v->parameters;
}
};
};
}; //namespace AISNavigation
#endif
corelib/src/toro3d/transformation3.hh
+275
View File
@@ -0,0 +1,275 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
#ifndef _TRANSFORMATION3_HXX_
#define _TRANSFORMATION3_HXX_
#include <assert.h>
#include <cmath>
#include "dmatrix.hh"
namespace AISNavigation {
template <class T>
struct Vector3 {
T elems[3] ;
Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
Vector3() {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
Vector3(const DVector<T>& t){}
// translational view
inline const T& x() const {return elems[0];}
inline const T& y() const {return elems[1];}
inline const T& z() const {return elems[2];}
inline T& x() {return elems[0];}
inline T& y() {return elems[1];}
inline T& z() {return elems[2];}
// rotational view
inline const T& roll() const {return elems[0];}
inline const T& pitch() const {return elems[1];}
inline const T& yaw() const {return elems[2];}
inline T& roll() {return elems[0];}
inline T& pitch() {return elems[1];}
inline T& yaw() {return elems[2];}
};
template <class T>
struct Pose3 : public DVector<T>{
Pose3();
Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);}
inline operator const DVector<T>& () {return (const DVector<T>)*this;}
inline operator DVector<T>& () {return *this;}
inline const T& roll() const {return DVector<T>::elems[0];}
inline const T& pitch() const {return DVector<T>::elems[1];}
inline const T& yaw() const {return DVector<T>::elems[2];}
inline const T& x() const {return DVector<T>::elems[3];}
inline const T& y() const {return DVector<T>::elems[4];}
inline const T& z() const {return DVector<T>::elems[5];}
inline T& roll() {return DVector<T>::elems[0];}
inline T& pitch() {return DVector<T>::elems[1];}
inline T& yaw() {return DVector<T>::elems[2];}
inline T& x() {return DVector<T>::elems[3];}
inline T& y() {return DVector<T>::elems[4];}
inline T& z() {return DVector<T>::elems[5];}
};
/*!
* A Quaternion can be used to either represent a rotational axis
* and a Rotation, or, the point which will be rotated
*/
template <class T>
struct Quaternion{
/*!
* Default Constructor: w=x=y=z=0;
*/
Quaternion();
/*!
* The Quaternion representation of the point "pose"
*/
Quaternion(const Vector3<T>& pose);
/*!
* create a Quaternion by scalar w and the imaginery parts x,y, and z.
*/
Quaternion(const T _w, const T _x, const T _y, const T _z);
/*!
* create a rotational Quaternion, roll along x-axis, pitch along y-axis and yaw along z-axis
*/
Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
/*!
* @return the conjugated version of this quaternion
*/
inline Quaternion<T> conjugated() const;
/*!
* @return this quaternion, but normalized
*/
inline Quaternion<T> normalized() const;
/*!
* @return the inverse of this Quaternion
*/
inline Quaternion<T> inverse() const;
/*construct a quaternion on the axis/angle representation*/
inline Quaternion(const Vector3<T>& axis, const T& angle);
/*!
* if this Quaternion represents a point, use this function
* to rotate the point along <axis> with angle <alpha>
* @param axis the rotational axis
* @param alpha rotational angle
*/
inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
/*!
* if this Quaternion represents a rotational axis + rotation,
* use this function to rotate another point represented as a Quaternion p
* @param p the point to be rotated by <this>. Point is represented as a Quaternion
* @return rotated Point (represented as a Quaternion)
*/
inline Quaternion<T> rotatePoint(const Quaternion& p) const;
/*!
* if this Quaternion represents a rotational axis + rotation,
* use this function to rotate another point
* @param p the point to be rotated by <this>.
* @return rotated Point
*/
inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
/*!
* if this Quaternion represents a rotational axis, add a rotation of angle <alpha>
* along <this> axis to the Quaternion
* @param alpha rotational value
* @return this Quaternion with included information about the rotation along <this> axis
*/
inline Quaternion withRotation (const T alpha) const;
/*!
* Given rotational axis x,y,z, get the rotation along these axis encoded in this Quaternion
* @return rotation along x,y,z axis encoded in <this> Quaternion
*/
inline Vector3<T> toAngles() const;
inline Vector3<T> axis() const;
inline T angle() const;
/*!
* @return the norm of this Quaternion
*/
inline T norm() const;
/*!
* @return the real part (==w) of this Quaternion
*/
inline T re() const;
/*!
* @return the imaginery part (== (x,y,z)) of this Quaternion
*/
inline Vector3<T> im() const;
T w,x,y,z;
};
template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);
template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);
template <class T>
struct Transformation3{
Quaternion<T> rotationQuaternion;
Vector3<T> translationVector;
Transformation3(){}
inline static Transformation3<T> identity();
Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
Transformation3 (const Pose3<T>& v);
Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
inline Vector3<T> translation() const;
inline Quaternion <T> rotation() const;
inline Pose3<T> toPoseType() const;
inline void setTranslation(const Vector3<T>& t);
inline void setTranslation(const T& x, const T& y, const T& z);
inline void setRotation(const Vector3<T>& r);
inline void setRotation(const T& roll, const T& pitch, const T& yaw);
inline void setRotation(const Quaternion<T>& q);
inline Transformation3<T> inv() const;
inline bool validRotation(const T& epsilon=0.001) const;
};
template <class T>
inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);
template <class T>
inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);
template <class T>
struct Operations3D{
typedef T BaseType;
typedef Pose3<T> PoseType;
typedef Quaternion<T> RotationType;
typedef Vector3<T> TranslationType;
typedef Transformation3<T> TransformationType;
typedef DMatrix<T> CovarianceType;
typedef DMatrix<T> InformationType;
typedef Transformation3<T> ParametersType;
};
} // namespace AISNavigation
/**************************** IMPLEMENTATION ****************************/
#include "transformation3.hxx"
#endif
corelib/src/toro3d/transformation3.hxx
+451
View File
@@ -0,0 +1,451 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
#include <limits>
namespace AISNavigation {
template <class T>
inline Vector3<T> operator * (const T& d, const Vector3<T>& v) {
return Vector3<T>(v.elems[0]*d, v.elems[1]*d, v.elems[2]*d);
}
template <class T>
inline Vector3<T> operator * (const Vector3<T>& v, const T& d) {
return Vector3<T>(v.elems[0]*d, v.elems[1]*d, v.elems[2]*d);
}
template <class T>
inline T operator * (const Vector3<T>& v1, const Vector3<T>& v2){
return v1.elems[0]*v2.elems[0]
+ v1.elems[1]*v2.elems[1]
+ v1.elems[2]*v2.elems[2];
}
template <class T>
inline Vector3<T> operator + (const Vector3<T>& v1, const Vector3<T>& v2){
return Vector3<T>(v1.elems[0]+v2.elems[0],
v1.elems[1]+v2.elems[1],
v1.elems[2]+v2.elems[2]);
}
template <class T>
Vector3<T> operator - (const Vector3<T>& v1, const Vector3<T>& v2){
return Vector3<T>(v1.elems[0]-v2.elems[0],
v1.elems[1]-v2.elems[1],
v1.elems[2]-v2.elems[2]);
}
template <class T>
Pose3<T>::Pose3(): DVector<T>(6){
}
template <class T>
Pose3<T>::Pose3(const Vector3<T>& trans, const Vector3<T>& rot): DVector<T>(6){
DVector<T>::elems[0]=rot.roll();
DVector<T>::elems[1]=rot.pitch();
DVector<T>::elems[2]=rot.yaw();
DVector<T>::elems[3]=trans.x();
DVector<T>::elems[4]=trans.y();
DVector<T>::elems[5]=trans.z();
}
template <class T>
Pose3<T>::Pose3(const T& x, const T& y, const T& z, const T& r, const T& p, const T& yw): DVector<T>(6){
DVector<T>::elems[0]=r;
DVector<T>::elems[1]=p;
DVector<T>::elems[2]=yw;
DVector<T>::elems[3]=x;
DVector<T>::elems[4]=y;
DVector<T>::elems[5]=z;
}
#define MY_MAX(a,b) (((a)>(b))?(a):(b))
template<class T>
Quaternion<T>::Quaternion(){
w = 1;
x = 0;
y = 0;
z = 0;
}
template<class T>
Quaternion<T>::Quaternion(const Vector3<T>& pose){
w = 0;
x = pose.x();
y = pose.y();
z = pose.z();
}
template<class T>
Quaternion<T>::Quaternion(const Vector3<T>& axis, const T& angle){
T sa=sin(angle/2);
T ca=cos(angle/2);
w=ca;
x=axis.x()*sa;
y=axis.y()*sa;
z=axis.z()*sa;
}
template<class T>
Quaternion<T>::Quaternion(const T _w, const T _x, const T _y, const T _z){
w = _w;
x = _x;
y = _y;
z = _z;
}
template<class T>
Quaternion<T>::Quaternion(const T phi, const T theta, const T psi){
T sphi = sin(phi);
T stheta = sin(theta);
T spsi = sin(psi);
T cphi = cos(phi);
T ctheta = cos(theta);
T cpsi = cos(psi);
T _r[3][3] = { //create rotational Matrix
{cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
{spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
{ -stheta, ctheta*sphi, ctheta*cphi}
};
T _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0;
T _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0;
T _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0;
T _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0;
this->w = _w;
this->x = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x);
this->y = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y);
this->z = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z);
}
template<class T>
inline Quaternion<T> Quaternion<T>::conjugated() const{
return Quaternion<T>(w,-x,-y,-z);
}
template<class T>
inline Quaternion<T> Quaternion<T>::normalized() const{
T n = this->norm();
if (n > 0)
return ((1./n) * (*this));
else
return Quaternion<T>(0.,0.,0.,0.);
}
template<class T>
inline Quaternion<T> Quaternion<T>::inverse() const{
return ((1./this->norm()) * this->conjugated());
}
template<class T>
inline Quaternion<T> Quaternion<T>::rotateThisAlong(const Vector3<T>& axis, const T alpha) const{
Quaternion<T> q(axis);
q = q.normalized();
q = q.withRotation(alpha);
return q.rotatePoint(*this);
}
template<class T>
inline Quaternion<T> Quaternion<T>::rotatePoint(const Quaternion<T>& p) const{
return (*this)*p*(this->conjugated());
}
template<class T>
inline Vector3<T> Quaternion<T>::rotatePoint(const Vector3<T>& point) const{
Quaternion<T> p(point);
Quaternion<T> q = this->rotatePoint(p);
return q.im();
}
template<class T>
inline Quaternion<T> Quaternion<T>::withRotation(const T alpha) const{
Quaternion<T> q = normalized();
T salpha = sin(alpha/2.);
T calpha = cos(alpha/2.);
q.w = calpha;
q.x = salpha * q.x;
q.y = salpha * q.y;
q.z = salpha * q.z;
return q;
}
template<class T>
inline Vector3<T> Quaternion<T>::toAngles() const{
T n = this->norm();
T s = n > 0?2./(n*n):0.;
T m00, m01, m02, m10, m11, m12, m20, m21, m22;
T phi,theta,psi;
T xs = this->x*s;
T ys = this->y*s;
T zs = this->z*s;
T wx = this->w*xs;
T wy = this->w*ys;
T wz = this->w*zs;
T xx = this->x*xs;
T xy = this->x*ys;
T xz = this->x*zs;
T yy = this->y*ys;
T yz = this->y*zs;
T zz = this->z*zs;
m00 = 1.0 - (yy + zz);
m11 = 1.0 - (xx + zz);
m22 = 1.0 - (xx + yy);
m10 = xy + wz;
m01 = xy - wz;
m20 = xz - wy;
m02 = xz + wy;
m21 = yz + wx;
m12 = yz - wx;
phi = atan2(m21,m22);
theta = atan2(-m20,sqrt(m21*m21 + m22*m22));
psi = atan2(m10,m00);
return Vector3<T>(phi, theta, psi);
}
template<class T>
inline Vector3<T> Quaternion<T>::axis() const {
double imNorm=sqrt(x*x+y*y+z*z);
if (imNorm<std::numeric_limits<double>::min()){
return Vector3<T>(0.,0.,1.);
}
return Vector3<T>(x/imNorm, y/imNorm, z/imNorm);
}
template<class T>
inline T Quaternion<T>::angle() const{
Quaternion<T> q=normalized();
double a=2*atan2(sqrt(q.x*q.x + q.y*q.y + q.z*q.z), q.w);
return atan2(sin(a), cos(a));
}
template<class T>
inline T Quaternion<T>::norm() const{
return sqrt(w*w + x*x + y*y + z*z);
}
template<class T>
inline T Quaternion<T>::re() const{
return w;
}
template<class T>
inline Vector3<T> Quaternion<T>::im() const{
return Vector3<T>(x, y, z);
}
template<class T>
inline Quaternion<T> operator + (const Quaternion<T>& left, const Quaternion<T>& right){
return Quaternion<T>(left.w + right.w, left.x + right.x, left.y + right.y, left.z + right.z);
}
template<class T>
inline Quaternion<T> operator - (const Quaternion<T>& left, const Quaternion<T>& right){
return Quaternion<T>(left.w - right.w, left.x - right.x, left.y - right.y, left.z - right.z);
}
template<class T>
inline Quaternion<T> operator * (const Quaternion<T>& q1, const Quaternion<T>& q2){
return Quaternion<T> (q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
q1.y*q2.z - q2.y*q1.z + q1.w*q2.x + q2.w*q1.x,
q1.z*q2.x - q2.z*q1.x + q1.w*q2.y + q2.w*q1.y,
q1.x*q2.y - q2.x*q1.y + q1.w*q2.z + q2.w*q1.z);
}
template<class T>
inline Quaternion<T> operator * (const Quaternion<T>& q, const T s){
return Quaternion<T>(s*q.w, s*q.x, s*q.y, s*q.z);
}
template<class T>
inline Quaternion<T> operator * (const T s, const Quaternion<T>& q){
return Quaternion<T>(q.w*s, q.x*s, q.y*s, q.z*s);
}
template<class T>
std::ostream& operator << (std::ostream& os, const Quaternion<T>& q){
os << q.w << " " << q.x << " " << q.y << " " << q.z << " ";
return os;
}
template<class T>
inline T innerproduct(const Quaternion<T>& q1, const Quaternion<T>& q2){
return q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z;
}
template<class T>
inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda){
Quaternion<T> _from = from.normalized();
Quaternion<T> _to = to.normalized();
T _cos_omega = innerproduct(_from,_to);
_cos_omega = (_cos_omega>1)?1:_cos_omega;
_cos_omega = (_cos_omega<-1)?-1:_cos_omega;
T _omega = acos(_cos_omega);
assert (!isnan(_cos_omega));
if (fabs(_omega) < 1e-6)
return to;
//determine right direction of slerp:
Quaternion<T> _pq = _from - _to;
Quaternion<T> _pmq = _from + _to;
T _first = _pq.norm();
T _alternativ = _pmq.norm();
Quaternion<T> q1 = _from;
Quaternion<T> q2 = (_first < _alternativ)? (Quaternion<T>) _to: -1.*(Quaternion<T>)_to;
//now calculate intermediate quaternion.
Quaternion<T> ret = q1*(sin((1-lambda)*_omega)/(sin(_omega))) + q2*(sin(lambda*_omega)/sin(_omega));
assert (!(isnan(ret.w) || isnan(ret.x) || isnan(ret.y) || isnan(ret.z)));
return ret;
}
template <class T>
inline Transformation3<T> Transformation3<T>::identity(){
Transformation3<T> m;
m.rotationQuaternion=Quaternion<T>();
m.translationVector(0.,0.,0.);
return m;
}
template <class T>
inline Transformation3<T>::Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw){
rotationQuaternion=Quaternion<T>(roll,pitch,yaw);
translationVector=Vector3<T>(x,y,z);
}
template <class T>
inline Transformation3<T>::Transformation3 (const Pose3<T>& v){
rotationQuaternion=Quaternion<T>(v.roll(),v.pitch(),v.yaw());
translationVector=Vector3<T>(v.x(),v.y(),v.z());
}
template <class T>
inline Vector3<T> Transformation3<T>::translation() const {
return translationVector;
}
template <class T>
inline Quaternion<T> Transformation3<T>::rotation() const {
return rotationQuaternion;
}
template <class T>
inline Pose3<T> Transformation3<T>::toPoseType() const {
Vector3<T> t=translation();
Vector3<T> r=rotationQuaternion.toAngles();
Pose3<T> rv(t.x(), t.y(), t.z(), r.roll(), r.pitch(), r.yaw() );
return rv;
}
template <class T>
inline void Transformation3<T>::setTranslation(const Vector3<T>& t){
translationVector=t;
}
template <class T>
inline void Transformation3<T>::setRotation(const Quaternion<T>& q){
rotationQuaternion=q.normalized();
}
template <class T>
inline void Transformation3<T>::setRotation(const Vector3<T>& r){
setRotation(r.roll(),r.pitch(), r.yaw());
}
template <class T>
inline void Transformation3<T>::setRotation(const T& roll_phi, const T& pitch_theta, const T& yaw_psi){
rotationQuaternion=Quaternion<T>(roll_phi, pitch_theta, yaw_psi);
}
template <class T>
inline void Transformation3<T>::setTranslation(const T& x, const T& y, const T& z){
translationVector=Vector3<T>(x,y,z);
}
template <class T>
inline Transformation3<T> Transformation3<T>::inv() const {
Transformation3<T> rv(*this);
rv.rotationQuaternion=rotationQuaternion.inverse().normalized();
rv.translationVector=rv.rotationQuaternion.rotatePoint(translationVector*-1.);
return rv;
}
template <class T>
inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v){
return m.translationVector+m.rotationQuaternion.rotatePoint(v);
}
template <class T>
inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2){
Transformation3<T> rv;
rv.translationVector=m1.rotationQuaternion.rotatePoint(m2.translationVector)+m1.translationVector;
rv.rotationQuaternion=(m1.rotationQuaternion*m2.rotationQuaternion).normalized();
return rv;
}
} // namespace AISNavigation
corelib/src/toro3d/treeoptimizer3.cpp
+361
View File
@@ -0,0 +1,361 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file treeoptimizer3.cpp
*
* \brief Defines the core optimizer class for 3D graphs which is a
* subclass of TreePoseGraph3
*
**/
#include "treeoptimizer3.hh"
#include <fstream>
#include <sstream>
#include <string>
using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
TreeOptimizer3::TreeOptimizer3(){
restartOnDivergence=false;
sortedEdges=0;
mpl=-1;
edgeCompareMode=EVComparator<Edge*>::CompareLevel;
}
TreeOptimizer3::~TreeOptimizer3(){
}
void TreeOptimizer3::initializeTreeParameters(){
ParameterPropagator pp;
treeDepthVisit(pp,root);
}
void TreeOptimizer3::iterate(TreePoseGraph3::EdgeSet* eset, bool noPreconditioner){
TreePoseGraph3::EdgeSet* temp=sortedEdges;
if (eset){
sortedEdges=eset;
}
if (noPreconditioner)
propagateErrors(false);
else {
if (iteration==1)
computePreconditioner();
propagateErrors(true);
}
sortedEdges=temp;
onRestartBegin();
if (restartOnDivergence){
double mte, ate;
double mre, are;
error(&mre, &mte, &are, &ate);
maxTranslationalErrors.push_back(mte);
maxRotationalErrors.push_back(mre);
int interval=3;
if ((int)maxRotationalErrors.size()>=interval){
uint s=maxRotationalErrors.size();
double re0 = maxRotationalErrors[s-interval];
double re1 = maxRotationalErrors[s-1];
if ((re1-re0)>are || sqrt(re1)>0.99*M_PI){
double rg=rotGain;
if (sqrt(re1)>M_PI/4){
cerr << "RESTART!!!!! : Angular wraparound may be occourring" << endl;
cerr << " err=" << re0 << " -> " << re1 << endl;
cerr << "Restarting optimization and reducing the rotation factor" << endl;
cerr << rg << " -> ";
initializeOnTree();
initializeTreeParameters();
initializeOptimization();
error(&mre, &mte);
maxTranslationalErrors.push_back(mte);
maxRotationalErrors.push_back(mre);
rg*=0.1;
rotGain=rg;
cerr << rotGain << endl;
}
else {
cerr << "decreasing angular gain" << rotGain*0.1 << endl;
rotGain*=0.1;
}
}
}
}
onRestartDone();
}
void TreeOptimizer3::recomputeTransformations(Vertex*v, Vertex* top){
if (v==top)
return;
recomputeTransformations(v->parent, top);
v->transformation=v->parent->transformation*v->parameters;
}
void TreeOptimizer3::recomputeParameters(Vertex*v, Vertex* top){
while (v!=top){
v->parameters=v->parent->transformation.inv()*v->transformation;
v=v->parent;
}
}
TreeOptimizer3::Transformation TreeOptimizer3::getPose(Vertex*v, Vertex* top){
Transformation t(0.,0.,0.,0.,0.,0.);
if (v==top)
return v->transformation;
while (v!=top){
t=v->parameters*t;
v=v->parent;
}
return top->transformation*t;
}
TreeOptimizer3::Rotation TreeOptimizer3::getRotation(Vertex*v, Vertex* top){
Rotation r(0.,0.,0.);
if (v==top)
return v->transformation.rotation();
while (v!=top){
r=v->parameters.rotation()*r;
v=v->parent;
}
return top->transformation.rotation()*r;
}
double TreeOptimizer3::error(const Edge* e) const{
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
Transformation et=e->transformation;
Transformation t1=v1->transformation;
Transformation t2=v2->transformation;
Transformation t12=(t1*et)*t2.inv();
Pose p12=t12.toPoseType();
Pose ps=e->informationMatrix*p12;
double err=p12*ps;
DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
return err;
}
double TreeOptimizer3::traslationalError(const Edge* e) const{
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
Transformation et=e->transformation;
Transformation t1=v1->transformation;
Transformation t2=v2->transformation;
Translation t12=(t2.inv()*(t1*et)).translation();
return t12*t12;;
}
double TreeOptimizer3::rotationalError(const Edge* e) const{
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
Rotation er=e->transformation.rotation();
Rotation r1=v1->transformation.rotation();
Rotation r2=v2->transformation.rotation();
Rotation r12=r2.inverse()*(r1*er);
double r=r12.angle();
return r*r;
}
double TreeOptimizer3::loopError(const Edge* e) const{
double err=0;
const Vertex* v=e->v1;
while (v!=e->top){
err+=error(v->parentEdge);
v=v->parent;
}
v=e->v2;
while (v==e->top){
err+=error(v->parentEdge);
v=v->parent;
}
if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
err+=error(e);
return err;
}
double TreeOptimizer3::loopRotationalError(const Edge* e) const{
double err=0;
const Vertex* v=e->v1;
while (v!=e->top){
err+=rotationalError(v->parentEdge);
v=v->parent;
}
v=e->v2;
while (v!=e->top){
err+=rotationalError(v->parentEdge);
v=v->parent;
}
if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
err+=rotationalError(e);
return err;
}
double TreeOptimizer3::error(double* mre, double* mte, double* are, double* ate, TreePoseGraph3::EdgeSet* eset) const{
double globalRotError=0.;
double maxRotError=0;
double globalTrasError=0.;
double maxTrasError=0;
int c=0;
if (! eset){
for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
double re=rotationalError(it->second);
globalRotError+=re;
maxRotError=maxRotError>re?maxRotError:re;
double te=traslationalError(it->second);
globalTrasError+=te;
maxTrasError=maxTrasError>te?maxTrasError:te;
c++;
}
} else {
for (TreePoseGraph3::EdgeSet::const_iterator it=eset->begin(); it!=eset->end(); it++){
const TreePoseGraph3::Edge* edge=*it;
double re=rotationalError(edge);
globalRotError+=re;
maxRotError=maxRotError>re?maxRotError:re;
double te=traslationalError(edge);
globalTrasError+=te;
maxTrasError=maxTrasError>te?maxTrasError:te;
c++;
}
}
if (mte)
*mte=maxTrasError;
if (mre)
*mre=maxRotError;
if (ate)
*ate=globalTrasError/c;
if (are)
*are=globalRotError/c;
return globalRotError+globalTrasError;
}
void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
edgeCompareMode=mode;
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
// sorting edges
if (sortedEdges!=0){
delete sortedEdges;
sortedEdges=0;
}
sortedEdges=sortEdges();
mpl=maxPathLength();
rotGain=1.;
trasGain=1.;
}
void TreeOptimizer3::initializeOnlineIterations(){
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
maxRotationalErrors.clear();
maxTranslationalErrors.clear();
rotGain=1.;
trasGain=1.;
}
void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
edgeCompareMode=mode;
// compute the size of the preconditioning matrix
clear();
Vertex* v0=addVertex(0,Pose(0,0,0,0,0,0));
root=v0;
v0->parameters=Transformation(v0->pose);
v0->parentEdge=0;
v0->parent=0;
v0->level=0;
v0->transformation=Transformation(TreePoseGraph3::Pose(0,0,0,0,0,0));
}
void TreeOptimizer3::onStepStart(Edge* e){
DEBUG(5) << "entering edge" << e << endl;
}
void TreeOptimizer3::onStepFinished(Edge* e){
DEBUG(5) << "exiting edge" << e << endl;
}
void TreeOptimizer3::onIterationStart(int iteration){
DEBUG(5) << "entering iteration " << iteration << endl;
}
void TreeOptimizer3::onIterationFinished(int iteration){
DEBUG(5) << "exiting iteration " << iteration << endl;
}
void TreeOptimizer3::onRestartBegin(){}
void TreeOptimizer3::onRestartDone(){}
bool TreeOptimizer3::isDone(){
return false;
}
}; //namespace AISNavigation
corelib/src/toro3d/treeoptimizer3.hh
+181
View File
@@ -0,0 +1,181 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file treeoptimizer3.hh
*
* \brief Defines the core optimizer class for 3D graphs which is a
* subclass of TreePoseGraph3
*
**/
#ifndef _TREEOPTIMIZER3_HH_
#define _TREEOPTIMIZER3_HH_
#include "posegraph3.hh"
namespace AISNavigation {
/** \brief Class that contains the core optimization algorithm **/
struct TreeOptimizer3: public TreePoseGraph3{
typedef std::vector<Pose> PoseVector;
/** Constructor **/
TreeOptimizer3();
/** Destructor **/
virtual ~TreeOptimizer3();
/** Initialization function **/
void initializeTreeParameters();
/** Initialization function **/
void initializeOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
void initializeOnlineOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
void initializeOnlineIterations();
/** Performs one iteration of the algorithm **/
void iterate(TreePoseGraph3::EdgeSet* eset=0, bool noPreconditioner=false);
/** Conmputes the gloabl error of the network **/
double error(double* mre=0, double* mte=0, double* are=0, double* ate=0, TreePoseGraph3::EdgeSet* eset=0) const;
/** Conmputes the gloabl error of the network **/
double angularError() const;
/** Conmputes the gloabl error of the network **/
double translationalError() const;
bool restartOnDivergence;
inline double getRotGain() const {return rotGain;}
/** Iteration counter **/
int iteration;
double rpFraction;
protected:
/** Recomputes only the pose of the node v wrt. to an arbitraty
parent (top) of v in the tree **/
Transformation getPose(Vertex*v, Vertex* top);
/** Recomputes only the pose of the node v wrt. to an arbitraty
parent (top) of v in the tree **/
Rotation getRotation(Vertex*v, Vertex* top);
void recomputeTransformations(Vertex*v, Vertex* top);
void recomputeParameters(Vertex*v, Vertex* top);
void computePreconditioner();
void propagateErrors(bool usePreconditioner=false);
/** Computes the error of the constraint/edge e **/
double error(const Edge* e) const;
/** Computes the error of the constraint/edge e **/
double loopError(const Edge* e) const;
/** Computes the rotational error of the constraint/edge e **/
double loopRotationalError(const Edge* e) const;
/** Conmputes the error of the constraint/edge e **/
double translationalError(const Edge* e) const;
/** Conmputes the error of the constraint/edge e **/
double rotationalError(const Edge* e) const;
double traslationalError(const Edge* e) const;
/** Used to compute the learning rate lambda **/
double gamma[2];
/** The simplified version of the preconditioning matrix **/
struct PM_t{
double v [2];
inline double& operator[](int i){return v[i];}
};
typedef std::vector< PM_t > PMVector;
PMVector M;
/**cached maximum path length*/
int mpl;
/**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
std::vector<double> maxRotationalErrors;
/**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
std::vector<double> maxTranslationalErrors;
double rotGain, trasGain;
/**callback invoked before starting the optimization of an individual constraint,
@param e: the constraint being optimized*/
virtual void onStepStart(Edge* e);
/**callback invoked after finishing the optimization of an individual constraint,
@param e: the constraint optimized*/
virtual void onStepFinished(Edge* e);
/**callback invoked before starting a full iteration,
@param i: the current iteration number*/
virtual void onIterationStart(int i);
/**callback invoked after finishing a full iteration,
@param i: the current iteration number*/
virtual void onIterationFinished(int iteration);
/**callback invoked before a restart of the optimizer
when the angular wraparound is detected*/
virtual void onRestartBegin();
/**callback invoked after a restart of the optimizer*/
virtual void onRestartDone();
/**callback for determining a termination condition,
it can be used by an external thread for stopping the optimizer while performing an iteration.
@returns true when the optimizer has to stop.*/
virtual bool isDone();
};
}; //namespace AISNavigation
#endif
corelib/src/toro3d/treeoptimizer3_iteration.cpp
+343
View File
@@ -0,0 +1,343 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
#include "treeoptimizer3.hh"
#include <fstream>
#include <string>
using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
//helper functions. Should I explain :-)?
inline double max3( const double& a, const double& b, const double& c){
double m=a>b?a:b;
return m>c?m:c;
}
inline double min3( const double& a, const double& b, const double& c){
double m=a<b?a:b;
return m<c?m:c;
}
struct NodeInfo{
TreeOptimizer3::Vertex* n;
double translationalWeight;
double rotationalWeight;
int direction;
TreeOptimizer3::Transformation transformation;
TreeOptimizer3::Transformation parameters;
NodeInfo(TreeOptimizer3::Vertex* v=0, double tw=0, double rw=0, int dir=0,
TreeOptimizer3::Transformation t=TreeOptimizer3::Transformation(0,0,0,0,0,0),
TreeOptimizer3::Parameters p=TreeOptimizer3::Transformation(0,0,0,0,0,0)){
n=v;
translationalWeight=tw;
rotationalWeight=rw;
direction=dir;
transformation=t;
parameters=p;
}
};
typedef std::vector<NodeInfo> NodeInfoVector;
/********************************** Preconditioned and unpreconditioned error distribution ************************************/
void TreeOptimizer3::computePreconditioner(){
for (uint i=0; i<M.size(); i++){
M[i][0]=0;
M[i][1]=0;
}
gamma[0] = gamma[1] = numeric_limits<double>::max();
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%1000))
DEBUG(1) << "m";
Edge* e=*it;
Transformation t=e->transformation;
InformationMatrix W=e->informationMatrix;
Vertex* top=e->top;
for (int dir=0; dir<2; dir++){
Vertex* n = (dir==0)? e->v1 : e->v2;
while (n!=top){
uint i=n->id;
double rW=min3(W[0][0], W[1][1], W[2][2]);
double tW=min3(W[3][3], W[4][4], W[5][5]);
M[i][0]+=rW;
M[i][1]+=tW;
gamma[0]=gamma[0]<rW?gamma[0]:rW;
gamma[1]=gamma[1]<tW?gamma[1]:tW;
n=n->parent;
}
}
}
if (verboseLevel>1){
for (uint i=0; i<M.size(); i++){
cerr << "M[" << i << "]=" << M[i][0] << " " << M[i][1] << endl;
}
}
}
void TreeOptimizer3::propagateErrors(bool usePreconditioner){
iteration++;
int edgeCount=0;
// this is the workspace for computing the paths without
// bothering too much the memory allocation
static NodeInfoVector path;
path.resize(edges.size()+1);
static Rotation zero(0.,0.,0.);
onIterationStart(iteration);
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%1000))
DEBUG(1) << "c";
if (isDone())
return;
Edge* e=*it;
Vertex* top=e->top;
Vertex* v1=e->v1;
Vertex* v2=e->v2;
int l=e->length;
onStepStart(e);
recomputeTransformations(v1,top);
recomputeTransformations(v2,top);
DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
//BEGIN: Path and weight computation
int pc=0;
Vertex* aux=v1;
double totTW=0, totRW=0;
while(aux!=top){
int index=aux->id;
double tw=1./(double)l, rw=1./(double)l;
if (usePreconditioner){
tw=1./M[index][0];
rw=1./M[index][1];
}
totTW+=tw;
totRW+=rw;
path[pc++]=NodeInfo(aux,tw,rw,-1,aux->transformation, aux->parameters);
aux=aux->parent;
}
int topIndex=pc;
path[pc++]=NodeInfo(top,0.,0.,0, top->transformation, top->parameters);
pc=l;
aux=v2;
while(aux!=top){
int index=aux->id;
double tw=1./l, rw=1./l;
if (usePreconditioner){
tw=1./M[index][0];
rw=1./M[index][1];
}
totTW+=tw;
totRW+=rw;
path[pc--]=NodeInfo(aux,tw,rw,1,aux->transformation, aux->parameters);
aux=aux->parent;
}
//store the transformations relative to the top node
Transformation topTransformation=top->transformation;
Transformation topParameters=top->parameters;
//END: Path and weight computation
//BEGIN: Rotational Error
Rotation r1=getRotation(v1, top);
Rotation r2=getRotation(v2, top);
Rotation re=e->transformation.rotation();
Rotation rR=r2.inverse()*(r1*re);
double rotationFactor=(usePreconditioner)?
sqrt(double(l))* min3(e->informationMatrix[0][0],
e->informationMatrix[1][1],
e->informationMatrix[2][2])/
( gamma[0]* (double)iteration ):
sqrt(double(l))*rotGain/(double)iteration;
// double rotationFactor=(usePreconditioner)?
// sqrt(double(l))*rotGain/
// ( gamma[0]* (double)iteration * min3(e->informationMatrix[0][0],
// e->informationMatrix[1][1],
// e->informationMatrix[2][2])):
// sqrt(double(l))*rotGain/(double)iteration;
if (rotationFactor>1)
rotationFactor=1;
Rotation totalRotation = path[l].transformation.rotation() * rR * path[l].transformation.rotation().inverse();
Translation axis = totalRotation.axis();
double angle=totalRotation.angle();
double cw=0;
for (int i= 1; i<=topIndex; i++){
cw+=path[i-1].rotationalWeight/totRW;
Rotation R=path[i].transformation.rotation();
Rotation B(axis, angle*cw*rotationFactor);
R= B*R;
path[i].transformation.setRotation(R);
}
for (int i= topIndex+1; i<=l; i++){
cw+=path[i].rotationalWeight/totRW;
Rotation R=path[i].transformation.rotation();
Rotation B(axis, angle*cw*rotationFactor);
R= B*R;
path[i].transformation.setRotation(R);
}
//recompute the parameters based on the transformation
for (int i=0; i<topIndex; i++){
Vertex* n=path[i].n;
n->parameters.setRotation(path[i+1].transformation.rotation().inverse()*path[i].transformation.rotation());
}
for (int i= topIndex+1; i<=l; i++){
Vertex* n=path[i].n;
n->parameters.setRotation(path[i-1].transformation.rotation().inverse()*path[i].transformation.rotation());
}
//END: Rotational Error
//now spread the parameters
recomputeTransformations(v1,top);
recomputeTransformations(v2,top);
//BEGIN: Translational Error
Translation topTranslation=top->transformation.translation();
Transformation tr12=v1->transformation*e->transformation;
Translation tR=tr12.translation()-v2->transformation.translation();
// double translationFactor=(usePreconditioner)?
// trasGain*l/( gamma[1]* (double)iteration * min3(e->informationMatrix[3][3],
// e->informationMatrix[4][4],
// e->informationMatrix[5][5])):
// trasGain*l/(double)iteration;
double translationFactor=(usePreconditioner)?
trasGain*l*min3(e->informationMatrix[3][3],
e->informationMatrix[4][4],
e->informationMatrix[5][5])/( gamma[1]* (double)iteration):
trasGain*l/(double)iteration;
if (translationFactor>1)
translationFactor=1;
Translation dt=tR*translationFactor;
//left wing
double lcum=0;
for (int i=topIndex-1; i>=0; i--){
Vertex* n=path[i].n;
lcum-=(usePreconditioner) ? path[i].translationalWeight/totTW : 1./(double)l;
double fraction=lcum;
Translation offset= dt*fraction;
Translation T=n->transformation.translation()+offset;
n->transformation.setTranslation(T);
}
//right wing
double rcum=0;
for (int i=topIndex+1; i<=l; i++){
Vertex* n=path[i].n;
rcum+=(usePreconditioner) ? path[i].translationalWeight/totTW : 1./(double)l;
double fraction=rcum;
Translation offset= dt*fraction;
Translation T=n->transformation.translation()+offset;
n->transformation.setTranslation(T);
}
assert(fabs(lcum+rcum)-1<1e-6);
recomputeParameters(v1, top);
recomputeParameters(v2, top);
//END: Translational Error
onStepFinished(e);
if (verboseLevel>2){
Rotation newRotResidual=v2->transformation.rotation().inverse()*(v1->transformation.rotation()*re);
Translation newRotResidualAxis=newRotResidual.axis();
double newRotResidualAngle=newRotResidual.angle();
Translation rotResidualAxis=rR.axis();
double rotResidualAngle=rR.angle();
Translation newTransResidual=(v1->transformation*e->transformation).translation()-v2->transformation.translation();
cerr << "RotationalFraction: " << rotationFactor << endl;
cerr << "Rotational residual: "
<< " axis " << rotResidualAxis.x() << "\t" << rotResidualAxis.y() << "\t" << rotResidualAxis.z() << " --> "
<< " -> " << newRotResidualAxis.x() << "\t" << newRotResidualAxis.y() << "\t" << newRotResidualAxis.z() << endl;
cerr << " angle " << rotResidualAngle << "\t" << newRotResidualAngle << endl;
cerr << "Translational Fraction: " << translationFactor << endl;
cerr << "Translational Residual" << endl;
cerr << " " << tR.x() << "\t" << tR.y() << "\t" << tR.z() << endl;
cerr << " " << newTransResidual.x() << "\t" << newTransResidual.y() << "\t" << newTransResidual.z() << endl;
}
if (verboseLevel>101){
char filename [1000];
sprintf(filename, "po-%02d-%03d-%03d-.dat", iteration, v1->id, v2->id);
recomputeAllTransformations();
saveGnuplot(filename);
}
}
onIterationFinished(iteration);
}
};//namespace AISNavigation
corelib/src/util3d.cpp
+1706
View File
File diff suppressed because it is too large Load Diff
examples/CMakeLists.txt
+5 -1
View File
@@ -6,16 +6,20 @@ SET(SRC_FILES
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
SET(LIBRARIES
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
)
add_definitions(${PCL_DEFINITIONS})
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(example ${SRC_FILES})
TARGET_LINK_LIBRARIES(example rtabmap_corelib ${LIBRARIES})
TARGET_LINK_LIBRARIES(example rtabmap_core ${LIBRARIES})
SET_TARGET_PROPERTIES( example
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-example)
guilib/include/rtabmap/gui/CloudViewer.h
+138
View File
@@ -0,0 +1,138 @@
/*
* CloudViewer.h
*
* Created on: 2013-10-13
* Author: Mathieu
*/
#ifndef CLOUDVIEWER_H_
#define CLOUDVIEWER_H_
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include <QVTKWidget.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/PolygonMesh.h>
#include "rtabmap/core/Transform.h"
#include <QtCore/QMap>
#include <pcl/PCLPointCloud2.h>
namespace pcl {
namespace visualization {
class PCLVisualizer;
}
}
class QMenu;
namespace rtabmap {
class RTABMAPGUI_EXP CloudViewer : public QVTKWidget
{
Q_OBJECT
public:
CloudViewer(QWidget * parent = 0);
virtual ~CloudViewer();
bool updateCloudPose(
const std::string & id,
const Transform & pose); //including mesh
bool updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
bool updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
bool addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
bool addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
bool addCloud(
const std::string & id,
const pcl::PCLPointCloud2Ptr & binaryCloud,
const Transform & pose,
bool rgb);
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
bool addCloudMesh(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons,
const Transform & pose = Transform::getIdentity());
bool addCloudMesh(
const std::string & id,
const pcl::PolygonMesh::Ptr & mesh,
const Transform & pose = Transform::getIdentity());
void updateCameraPosition(
const Transform & pose);
void setTrajectoryShown(bool shown);
void setTrajectorySize(int value);
void removeAllClouds(); //including meshes
bool removeCloud(const std::string & id); //including mesh
bool getPose(const std::string & id, Transform & pose); //including meshes
const QMap<std::string, Transform> & getAddedClouds() {return _addedClouds;} //including meshes
public slots:
void render();
void setBackgroundColor(const QColor & color);
void setCloudVisibility(const std::string & id, bool isVisible);
void setCloudOpacity(const std::string & id, double opacity = 1.0);
void setCloudPointSize(const std::string & id, int size);
protected:
virtual void contextMenuEvent(QContextMenuEvent * event);
virtual void handleAction(QAction * event);
QMenu * menu() {return _menu;}
private:
void createMenu();
private:
pcl::visualization::PCLVisualizer * _visualizer;
QAction * _aLockCamera;
QAction * _aFollowCamera;
QAction * _aResetCamera;
QAction * _aLockViewZ;
QAction * _aShowTrajectory;
QAction * _aSetTrajectorySize;
QAction * _aClearTrajectory;
QAction * _aShowGrid;
QMenu * _menu;
pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
unsigned int _maxTrajectorySize;
QMap<std::string, Transform> _addedClouds; // include meshes
Transform _lastPose;
std::list<std::string> _gridLines;
};
} /* namespace rtabmap */
#endif /* CLOUDVIEWER_H_ */
guilib/include/rtabmap/gui/DataRecorder.h
+48
View File
@@ -0,0 +1,48 @@
/*
* DataRecorder.h
*
* Created on: 2013-10-30
* Author: Mathieu
*/
#ifndef DATARECORDER_H_
#define DATARECORDER_H_
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include <rtabmap/utilite/UEventsHandler.h>
#include <QtGui/QWidget>
#include <rtabmap/core/Image.h>
#include <rtabmap/utilite/UTimer.h>
namespace rtabmap {
class Memory;
class ImageView;
class RTABMAPGUI_EXP DataRecorder : public QWidget, public UEventsHandler
{
Q_OBJECT
public:
DataRecorder(QWidget * parent = 0);
bool init(const QString & path);
void close();
virtual ~DataRecorder();
public slots:
void addData(const rtabmap::Image & image);
void showImage(const rtabmap::Image & image);
protected:
void handleEvent(UEvent * event);
private:
Memory * memory_;
ImageView* imageView_;
UTimer timer_;
int dataQueue_;
};
} /* namespace rtabmap */
#endif /* DATARECORDER_H_ */
guilib/include/rtabmap/gui/DatabaseViewer.h
+4 -3
View File
@@ -39,6 +39,7 @@ class QLabel;
namespace rtabmap
{
class Memory;
class ImageView;
}
class RTABMAP_EXP DatabaseViewer : public QMainWindow
@@ -54,6 +55,7 @@ private slots:
void openDatabase();
void generateGraph();
void generateLocalGraph();
void generate3DMap();
void sliderAValueChanged(int);
void sliderBValueChanged(int);
void sliderAMoved(int);
@@ -61,19 +63,18 @@ private slots:
private:
void updateIds();
QImage ipl2QImage(const IplImage *newImage);
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene);
void update(int value,
QLabel * labelIndex,
QLabel * labelActions,
QLabel * labelParents,
QLabel * labelChildren,
QGraphicsView * view,
rtabmap::ImageView * view,
QLabel * labelId);
private:
Ui_DatabaseViewer * ui_;
QMap<int, QByteArray> imagesMap_;
QMap<int, QByteArray> depthImagesMap_;
QList<int> ids_;
rtabmap::Memory * memory_;
QString pathDatabase_;
guilib/include/rtabmap/gui/ImageView.h
+21 -3
View File
@@ -20,17 +20,21 @@
#ifndef IMAGEVIEW_H_
#define IMAGEVIEW_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include <QtGui/QGraphicsView>
#include <QtCore/QRectF>
#include <opencv2/features2d/features2d.hpp>
#include <map>
class QAction;
class QMenu;
namespace rtabmap {
class RTABMAP_EXP ImageView : public QGraphicsView {
class KeypointItem;
class RTABMAPGUI_EXP ImageView : public QGraphicsView {
Q_OBJECT
@@ -41,12 +45,21 @@ public:
void resetZoom();
bool isImageShown();
bool isImageDepthShown();
bool isFeaturesShown();
bool isLinesShown();
void setFeaturesShown(bool shown);
void setImageShown(bool shown);
void setImageDepthShown(bool shown);
void setLinesShown(bool shown);
void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords);
void setImage(const QImage & image);
void setImageDepth(const QImage & image);
void clear();
protected:
virtual void contextMenuEvent(QContextMenuEvent * e);
virtual void wheelEvent(QWheelEvent * e);
@@ -55,7 +68,7 @@ private slots:
void updateZoom();
private:
void updateItemsShown();
void updateOpacity();
private:
int _zoom;
@@ -64,9 +77,14 @@ private:
QMenu * _menu;
QAction * _showImage;
QAction * _showImageDepth;
QAction * _showFeatures;
QAction * _showLines;
QAction * _saveImage;
QList<rtabmap::KeypointItem *> _features;
QGraphicsPixmapItem * _image;
QGraphicsPixmapItem * _imageDepth;
};
}
guilib/include/rtabmap/gui/LoopClosureViewer.h
+60
View File
@@ -0,0 +1,60 @@
/*
* LoopClosureViewer.h
*
* Created on: 2013-10-21
* Author: Mathieu
*/
#ifndef LOOPCLOSUREVIEWER_H_
#define LOOPCLOSUREVIEWER_H_
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Transform.h>
#include <opencv2/opencv.hpp>
#include <QtGui/QWidget>
class Ui_loopClosureViewer;
namespace rtabmap {
class Signature;
class RTABMAPGUI_EXP LoopClosureViewer : public QWidget {
Q_OBJECT
public:
LoopClosureViewer(QWidget * parent);
virtual ~LoopClosureViewer();
// take ownership
void setData(Signature * sA, Signature * sB); // sB contains loop transform as pose() from sA
const Signature * sA() const {return sA_;}
const Signature * sB() const {return sB_;}
public slots:
void setDecimation(int decimation) {decimation_ = decimation;}
void setMaxDepth(int maxDepth) {maxDepth_ = maxDepth;}
void setSamples(int samples) {samples_ = samples;}
void updateView(const Transform & AtoB = Transform());
protected:
virtual void showEvent(QShowEvent * event);
private:
Ui_loopClosureViewer * ui_;
Signature * sA_;
Signature * sB_;
Transform transform_;
int decimation_;
float maxDepth_;
int samples_;
};
} /* namespace rtabmap */
#endif /* LOOPCLOSUREVIEWER_H_ */
guilib/include/rtabmap/gui/MainWindow.h
+62 -5
View File
@@ -26,11 +26,19 @@
#include <QtGui/QMainWindow>
#include <QtCore/QSet>
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/Image.h"
#include "rtabmap/gui/PreferencesDialog.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PolygonMesh.h>
namespace rtabmap {
class CameraThread;
class DBReader;
class CameraOpenni;
class OdometryThread;
class CloudViewer;
}
class QGraphicsScene;
@@ -57,7 +65,8 @@ public:
kStartingDetection,
kDetecting,
kPaused,
kMonitoring
kMonitoring,
kMonitoringPaused
};
enum SrcType {
@@ -76,9 +85,10 @@ public:
virtual ~MainWindow();
QString getWorkingDirectory() const;
void setMonitoringState(bool pauseChecked = false); // in monitoring state, only some actions are enabled
public slots:
void changeState(MainWindow::State state);
void processStats(const rtabmap::Statistics & stat);
protected:
virtual void closeEvent(QCloseEvent* event);
@@ -86,6 +96,7 @@ protected:
virtual void resizeEvent(QResizeEvent* anEvent);
private slots:
void changeState(MainWindow::State state);
void beep();
void startDetection();
void pauseDetection();
@@ -100,21 +111,27 @@ private slots:
void selectVideo();
void selectStream();
void selectDatabase();
void selectOpenni();
void resetTheMemory();
void dumpTheMemory();
void dumpThePrediction();
void downloadAllClouds();
void clearTheCache();
void saveFigures();
void loadFigures();
void openPreferences();
void selectScreenCaptureFormat(bool checked);
void updateElapsedTime();
void processStats(const rtabmap::Statistics & stat);
void processOdometry(const rtabmap::Image & data);
void applyAllPrefSettings();
void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
void applyPrefSettings(const rtabmap::ParametersMap & parameters);
void processRtabmapEventInit(int status, const QString & info);
void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
void changeImgRateSetting();
void changeDetectionRateSetting();
void changeTimeLimitSetting();
void changeMappingMode();
void captureScreen();
void setAspectRatio(int w, int h);
void setAspectRatio16_9();
@@ -125,23 +142,53 @@ private slots:
void setAspectRatio480p();
void setAspectRatio720p();
void setAspectRatio1080p();
void savePointClouds();
void saveMeshes();
void viewPointClouds();
void viewMeshes();
void resetOdometry();
void triggerNewMap();
signals:
void statsReceived(const rtabmap::Statistics &);
void odometryReceived(const rtabmap::Image &);
void thresholdsChanged(int, int);
void stateChanged(MainWindow::State);
void rtabmapEventInitReceived(int status, const QString & info);
void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
void imgRateChanged(double);
void detectionRateChanged(double);
void timeLimitChanged(float);
void mappingModeChanged(bool);
void noMoreImagesReceived();
void loopClosureThrChanged(float);
void twistReceived(float x, float y, float z, float roll, float pitch, float yaw, int row, int col);
private:
void update3DMapVisibility(bool cloudsShown, bool scansShown);
void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose);
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
void setupMainLayout(bool vertical);
void updateSelectSourceImageMenu(int type);
void updateSelectSourceDatabase(bool used);
void updateSelectSourceOpenni(bool used);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createAssembledCloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
int id,
const cv::Mat & rgb,
const cv::Mat & depth,
float depthConstant,
const Transform & localTransform,
const Transform & pose,
float voxelSize,
int decimation,
float maxDepth);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > createPointClouds();
std::map<int, pcl::PolygonMesh::Ptr> createMeshes();
void savePointClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
void saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes);
private:
Ui_mainWindow * _ui;
@@ -149,6 +196,8 @@ private:
State _state;
rtabmap::CameraThread * _camera;
rtabmap::DBReader * _dbReader;
rtabmap::CameraOpenni * _cameraOpenni;
rtabmap::OdometryThread * _odomThread;
SrcType _srcType;
QString _srcPath;
@@ -160,8 +209,17 @@ private:
QSet<int> _lastIds;
int _lastId;
bool _processingStatistics;
bool _odometryReceived;
QMap<int, QByteArray> _imagesMap;
QMap<int, std::vector<unsigned char> > _imagesMap;
QMap<int, std::vector<unsigned char> > _depthsMap;
QMap<int, std::vector<unsigned char> > _depths2DMap;
QMap<int, float> _depthConstantsMap;
QMap<int, Transform> _localTransformsMap;
std::map<int, Transform> _currentPosesMap;
Transform _odometryCorrection;
Transform _lastOdomPose;
bool _lastOdometryProcessed;
QTimer * _oneSecondTimer;
QTime * _elapsedTime;
@@ -172,7 +230,6 @@ private:
PdfPlotCurve * _rawLikelihoodCurve;
DetailedProgressDialog * _initProgressDialog;
QActionGroup * _selectSourceImageGrp;
QString _graphSavingFileName;
QString _autoScreenCaptureFormat;
guilib/include/rtabmap/gui/OdometryViewer.h
+52
View File
@@ -0,0 +1,52 @@
/*
* OdometryViewer.h
*
* Created on: 2013-10-15
* Author: Mathieu
*/
#ifndef ODOMETRYVIEWER_H_
#define ODOMETRYVIEWER_H_
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include "rtabmap/core/Image.h"
#include "rtabmap/gui/CloudViewer.h"
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UMutex.h"
namespace rtabmap {
class RTABMAPGUI_EXP OdometryViewer : public CloudViewer, public UEventsHandler
{
Q_OBJECT
public:
OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, QWidget * parent = 0);
virtual ~OdometryViewer() {}
protected:
void handleAction(QAction * a);
virtual void handleEvent(UEvent * event);
private slots:
void processData();
private:
UMutex dataMutex_;
std::list<rtabmap::Image> buffer_;
UTimer timer_;
int maxClouds_;
float voxelSize_;
int decimation_;
int id_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
QAction * _aSetVoxelSize;
QAction * _aSetDecimation;
QAction * _aSetCloudHistorySize;
QAction * _aPause;
};
} /* namespace rtabmap */
#endif /* ODOMETRYVIEWER_H_ */
guilib/include/rtabmap/gui/PreferencesDialog.h
+79 -11
View File
@@ -20,12 +20,14 @@
#ifndef PREFERENCESDIALOG_H_
#define PREFERENCESDIALOG_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include <QtGui/QDialog>
#include <QtCore/QModelIndex>
#include <QtCore/QVector>
#include <set>
#include "rtabmap/core/Transform.h"
#include "rtabmap/core/Parameters.h"
class Ui_preferencesDialog;
@@ -40,22 +42,30 @@ class QLineEdit;
class QSlider;
class QProgressDialog;
class UPlotCurve;
class QStackedWidget;
class QCheckBox;
class QSpinBox;
class QDoubleSpinBox;
namespace rtabmap {
class RTABMAP_EXP PreferencesDialog : public QDialog
class CameraOpenni;
class OdometryThread;
class Signature;
class LoopClosureViewer;
class RTABMAPGUI_EXP PreferencesDialog : public QDialog
{
Q_OBJECT
public:
enum PanelFlag {
kPanelDummy = 0,
kPanelGeneralStrategy = 1,
kPanelGeneral = 2,
kPanelFourier = 4,
kPanelSurf = 8,
kPanelSource = 16,
kPanelAll = 31
kPanelGeneral = 1,
kPanelCloudRendering = 2,
kPanelLogging = 4,
kPanelSource = 8,
kPanelAll = 15
};
// TODO, tried to change the name of PANEL_FLAGS to PanelFlags... but signals/slots errors appeared...
Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag);
@@ -67,11 +77,17 @@ public:
kSrcVideo
};
enum OdomTest {
kOdomBIN,
kOdomBOW,
kOdomICP
};
public:
PreferencesDialog(QWidget * parent = 0);
virtual ~PreferencesDialog();
virtual QString getIniFilePath();
virtual QString getIniFilePath() const;
void init();
void saveWindowGeometry(const QString & windowName, const QWidget * window);
@@ -96,12 +112,28 @@ public:
bool imageHighestHypShown() const;
bool beepOnPause() const;
int getKeypointsOpacity() const;
QString getWorkingDirectory();
bool isCloudMeshing(int index) const; // 0=map
bool isCloudsShown(int index) const; // 0=map, 1=odom, 2=save
double getCloudVoxelSize(int index) const; // 0=map, 1=odom, 2=save
int getCloudDecimation(int index) const; // 0=map, 1=odom, 2=save
double getCloudMaxDepth(int index) const; // 0=map, 1=odom, 2=save
double getCloudOpacity(int index) const; // 0=map, 1=odom, 2=save
int getCloudPointSize(int index) const; // 0=map, 1=odom, 2=save
bool isScansShown(int index) const; // 0=map, 1=odom, 2=save
double getScanOpacity(int index) const; // 0=map, 1=odom, 2=save
int getScanPointSize(int index) const; // 0=map, 1=odom, 2=save
QString getWorkingDirectory() const;
// source panel
double getGeneralInputRate() const;
bool isSourceImageUsed() const;
bool isSourceDatabaseUsed() const;
bool isSourceOpenniUsed() const;
bool isSourceOpenniOdometryBIN() const;
bool isSourceOpenniOdometryBOW() const;
bool getGeneralAutoRestart() const;
bool getGeneralCameraKeypoints() const;
int getSourceImageType() const;
@@ -117,13 +149,18 @@ public:
QString getSourceVideoPath() const; //Video group
int getSourceUsbDeviceId() const; //UsbDevice group
QString getSourceDatabasePath() const; //Database group
bool getSourceDatabaseOdometryIgnored() const; //Database group
int getSourceDatabaseStartPos() const; //Database group
QString getSourceOpenniDevice() const; //Openni group
Transform getSourceOpenniLocalTransform() const; //Openni group
int getIgnoredDCComponents() const;
//
bool isImagesKept() const;
float getTimeLimit() const;
float getDetectionRate() const;
bool isSLAMMode() const;
//specific
bool isStatisticsPublished() const;
@@ -132,7 +169,7 @@ public:
double getExpThr() const;
//
void disableGeneralCameraKeypoints();
void setMonitoringState(bool monitoringState) {_monitoringState = monitoringState;}
signals:
void settingsChanged(PreferencesDialog::PANEL_FLAGS);
@@ -140,11 +177,14 @@ signals:
public slots:
void setInputRate(double value);
void setDetectionRate(double value);
void setHardThr(int value);
void setAutoRestart(bool value);
void setTimeLimit(float value);
void setSLAMMode(bool enabled);
void selectSourceImage(Src src = kSrcUndef);
void selectSourceDatabase(bool user = false);
void selectSourceOpenni(bool user = false);
private slots:
void closeDialog ( QAbstractButton * button );
@@ -153,22 +193,29 @@ private slots:
void loadConfigFrom();
void saveConfigTo();
void makeObsoleteGeneralPanel();
void makeObsoleteCloudRenderingPanel();
void makeObsoleteLoggingPanel();
void makeObsoleteSourcePanel();
void clicked(const QModelIndex &index);
void addParameter(int value);
void addParameter(bool value);
void addParameter(double value);
void addParameter(const QString & value);
void updatePredictionPlot();
void updateKpROI();
void changeDatabasePath();
void changeWorkingDirectory();
void changeDictionaryPath();
void readSettingsEnd();
void setupTreeView();
void updateBasicParameter();
void openDatabaseViewer();
void cleanOdometryTest();
void testSourceOdometry();
protected:
virtual void showEvent ( QShowEvent * event );
virtual void closeEvent(QCloseEvent *event);
void setParameter(const std::string & key, const std::string & value);
@@ -189,12 +236,17 @@ private:
void setupSignals();
void setupKpRoiPanel();
bool parseModel(QList<QGroupBox*> & boxes, QStandardItem * parentItem, int currentLevel, int & absoluteIndex);
void resetSettings(QGroupBox * groupBox);
void addParameter(const QObject * object, int value);
void addParameter(const QObject * object, bool value);
void addParameter(const QObject * object, double value);
void addParameter(const QObject * object, const QString & value);
void addParameters(const QObjectList & children);
void addParameters(const QStackedWidget * stackedWidget);
void addParameters(const QGroupBox * box);
QList<QGroupBox*> getGroupBoxes();
void readSettingsBegin();
void testOdometry(OdomTest test);
protected:
rtabmap::ParametersMap _parameters;
@@ -204,8 +256,24 @@ private:
Ui_preferencesDialog * _ui;
QStandardItemModel * _indexModel;
bool _initialized;
bool _monitoringState;
QProgressDialog * _progressDialog;
//Odometry test
CameraOpenni * _odomCamera;
OdometryThread * _odomThread;
QVector<QCheckBox*> _3dRenderingShowClouds;
QVector<QDoubleSpinBox*> _3dRenderingVoxelSize;
QVector<QSpinBox*> _3dRenderingDecimation;
QVector<QDoubleSpinBox*> _3dRenderingMaxDepth;
QVector<QDoubleSpinBox*> _3dRenderingOpacity;
QVector<QSpinBox*> _3dRenderingPtSize;
QVector<QCheckBox*> _3dRenderingShowScans;
QVector<QDoubleSpinBox*> _3dRenderingOpacityScan;
QVector<QSpinBox*> _3dRenderingPtSizeScan;
QVector<QCheckBox*> _3dRenderingMeshing;
};
Q_DECLARE_OPERATORS_FOR_FLAGS(PreferencesDialog::PANEL_FLAGS)
guilib/include/rtabmap/gui/UCv2Qt.h
+177
View File
@@ -0,0 +1,177 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UCV2QT_H_
#define UCV2QT_H_
#include <QtGui/QImage>
#include <opencv2/core/core.hpp>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UThread.h>
/**
* Convert a cv::Mat image to a QImage. Support
* depth (float32, uint16) image and RGB/BGR 8bits images.
* @param image the cv::Mat image (can be 1 channel [CV_8U, CV_16U or CV_32F] or 3 channels [CV_U8])
* @param isBgr if 3 channels, it is BGR or RGB order.
* @return the QImage
*/
inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true)
{
QImage qtemp;
if(!image.empty() && image.depth() == CV_8U)
{
if(image.channels()==3)
{
const unsigned char * data = image.data;
if(image.channels() == 3)
{
qtemp = QImage(image.cols, image.rows, QImage::Format_RGB32);
for(int y = 0; y < image.rows; ++y, data += image.cols*image.elemSize())
{
for(int x = 0; x < image.cols; ++x)
{
QRgb * p = ((QRgb*)qtemp.scanLine (y)) + x;
if(isBgr)
{
*p = qRgb(data[x * image.channels()+2], data[x * image.channels()+1], data[x * image.channels()]);
}
else
{
*p = qRgb(data[x * image.channels()], data[x * image.channels()+1], data[x * image.channels()+2]);
}
}
}
}
}
else if(image.channels() == 1)
{
// mono grayscale
qtemp = QImage(image.data, image.cols, image.rows, image.cols, QImage::Format_Indexed8).copy();
}
else
{
printf("Wrong image format, must have 1 or 3 channels\n");
}
}
else if(image.depth() == CV_32F && image.channels()==1)
{
// Assume depth image (float in meters)
const float * data = (const float *)image.data;
float min=0, max=0;
uMinMax(data, image.rows*image.cols, min, max);
qtemp = QImage(image.cols, image.rows, QImage::Format_Indexed8);
for(int y = 0; y < image.rows; ++y, data += image.cols)
{
for(int x = 0; x < image.cols; ++x)
{
uchar * p = qtemp.scanLine (y) + x;
if(data[x] < min || data[x] > max || uIsNan(data[x]))
{
*p = 0;
}
else
{
*p = uchar(255.0f - ((data[x]-min)*255.0f)/(max-min));
if(*p == 255)
{
*p = 0;
}
}
}
}
QVector<QRgb> my_table;
for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
qtemp.setColorTable(my_table);
}
else if(image.depth() == CV_16U && image.channels()==1)
{
// Assume depth image (unsigned short in mm)
const unsigned short * data = (const unsigned short *)image.data;
unsigned short min=data[0], max=data[0];
for(unsigned int i=1; i<image.total(); ++i)
{
if(!uIsNan(data[i]) && data[i] > 0)
{
if((uIsNan(min) && data[i] > 0) ||
(data[i] > 0 && data[i]<min))
{
min = data[i];
}
if((uIsNan(max) && data[i] > 0) ||
(data[i] > 0 && data[i]>max))
{
max = data[i];
}
}
}
qtemp = QImage(image.cols, image.rows, QImage::Format_Indexed8);
for(int y = 0; y < image.rows; ++y, data += image.cols)
{
for(int x = 0; x < image.cols; ++x)
{
uchar * p = qtemp.scanLine (y) + x;
if(data[x] < min || data[x] > max || uIsNan(data[x]) || max == min)
{
*p = 0;
}
else
{
*p = uchar(255.0f - (float(data[x]-min)/float(max-min))*255.0f);
if(*p == 255)
{
*p = 0;
}
}
}
}
QVector<QRgb> my_table;
for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
qtemp.setColorTable(my_table);
}
else if(!image.empty() && image.depth() != CV_8U)
{
printf("Wrong image format, must be 8_bits/3channels or (depth) 32bitsFloat/1channel, 16bits/1channel\n");
}
return qtemp;
}
class UCvMat2QImageThread : public UThread
{
public:
UCvMat2QImageThread(const cv::Mat & image, bool isBgr = true) :
image_(image),
isBgr_(isBgr) {}
QImage & getQImage() {return qtImage_;}
protected:
virtual void mainLoop()
{
qtImage_ = uCvMat2QImage(image_, isBgr_);
this->kill();
}
private:
cv::Mat image_;
bool isBgr_;
QImage qtImage_;
};
#endif /* UCV2QT_H_ */
guilib/src/AboutDialog.cpp
+7 -1
View File
@@ -21,6 +21,7 @@
#include "rtabmap/core/Rtabmap.h"
#include "ui_aboutDialog.h"
#include <opencv2/core/version.hpp>
#include <pcl/pcl_config.h>
namespace rtabmap {
@@ -29,8 +30,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
{
_ui = new Ui_aboutDialog();
_ui->setupUi(this);
_ui->label_version->setText(Rtabmap::getVersion().c_str());
QString version = Rtabmap::getVersion().c_str();
#if DEMO_BUILD
version.append(" [DEMO]");
#endif
_ui->label_version->setText(version);
_ui->label_opencv_version->setText(CV_VERSION);
_ui->label_pcl_version->setText(PCL_VERSION_PRETTY);
}
AboutDialog::~AboutDialog()
guilib/src/CMakeLists.txt
+26 -9
View File
@@ -12,6 +12,10 @@ SET(headers_ui
./StatsToolBox.h
./DetailedProgressDialog.h
./utilite/UPlot.h
../include/${PROJECT_PREFIX}/gui/CloudViewer.h
../include/${PROJECT_PREFIX}/gui/OdometryViewer.h
../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h
../include/${PROJECT_PREFIX}/gui/DataRecorder.h
)
SET(uis
@@ -20,6 +24,7 @@ SET(uis
./ui/aboutDialog.ui
./ui/consoleWidget.ui
./ui/DatabaseViewer.ui
./ui/loopClosureViewer.ui
)
SET(qrc
@@ -43,7 +48,6 @@ SET(SRC_FILES
./MainWindow.cpp
./PreferencesDialog.cpp
./KeypointItem.cpp
./qtipl.cpp
./ImageView.cpp
./PdfPlot.cpp
./StatsToolBox.cpp
@@ -52,6 +56,10 @@ SET(SRC_FILES
./ConsoleWidget.cpp
./DatabaseViewer.cpp
./utilite/UPlot.cpp
./CloudViewer.cpp
./OdometryViewer.cpp
./LoopClosureViewer.cpp
./DataRecorder.cpp
${moc_srcs}
${moc_uis}
${srcs_qrc}
@@ -64,33 +72,42 @@ SET(INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}
${OpenCV_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
${PCL_INCLUDE_DIRS}
)
INCLUDE(${QT_USE_FILE})
INCLUDE(${VTK_USE_FILE})
SET(LIBRARIES
${QT_LIBRARIES}
${OpenCV_LIBS}
${PCL_LIBRARIES}
QVTK
vtkHybrid
)
#include files
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
# create a library from the source files
ADD_LIBRARY(rtabmap_guilib ${SRC_FILES})
ADD_LIBRARY(rtabmap_gui ${SRC_FILES})
# Linking with Qt libraries
TARGET_LINK_LIBRARIES(rtabmap_guilib rtabmap_corelib rtabmap_utilite ${LIBRARIES})
TARGET_LINK_LIBRARIES(rtabmap_gui rtabmap_core rtabmap_utilite ${LIBRARIES})
SET_TARGET_PROPERTIES(
rtabmap_guilib
rtabmap_gui
PROPERTIES
OUTPUT_NAME ${PROJECT_PREFIX}_gui
INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
)
INSTALL(TARGETS rtabmap_guilib
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)
INSTALL(TARGETS rtabmap_gui
EXPORT RTABMapTargets
RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime
LIBRARY DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel
ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel)
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION include/ COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION "${INSTALL_INCLUDE_DIR}" COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
guilib/src/CloudViewer.cpp
+558
View File
@@ -0,0 +1,558 @@
/*
* CloudViewer.cpp
*
* Created on: 2013-10-13
* Author: Mathieu
*/
#include "rtabmap/gui/CloudViewer.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/util3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <QtGui/QMenu>
#include <QtGui/QAction>
#include <QtGui/QContextMenuEvent>
#include <QtGui/QInputDialog>
#include <QtGui/QWheelEvent>
#include <vtkRenderWindow.h>
namespace rtabmap {
CloudViewer::CloudViewer(QWidget *parent) :
QVTKWidget(parent),
#if defined(WIN32) || defined(__APPLE__)
_visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer")),
#else
_visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer", false)),
#endif
_aLockCamera(0),
_aFollowCamera(0),
_aResetCamera(0),
_aLockViewZ(0),
_aShowTrajectory(0),
_aSetTrajectorySize(0),
_aClearTrajectory(0),
_aShowGrid(0),
_menu(0),
_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
_maxTrajectorySize(100),
_lastPose(Transform::getIdentity())
{
this->setMinimumSize(200, 200);
this->SetRenderWindow(_visualizer->getRenderWindow());
#if !defined(WIN32) && !defined(__APPLE__)
_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
#endif
_visualizer->setCameraPosition(
-1, 0, 0,
0, 0, 0,
0, 0, 1);
//setup menu/actions
createMenu();
}
CloudViewer::~CloudViewer()
{
UDEBUG("");
//_visualizer->close();
delete _visualizer;
}
void CloudViewer::createMenu()
{
_aLockCamera = new QAction("Lock target", this);
_aLockCamera->setCheckable(true);
_aLockCamera->setChecked(false);
_aFollowCamera = new QAction("Follow", this);
_aFollowCamera->setCheckable(true);
_aFollowCamera->setChecked(true);
QAction * freeCamera = new QAction("Free", this);
freeCamera->setCheckable(true);
freeCamera->setChecked(false);
_aLockViewZ = new QAction("Lock view Z", this);
_aLockViewZ->setCheckable(true);
_aLockViewZ->setChecked(true);
_aResetCamera = new QAction("Reset position", this);
_aShowTrajectory= new QAction("Show trajectory", this);
_aShowTrajectory->setCheckable(true);
_aShowTrajectory->setChecked(true);
_aSetTrajectorySize = new QAction("Set trajectory size...", this);
_aClearTrajectory = new QAction("Clear trajectory", this);
_aShowGrid = new QAction("Show grid", this);
_aShowGrid->setCheckable(true);
QMenu * cameraMenu = new QMenu("Camera", this);
cameraMenu->addAction(_aLockCamera);
cameraMenu->addAction(_aFollowCamera);
cameraMenu->addAction(freeCamera);
cameraMenu->addSeparator();
cameraMenu->addAction(_aLockViewZ);
cameraMenu->addAction(_aResetCamera);
QActionGroup * group = new QActionGroup(this);
group->addAction(_aLockCamera);
group->addAction(_aFollowCamera);
group->addAction(freeCamera);
QMenu * trajectoryMenu = new QMenu("Trajectory", this);
trajectoryMenu->addAction(_aShowTrajectory);
trajectoryMenu->addAction(_aSetTrajectorySize);
trajectoryMenu->addAction(_aClearTrajectory);
//menus
_menu = new QMenu(this);
_menu->addMenu(cameraMenu);
_menu->addMenu(trajectoryMenu);
_menu->addAction(_aShowGrid);
}
bool CloudViewer::updateCloudPose(
const std::string & id,
const Transform & pose)
{
if(_addedClouds.contains(id))
{
UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
if(_addedClouds.find(id).value() == pose ||
_visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose)))
{
_addedClouds.find(id).value() = pose;
return true;
}
}
return false;
}
bool CloudViewer::updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose)
{
if(_addedClouds.contains(id))
{
UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
int index = _visualizer->getColorHandlerIndex(id);
this->removeCloud(id);
if(this->addCloud(id, cloud, pose))
{
_visualizer->updateColorHandlerIndex(id, index);
return true;
}
}
return false;
}
bool CloudViewer::updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose)
{
if(_addedClouds.contains(id))
{
UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
int index = _visualizer->getColorHandlerIndex(id);
this->removeCloud(id);
if(this->addCloud(id, cloud, pose))
{
_visualizer->updateColorHandlerIndex(id, index);
return true;
}
}
return false;
}
bool CloudViewer::addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose)
{
if(!updateCloud(id, cloud, pose))
{
return addCloud(id, cloud, pose);
}
return true;
}
bool CloudViewer::addOrUpdateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose)
{
if(!updateCloud(id, cloud, pose))
{
return addCloud(id, cloud, pose);
}
return true;
}
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PCLPointCloud2Ptr & binaryCloud,
const Transform & pose,
bool rgb)
{
if(!_addedClouds.contains(id))
{
Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
Eigen::Quaternionf orientation = Eigen::Quaternionf(util3d::transformToEigen3f(pose).rotation());
// add random color channel
pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id))
{
// white
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, 255, 255, 255));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
// x,y,z
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
if(rgb)
{
//rgb
colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
_visualizer->updateColorHandlerIndex(id, 5);
}
_addedClouds.insert(id, pose);
return true;
}
}
return false;
}
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose)
{
if(!_addedClouds.contains(id))
{
UDEBUG("Adding %s with %d points", id.c_str(), (int)cloud->size());
pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
pcl::toPCLPointCloud2(*cloud, *binaryCloud);
return addCloud(id, binaryCloud, pose, true);
}
return false;
}
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose)
{
if(!_addedClouds.contains(id))
{
UDEBUG("Adding %s with %d points", id.c_str(), (int)cloud->size());
pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
pcl::toPCLPointCloud2(*cloud, *binaryCloud);
return addCloud(id, binaryCloud, pose, false);
}
return false;
}
bool CloudViewer::addCloudMesh(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons,
const Transform & pose)
{
if(!_addedClouds.contains(id))
{
UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id))
{
_visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose));
_addedClouds.insert(id, pose);
return true;
}
}
return false;
}
bool CloudViewer::addCloudMesh(
const std::string & id,
const pcl::PolygonMesh::Ptr & mesh,
const Transform & pose)
{
if(!_addedClouds.contains(id))
{
UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
if(_visualizer->addPolygonMesh(*mesh, id))
{
_visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose));
_addedClouds.insert(id, pose);
return true;
}
}
return false;
}
void CloudViewer::setTrajectoryShown(bool shown)
{
_aShowTrajectory->setChecked(shown);
}
void CloudViewer::setTrajectorySize(int value)
{
_maxTrajectorySize = value;
}
void CloudViewer::removeAllClouds()
{
_addedClouds.clear();
_visualizer->removeAllPointClouds();
}
bool CloudViewer::removeCloud(const std::string & id)
{
_addedClouds.remove(id);
return _visualizer->removePointCloud(id);
}
bool CloudViewer::getPose(const std::string & id, Transform & pose)
{
if(_addedClouds.contains(id))
{
pose = _addedClouds.value(id);
return true;
}
return false;
}
void CloudViewer::updateCameraPosition(const Transform & pose)
{
if(!pose.isNull())
{
Eigen::Affine3f m = util3d::transformToEigen3f(pose);
Eigen::Vector3f pos = m.translation();
Eigen::Vector3f lastPos(0,0,0);
if(_trajectory->size())
{
lastPos[0]=_trajectory->back().x;
lastPos[1]=_trajectory->back().y;
lastPos[2]=_trajectory->back().z;
}
_trajectory->push_back(pcl::PointXYZ(pos[0], pos[1], pos[2]));
if(_maxTrajectorySize>0)
{
while(_trajectory->size() > _maxTrajectorySize)
{
_trajectory->erase(_trajectory->begin());
}
}
if(_aShowTrajectory->isChecked())
{
_visualizer->removeShape("trajectory");
pcl::PolygonMesh mesh;
pcl::Vertices vertices;
vertices.vertices.resize(_trajectory->size());
for(unsigned int i=0; i<vertices.vertices.size(); ++i)
{
vertices.vertices[i] = i;
}
pcl::toPCLPointCloud2(*_trajectory, mesh.cloud);
mesh.polygons.push_back(vertices);
_visualizer->addPolylineFromPolygonMesh(mesh, "trajectory");
}
if(pose != _lastPose)
{
std::vector<pcl::visualization::Camera> cameras;
_visualizer->getCameras(cameras);
if(_aLockCamera->isChecked())
{
//update camera position
Eigen::Vector3f diff = pos - Eigen::Vector3f(_lastPose.x(), _lastPose.y(), _lastPose.z());
cameras.front().pos[0] += diff[0];
cameras.front().pos[1] += diff[1];
cameras.front().pos[2] += diff[2];
cameras.front().focal[0] += diff[0];
cameras.front().focal[1] += diff[1];
cameras.front().focal[2] += diff[2];
}
else if(_aFollowCamera->isChecked())
{
Eigen::Vector3f vPosToFocal = Eigen::Vector3f(cameras.front().focal[0] - cameras.front().pos[0],
cameras.front().focal[1] - cameras.front().pos[1],
cameras.front().focal[2] - cameras.front().pos[2]).normalized();
Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
Eigen::Vector3f xAxis = yAxis.cross(zAxis);
Transform PR(xAxis[0], xAxis[1], xAxis[2],0,
yAxis[0], yAxis[1], yAxis[2],0,
zAxis[0], zAxis[1], zAxis[2],0);
Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
PR[4], PR[5], PR[6], cameras.front().pos[1],
PR[8], PR[9], PR[10], cameras.front().pos[2]);
Transform F(PR[0], PR[1], PR[2], cameras.front().focal[0],
PR[4], PR[5], PR[6], cameras.front().focal[1],
PR[8], PR[9], PR[10], cameras.front().focal[2]);
Transform N = pose;
Transform O = _lastPose;
Transform O2N = O.inverse()*N;
Transform F2O = F.inverse()*O;
Transform T = F2O * O2N * F2O.inverse();
Transform Fp = F * T;
Transform P2F = P.inverse()*F;
Transform Pp = P * P2F * T * P2F.inverse();
cameras.front().pos[0] = Pp.x();
cameras.front().pos[1] = Pp.y();
cameras.front().pos[2] = Pp.z();
cameras.front().focal[0] = Fp.x();
cameras.front().focal[1] = Fp.y();
cameras.front().focal[2] = Fp.z();
//FIXME: the view up is not set properly...
cameras.front().view[0] = Fp[8];
cameras.front().view[1] = Fp[9];
cameras.front().view[2] = Fp[10];
}
if(_aLockViewZ->isChecked())
{
cameras.front().view[0] = 0;
cameras.front().view[1] = 0;
cameras.front().view[2] = 1;
}
_visualizer->setCameraPosition(
cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
}
_visualizer->removeCoordinateSystem();
_visualizer->addCoordinateSystem(0.2, m);
}
_lastPose = pose;
}
void CloudViewer::render()
{
this->GetRenderWindow()->Render();
}
void CloudViewer::setBackgroundColor(const QColor & color)
{
_visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
}
void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
{
pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
if(iter != cloudActorMap->end())
{
iter->second.actor->SetVisibility(isVisible?1:0);
}
else
{
UERROR("Cannot find actor named \"%s\".", id.c_str());
}
}
void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
{
_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
}
void CloudViewer::setCloudPointSize(const std::string & id, int size)
{
_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
}
void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
{
QAction * a = _menu->exec(event->globalPos());
if(a)
{
handleAction(a);
}
}
void CloudViewer::handleAction(QAction * a)
{
if(a == _aSetTrajectorySize)
{
bool ok;
int value = QInputDialog::getInt(this, tr("Set trajectory size"), tr("Size (0=infinite)"), _maxTrajectorySize, 0, 10000, 10, &ok);
if(ok)
{
_maxTrajectorySize = value;
}
}
else if(a == _aClearTrajectory)
{
_trajectory->clear();
_visualizer->removeShape("trajectory");
this->render();
}
else if(a == _aResetCamera)
{
_visualizer->setCameraPosition(
-1, 0, 0,
0, 0, 0,
0, 0, 1);
this->render();
}
else if(a == _aShowGrid)
{
if(_aShowGrid->isChecked())
{
float cellSize = 1.0f;
int cellCount = 50;
double r=0.5;
double g=0.5;
double b=0.5;
int id = 0;
float min = -float(cellCount/2) * cellSize;
float max = float(cellCount/2) * cellSize;
std::string name;
for(float i=min; i<=max; i += cellSize)
{
//over x
name = uFormat("line%d", ++id);
_visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name);
_gridLines.push_back(name);
//over y
name = uFormat("line%d", ++id);
_visualizer->addLine(pcl::PointXYZ(min, i, 0.0f), pcl::PointXYZ(max, i, 0.0f), r, g, b, name);
_gridLines.push_back(name);
}
}
else
{
for(std::list<std::string>::iterator iter = _gridLines.begin(); iter!=_gridLines.end(); ++iter)
{
_visualizer->removeShape(*iter);
}
_gridLines.clear();
}
this->render();
}
}
} /* namespace rtabmap */
guilib/src/DataRecorder.cpp
+130
View File
@@ -0,0 +1,130 @@
/*
* CloudRecorder.cpp
*
* Created on: 2013-10-30
* Author: Mathieu
*/
#include "rtabmap/gui/DataRecorder.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/CameraEvent.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/gui/ImageView.h>
#include <rtabmap/gui/UCv2Qt.h>
#include <QtCore/QMetaType>
#include <QtGui/QHBoxLayout>
namespace rtabmap {
DataRecorder::DataRecorder(QWidget * parent) :
QWidget(parent),
memory_(0),
imageView_(new ImageView(this)),
dataQueue_(0)
{
qRegisterMetaType<rtabmap::Image>("rtabmap::Image");
QHBoxLayout * layout = new QHBoxLayout(this);
layout->addWidget(imageView_);
this->setLayout(layout);
}
bool DataRecorder::init(const QString & path)
{
if(!memory_)
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "-1")); // desactivate keypoints extraction
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true")); // to keep images
memory_ = new Memory();
if(!memory_->init(path.toStdString(), true, customParameters, false))
{
delete memory_;
memory_ = 0;
UERROR("Error initializing the memory.");
return false;
}
return true;
}
else
{
UERROR("Already initialized, close it first.");
return false;
}
}
void DataRecorder::close()
{
if(memory_)
{
delete memory_;
memory_ = 0;
}
}
DataRecorder::~DataRecorder()
{
this->close();
}
void DataRecorder::addData(const rtabmap::Image & image)
{
if(memory_)
{
//save to database
UTimer time;
memory_->update(image);
memory_->cleanup();
if(image.id() % 30)
{
memory_->emptyTrash();
}
UDEBUG("Time to process a message = %f s", time.ticks());
}
else
{
UWARN("CloudRecorder not initialized!");
}
--dataQueue_;
}
void DataRecorder::showImage(const rtabmap::Image & image)
{
if(this->isVisible() && !image.empty())
{
imageView_->setImage(uCvMat2QImage(image.image()));
imageView_->setImageDepth(uCvMat2QImage(image.depth()));
imageView_->fitInView(imageView_->sceneRect(), Qt::KeepAspectRatio);
}
}
void DataRecorder::handleEvent(UEvent * event)
{
if(event->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * camEvent = (CameraEvent*)event;
if(camEvent->getCode() == CameraEvent::kCodeImageDepth ||
camEvent->getCode() == CameraEvent::kCodeImage)
{
if(!camEvent->image().empty())
{
UINFO("Receiving rate = %f Hz", 1.0f/timer_.ticks());
QMetaObject::invokeMethod(this, "addData", Q_ARG(rtabmap::Image, camEvent->image()));
++dataQueue_;
if(dataQueue_ < 2 && this->isVisible())
{
QMetaObject::invokeMethod(this, "showImage", Q_ARG(rtabmap::Image, camEvent->image()));
}
}
}
}
}
} /* namespace rtabmap */
guilib/src/DatabaseViewer.cpp
+142 -66
View File
@@ -32,6 +32,13 @@
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/gui/KeypointItem.h"
#include "rtabmap/gui/UCv2Qt.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Signature.h"
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
DatabaseViewer::DatabaseViewer(QWidget * parent) :
QMainWindow(parent),
@@ -54,9 +61,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
ui_->graphicsView_A->setScene(new QGraphicsScene(this));
ui_->graphicsView_B->setScene(new QGraphicsScene(this));
connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
ui_->horizontalSlider_A->setTracking(false);
ui_->horizontalSlider_B->setTracking(false);
@@ -99,6 +104,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
delete memory_;
memory_ = 0;
imagesMap_.clear();
depthImagesMap_.clear();
ids_.clear();
}
@@ -135,9 +141,8 @@ void DatabaseViewer::updateIds()
std::set<int> ids = memory_->getAllSignatureIds();
ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
ids_.prepend(0);
UDEBUG("Loaded %d ids", ids_.size());
UINFO("Loaded %d ids", ids_.size());
if(ids_.size())
{
@@ -145,12 +150,12 @@ void DatabaseViewer::updateIds()
ui_->horizontalSlider_B->setMinimum(0);
ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
ui_->horizontalSlider_A->setSliderPosition(0);
ui_->horizontalSlider_B->setSliderPosition(0);
ui_->horizontalSlider_A->setEnabled(true);
ui_->horizontalSlider_B->setEnabled(true);
ui_->label_idA->setText("0");
ui_->label_idB->setText("0");
ui_->horizontalSlider_A->setSliderPosition(0);
ui_->horizontalSlider_B->setSliderPosition(0);
sliderAValueChanged(0);
sliderBValueChanged(0);
}
else
{
@@ -217,31 +222,93 @@ void DatabaseViewer::generateLocalGraph()
}
}
void DatabaseViewer::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene)
void DatabaseViewer::generate3DMap()
{
if(!scene)
if(!ids_.size() || !memory_)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
return;
}
rtabmap::KeypointItem * item = 0;
int alpha = 70;
for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
bool ok = false;
int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
if(ok)
{
const cv::KeyPoint & r = (*i).second;
int id = (*i).first;
QString info = QString( "WordRef = %1\n"
"Laplacian = %2\n"
"Dir = %3\n"
"Hessian = %4\n"
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
if(ok)
{
float voxelSize = QInputDialog::getDouble(this, tr("Voxel size?"), tr("Voxel Size"), 0.01, 0, 0.1, 3, &ok);
if(ok)
{
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Map" + QString::number(id) + ".pcd", tr("PCL file (*.pcd)"));
if(!path.isEmpty())
{
std::map<int, int> ids = memory_->getNeighborsId(id, margin, -1, false);
if(ids.size() > 0)
{
std::map<int, rtabmap::Transform> poses, optimizedPoses;
std::multimap<int, std::pair<int, rtabmap::Transform> > edgeConstraints;
memory_->getMetricConstraints(uKeys(ids), memory_->getSignature(id)->mapId(), poses, edgeConstraints, true);
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
UINFO("Poses=%d, constraints=%d", poses.size(), edgeConstraints.size());
scene->addItem(item);
item->setZValue(1);
rtabmap::util3d::saveTOROGraph("toro1.graph", poses, edgeConstraints);
rtabmap::Transform mapCorrection;
rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, 100, optimizedPoses, mapCorrection);
rtabmap::util3d::saveTOROGraph("toro2.graph", optimizedPoses, edgeConstraints);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
if(!pose.isNull())
{
std::vector<unsigned char> image, depth, depth2d;
float depthConstant;
rtabmap::Transform localTransform;
memory_->getImageDepth(iter->first, image, depth, depth2d, depthConstant, localTransform);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
cloud = rtabmap::util3d::cloudFromDepthRGB(
imageMat,
depthMat,
depthMat.cols/2, depthMat.rows/2,
1.0f/depthConstant, 1.0f/depthConstant);
if(voxelSize > 0.0f)
{
cloud = rtabmap::util3d::voxelize(cloud, voxelSize);
}
cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
*assembledCloud += *cloud;
}
}
if(voxelSize > 0.0f)
{
pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(assembledCloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
filter.filter(*tmp);
assembledCloud = tmp;
}
pcl::io::savePCDFile(path.toStdString(), *assembledCloud, true);
QMessageBox::information(this, "Generated Map", tr("Map saved to %1!\n(%2 nodes, %3 points)").arg(path).arg(ids.size()).arg(assembledCloud->size()));
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
}
}
}
}
}
}
@@ -272,7 +339,7 @@ void DatabaseViewer::update(int value,
QLabel * labelActions,
QLabel * labelParents,
QLabel * labelChildren,
QGraphicsView * view,
rtabmap::ImageView * view,
QLabel * labelId)
{
UTimer timer;
@@ -282,23 +349,33 @@ void DatabaseViewer::update(int value,
labelChildren->clear();
if(value >= 0 && value < ids_.size())
{
view->scene()->clear();
view->clear();
int id = ids_.at(value);
labelId->setText(QString::number(id));
if(id>0)
{
//image
QImage img;
QImage imgDepth;
QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
QMap<int, QByteArray>::iterator iterDepth = depthImagesMap_.find(id);
if(iter == imagesMap_.end())
{
if(memory_)
{
cv::Mat image = memory_->getImage(id);
std::vector<unsigned char> image, depth, depth2d;
float depthConstant;
rtabmap::Transform localTransform;
memory_->getImageDepth(id, image, depth, depth2d, depthConstant, localTransform);
cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
UINFO("loaded image(%d/%d) depth(%d/%d) depthConstant(%f)",
imageMat.cols, imageMat.rows,
depthMat.cols, depthMat.rows,
depthConstant);
if(!image.empty())
{
IplImage iplImg = image;
img = ipl2QImage(&iplImg);
img = uCvMat2QImage(imageMat);
if(!img.isNull())
{
QByteArray ba;
@@ -308,11 +385,27 @@ void DatabaseViewer::update(int value,
imagesMap_.insert(id, ba);
}
}
if(!depth.empty())
{
imgDepth = uCvMat2QImage(depthMat);
if(!imgDepth.isNull())
{
QByteArray ba;
QBuffer buffer(&ba);
buffer.open(QIODevice::WriteOnly);
QPixmap::fromImage(imgDepth).save(&buffer, "PGM"); // writes image into ba in PGM format
depthImagesMap_.insert(id, ba);
}
}
}
}
else
{
img.loadFromData(iter.value(), "BMP");
if(iterDepth != depthImagesMap_.end())
{
imgDepth.loadFromData(iterDepth.value(), "PGM");
}
}
if(memory_)
@@ -320,38 +413,47 @@ void DatabaseViewer::update(int value,
std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
if(words.size())
{
drawKeypoints(words, view->scene());
view->setFeatures(words);
}
}
if(!img.isNull())
{
view->scene()->addPixmap(QPixmap::fromImage(img));
view->setImage(img);
}
else
{
ULOGGER_DEBUG("Image is empty");
}
if(!imgDepth.isNull())
{
view->setImageDepth(imgDepth);
}
else
{
ULOGGER_DEBUG("Image depth is empty");
}
view->fitInView(view->sceneRect(), Qt::KeepAspectRatio);
// loops
std::set<int> parents;
std::set<int> children;
std::map<int, rtabmap::Transform> parents;
std::map<int, rtabmap::Transform> children;
memory_->getLoopClosureIds(id, parents, children, true);
if(parents.size())
{
QString str;
for(std::set<int>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
for(std::map<int, rtabmap::Transform>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
{
str.append(QString("%1 ").arg(*iter));
str.append(QString("%1 ").arg(iter->first));
}
labelParents->setText(str);
}
if(children.size())
{
QString str;
for(std::set<int>::iterator iter=children.begin(); iter!=children.end(); ++iter)
for(std::map<int, rtabmap::Transform>::iterator iter=children.begin(); iter!=children.end(); ++iter)
{
str.append(QString("%1 ").arg(*iter));
str.append(QString("%1 ").arg(iter->first));
}
labelChildren->setText(str);
}
@@ -392,29 +494,3 @@ void DatabaseViewer::sliderBMoved(int value)
ULOGGER_ERROR("Slider index out of range ?");
}
}
QImage DatabaseViewer::ipl2QImage(const IplImage *newImage)
{
QImage qtemp;
if (newImage && newImage->depth == IPL_DEPTH_8U && cvGetSize(newImage).width>0)
{
int x;
int y;
char* data = newImage->imageData;
qtemp= QImage(newImage->width, newImage->height,QImage::Format_RGB32 );
for( y = 0; y < newImage->height; y++, data +=newImage->widthStep )
{
for( x = 0; x < newImage->width; x++)
{
uint *p = (uint*)qtemp.scanLine (y) + x;
*p = qRgb(data[x * newImage->nChannels+2], data[x * newImage->nChannels+1],data[x * newImage->nChannels]);
}
}
}
else
{
ULOGGER_ERROR("Wrong IplImage format");
}
return qtemp;
}
guilib/src/DetailedProgressDialog.cpp
+13 -4
View File
@@ -20,12 +20,13 @@
#include "DetailedProgressDialog.h"
#include <QtGui/QLayout>
#include <QtGui/QProgressBar>
#include <QtGui/QPlainTextEdit>
#include <QtGui/QTextEdit>
#include <QtGui/QLabel>
#include <QtGui/QPushButton>
#include <QtGui/QCloseEvent>
#include <QtGui/QCheckBox>
#include <QtCore/QTimer>
#include <QtCore/QTime>
#include "rtabmap/utilite/ULogger.h"
namespace rtabmap {
@@ -39,9 +40,9 @@ DetailedProgressDialog::DetailedProgressDialog(QWidget *parent, Qt::WindowFlags
_text->setWordWrap(true);
_progressBar = new QProgressBar(this);
_progressBar->setMaximum(1);
_detailedText = new QPlainTextEdit(this);
_detailedText = new QTextEdit(this);
_detailedText->setReadOnly(true);
_detailedText->setLineWrapMode(QPlainTextEdit::NoWrap);
_detailedText->setLineWrapMode(QTextEdit::NoWrap);
_closeButton = new QPushButton(this);
_closeButton->setText("Close");
_closeWhenDoneCheckBox = new QCheckBox(this);
@@ -76,7 +77,9 @@ void DetailedProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec)
void DetailedProgressDialog::appendText(const QString & text)
{
_text->setText(text);
_detailedText->appendPlainText(text);
QString html = tr("<html><font color=\"#999999\">%1 </font>%2</html>").arg(QTime::currentTime().toString("HH:mm:ss")).arg(text);
_detailedText->append(html);
_detailedText->ensureCursorVisible();
}
void DetailedProgressDialog::setValue(int value)
{
@@ -122,6 +125,12 @@ void DetailedProgressDialog::clear()
_closeButton->setEnabled(false);
}
void DetailedProgressDialog::resetProgress()
{
_progressBar->reset();
_closeButton->setEnabled(false);
}
void DetailedProgressDialog::closeEvent(QCloseEvent *event)
{
if(_progressBar->value() == _progressBar->maximum())
guilib/src/DetailedProgressDialog.h
+3 -2
View File
@@ -23,7 +23,7 @@
#include <QtGui/QDialog>
class QLabel;
class QPlainTextEdit;
class QTextEdit;
class QProgressBar;
class QPushButton;
class QCheckBox;
@@ -51,10 +51,11 @@ public slots:
void appendText(const QString & text);
void incrementStep();
void clear();
void resetProgress();
private:
QLabel * _text;
QPlainTextEdit * _detailedText;
QTextEdit * _detailedText;
QProgressBar * _progressBar;
QPushButton * _closeButton;
QCheckBox * _closeWhenDoneCheckBox;
guilib/src/GuiLib.qrc
+1
View File
@@ -6,6 +6,7 @@
<file>images/Play1Normal.png</file>
<file>images/Pause.ico</file>
<file>images/PauseOnLoop.ico</file>
<file>images/PauseOnLocalLoop.ico</file>
<file>images/PauseLoopRejected.ico</file>
<file>qss/default.qss</file>
<file>images/Plot16.png</file>
guilib/src/ImageView.cpp
+166 -17
View File
@@ -25,6 +25,7 @@
#include <QtGui/QFileDialog>
#include <QtCore/QDir>
#include <QtGui/QAction>
#include <QtGui/QGraphicsEffect>
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/gui/KeypointItem.h"
@@ -34,7 +35,9 @@ ImageView::ImageView(QWidget * parent) :
QGraphicsView(parent),
_zoom(250),
_minZoom(250),
_savedFileName((QDir::homePath()+ "/") + "picture" + ".png")
_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
_image(0),
_imageDepth(0)
{
this->setTransformationAnchor(QGraphicsView::AnchorUnderMouse);
this->setScene(new QGraphicsScene(this));
@@ -44,6 +47,9 @@ ImageView::ImageView(QWidget * parent) :
_showImage = _menu->addAction(tr("Show image"));
_showImage->setCheckable(true);
_showImage->setChecked(true);
_showImageDepth = _menu->addAction(tr("Show image depth"));
_showImageDepth->setCheckable(true);
_showImageDepth->setChecked(false);
_showFeatures = _menu->addAction(tr("Show features"));
_showFeatures->setCheckable(true);
_showFeatures->setChecked(true);
@@ -54,7 +60,7 @@ ImageView::ImageView(QWidget * parent) :
}
ImageView::~ImageView() {
clear();
}
void ImageView::resetZoom()
@@ -68,6 +74,11 @@ bool ImageView::isImageShown()
return _showImage->isChecked();
}
bool ImageView::isImageDepthShown()
{
return _showImageDepth->isChecked();
}
bool ImageView::isFeaturesShown()
{
return _showFeatures->isChecked();
@@ -76,7 +87,30 @@ bool ImageView::isFeaturesShown()
void ImageView::setFeaturesShown(bool shown)
{
_showFeatures->setChecked(shown);
this->updateItemsShown();
for(int i=0; i<_features.size(); ++i)
{
_features[i]->setVisible(_showFeatures->isChecked());
}
}
void ImageView::setImageShown(bool shown)
{
_showImage->setChecked(shown);
if(_image)
{
_image->setVisible(_showImage->isChecked());
this->updateOpacity();
}
}
void ImageView::setImageDepthShown(bool shown)
{
_showImageDepth->setChecked(shown);
if(_imageDepth)
{
_imageDepth->setVisible(_showImageDepth->isChecked());
this->updateOpacity();
}
}
bool ImageView::isLinesShown()
@@ -87,7 +121,14 @@ bool ImageView::isLinesShown()
void ImageView::setLinesShown(bool shown)
{
_showLines->setChecked(shown);
this->updateItemsShown();
QList<QGraphicsItem*> items = this->scene()->items();
for(int i=0; i<items.size(); ++i)
{
if( qgraphicsitem_cast<QGraphicsLineItem*>(items.at(i)))
{
items.at(i)->setVisible(_showLines->isChecked());
}
}
}
void ImageView::contextMenuEvent(QContextMenuEvent * e)
@@ -110,28 +151,46 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
img.save(text);
}
}
else if(action == _showFeatures || action == _showImage || action == _showLines)
else if(action == _showFeatures)
{
this->updateItemsShown();
this->setFeaturesShown(_showFeatures->isChecked());
}
else if(action == _showImage)
{
this->setImageShown(_showImage->isChecked());
}
else if(action == _showImageDepth)
{
this->setImageDepthShown(_showImageDepth->isChecked());
}
else if(action == _showLines)
{
this->setLinesShown(_showLines->isChecked());
}
if(action == _showImage || action ==_showImageDepth)
{
this->updateOpacity();
}
}
void ImageView::updateItemsShown()
void ImageView::updateOpacity()
{
QList<QGraphicsItem*> items = this->scene()->items();
for(int i=0; i<items.size(); ++i)
if(_image && _imageDepth)
{
if(qgraphicsitem_cast<KeypointItem*>(items.at(i)))
if(_image->isVisible() && _imageDepth->isVisible())
{
items.at(i)->setVisible(_showFeatures->isChecked());
QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
QGraphicsOpacityEffect * effect2 = new QGraphicsOpacityEffect();
effect->setOpacity(0.5);
effect2->setOpacity(0.5);
_image->setGraphicsEffect(effect);
_imageDepth->setGraphicsEffect(effect2);
}
else if( qgraphicsitem_cast<QGraphicsLineItem*>(items.at(i)))
else
{
items.at(i)->setVisible(_showLines->isChecked());
}
else if(qgraphicsitem_cast<QGraphicsPixmapItem*>(items.at(i)))
{
items.at(i)->setVisible(_showImage->isChecked());
_image->setGraphicsEffect(0);
_imageDepth->setGraphicsEffect(0);
}
}
}
@@ -175,4 +234,94 @@ void ImageView::wheelEvent(QWheelEvent * e)
this->setMatrix(matrix);
}
void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
{
for(int i=0; i<_features.size(); ++i)
{
scene()->removeItem(_features[i]);
delete _features[i];
}
_features.clear();
rtabmap::KeypointItem * item = 0;
int alpha = 70;
for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
{
const cv::KeyPoint & r = (*i).second;
int id = (*i).first;
QString info = QString( "WordRef = %1\n"
"Laplacian = %2\n"
"Dir = %3\n"
"Hessian = %4\n"
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
scene()->addItem(item);
_features.append(item);
item->setVisible(_showFeatures->isChecked());
item->setZValue(1);
}
}
void ImageView::setImage(const QImage & image)
{
if(_image)
{
_image->setPixmap(QPixmap::fromImage(image));
}
else
{
_image = scene()->addPixmap(QPixmap::fromImage(image));
_image->setVisible(_showImage->isChecked());
_showImage->setEnabled(true);
this->updateOpacity();
}
}
void ImageView::setImageDepth(const QImage & imageDepth)
{
if(_imageDepth)
{
_imageDepth->setPixmap(QPixmap::fromImage(imageDepth));
}
else
{
_imageDepth = scene()->addPixmap(QPixmap::fromImage(imageDepth));
_imageDepth->setVisible(_showImageDepth->isChecked());
_showImageDepth->setEnabled(true);
this->updateOpacity();
}
}
void ImageView::clear()
{
for(int i=0; i<_features.size(); ++i)
{
scene()->removeItem(_features[i]);
delete _features[i];
}
_features.clear();
if(_image)
{
scene()->removeItem(_image);
delete _image;
_image = 0;
_showImage->setEnabled(false);
}
if(_imageDepth)
{
scene()->removeItem(_imageDepth);
delete _imageDepth;
_imageDepth = 0;
_showImageDepth->setEnabled(false);
}
scene()->clear();
}
}
guilib/src/LoopClosureViewer.cpp
+212
View File
@@ -0,0 +1,212 @@
/*
* LoopClosureViewer.cpp
*
* Created on: 2013-10-21
* Author: Mathieu
*/
#include "rtabmap/gui/LoopClosureViewer.h"
#include "ui_loopClosureViewer.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UStl.h"
#include <QtCore/QTimer>
namespace rtabmap {
LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
QWidget(parent),
sA_(0),
sB_(0),
decimation_(1),
maxDepth_(0),
samples_(0)
{
ui_ = new Ui_loopClosureViewer();
ui_->setupUi(this);
connect(ui_->checkBox_rawCloud, SIGNAL(clicked()), this, SLOT(updateView()));
}
LoopClosureViewer::~LoopClosureViewer() {
delete ui_;
if(sA_)
{
delete sA_;
}
if(sB_)
{
delete sB_;
}
}
void LoopClosureViewer::setData(Signature * sA, Signature * sB)
{
if(sA_)
{
delete sA_;
}
if(sB_)
{
delete sB_;
}
sA_ = sA;
sB_ = sB;
if(sA_ && sB_)
{
ui_->label_idA->setText(QString("[%1-%2]").arg(sA->id()).arg(sB->id()));
}
}
void LoopClosureViewer::updateView(const Transform & transform)
{
if(sA_ && sB_)
{
int decimation = 1;
float maxDepth = 0;
int samples = 0;
if(!ui_->checkBox_rawCloud->isChecked())
{ decimation = decimation_;
maxDepth = maxDepth_;
samples = samples_;
}
UDEBUG("decimation = %d", decimation);
UDEBUG("maxDepth = %d", maxDepth);
UDEBUG("samples = %d", samples);
Transform t;
if(!transform.isNull())
{
transform_ = transform;
t = transform;
}
else if(!transform_.isNull())
{
t = transform_;
}
else
{
t = sB_->getPose();
}
UDEBUG("t= %s", t.prettyPrint().c_str());
ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
if(!t.isNull())
{
util3d::CompressionThread ctiA(sA_->getImage(), true);
util3d::CompressionThread ctdA(sA_->getDepth(), true);
util3d::CompressionThread ctiB(sB_->getImage(), true);
util3d::CompressionThread ctdB(sB_->getDepth(), true);
util3d::CompressionThread ct2dA(sA_->getDepth2D(), false);
util3d::CompressionThread ct2dB(sB_->getDepth2D(), false);
ctiA.start();
ctdA.start();
ctiB.start();
ctdB.start();
ct2dA.start();
ct2dB.start();
ctiA.join();
ctdA.join();
ctiB.join();
ctdB.join();
ct2dA.join();
ct2dB.join();
cv::Mat imageA = ctiA.getUncompressedData();
cv::Mat depthA = ctdA.getUncompressedData();
cv::Mat imageB = ctiB.getUncompressedData();
cv::Mat depthB = ctdB.getUncompressedData();
cv::Mat depth2dA = ct2dA.getUncompressedData();
cv::Mat depth2dB = ct2dB.getUncompressedData();
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
cloudA = util3d::cloudFromDepthRGB(
imageA,
depthA,
depthA.cols/2,
depthA.rows/2,
1.0f/sA_->getDepthConstant(),
1.0f/sA_->getDepthConstant(),
decimation);
cloudA = util3d::removeNaNFromPointCloud(cloudA);
if(maxDepth>0.0)
{
cloudA = util3d::passThrough(cloudA, "z", 0, maxDepth);
}
if(samples>0 && (int)cloudA->size() > samples)
{
cloudA = util3d::sampling(cloudA, samples);
}
cloudA = util3d::transformPointCloud(cloudA, sA_->getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
cloudB = util3d::cloudFromDepthRGB(
imageB,
depthB,
depthB.cols/2,
depthB.rows/2,
1.0f/sB_->getDepthConstant(),
1.0f/sB_->getDepthConstant(),
decimation);
cloudB = util3d::removeNaNFromPointCloud(cloudB);
if(maxDepth>0.0)
{
cloudB = util3d::passThrough(cloudB, "z", 0, maxDepth);
}
if(samples>0 && (int)cloudB->size() > samples)
{
cloudB = util3d::sampling(cloudB, samples);
}
cloudB = util3d::transformPointCloud(cloudB, t*sB_->getLocalTransform());
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::depth2DToPointCloud(depth2dA);
scanB = util3d::depth2DToPointCloud(depth2dB);
scanB = util3d::transformPointCloud(scanB, t);
ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_->id()).arg(cloudB->size()).arg(sA_->id()).arg(cloudA->size()));
if(cloudA->size())
{
ui_->cloudViewerTransform->addOrUpdateCloud("cloud0", cloudA);
}
if(cloudB->size())
{
ui_->cloudViewerTransform->addOrUpdateCloud("cloud1", cloudB);
}
if(scanA->size())
{
ui_->cloudViewerTransform->addOrUpdateCloud("scan0", scanA);
}
if(scanB->size())
{
ui_->cloudViewerTransform->addOrUpdateCloud("scan1", scanB);
}
}
else
{
ui_->cloudViewerTransform->removeAllClouds();
}
ui_->cloudViewerTransform->render();
}
}
void LoopClosureViewer::showEvent(QShowEvent * event)
{
QWidget::showEvent( event );
QTimer::singleShot(500, this, SLOT(updateView())); // make sure the QVTKWidget is shown!
}
} /* namespace rtabmap */
guilib/src/MainWindow.cpp
+1721 -162
View File
File diff suppressed because it is too large Load Diff
guilib/src/OdometryViewer.cpp
+174
View File
@@ -0,0 +1,174 @@
/*
* OdometryViewer.cpp
*
* Created on: 2013-10-15
* Author: Mathieu
*/
#include "rtabmap/gui/OdometryViewer.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UConversion.h"
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <QtGui/QInputDialog>
#include <QtGui/QAction>
#include <QtGui/QMenu>
#include <QtGui/QKeyEvent>
namespace rtabmap {
OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, QWidget * parent) :
CloudViewer(parent),
maxClouds_(maxClouds),
voxelSize_(voxelSize),
decimation_(decimation),
id_(0),
_aSetVoxelSize(0),
_aSetDecimation(0),
_aSetCloudHistorySize(0),
_aPause(0)
{
//add actions to CloudViewer menu
_aSetVoxelSize = new QAction("Set voxel size...", this);
_aSetDecimation = new QAction("Set depth image decimation...", this);
_aSetCloudHistorySize = new QAction("Set cloud history size...", this);
_aPause = new QAction("Pause", this);
_aPause->setCheckable(true);
menu()->addAction(_aSetVoxelSize);
menu()->addAction(_aSetDecimation);
menu()->addAction(_aSetCloudHistorySize);
menu()->addAction(_aPause);
}
void OdometryViewer::processData()
{
rtabmap::Image data;
dataMutex_.lock();
if(buffer_.size())
{
data = buffer_.back();
buffer_.clear();
}
dataMutex_.unlock();
if(!data.empty() && this->isVisible())
{
UINFO("New pose = %s", data.pose().prettyPrint().c_str());
// visualization: buffering the clouds
// Create the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = util3d::cloudFromDepthRGB(
data.image(),
data.depth(),
float(data.depth().cols/2),
float(data.depth().rows/2),
1.0f/data.depthConstant(),
1.0f/data.depthConstant(),
decimation_);
if(voxelSize_ > 0.0f)
{
cloud = util3d::voxelize(cloud, voxelSize_);
}
cloud = util3d::transformPointCloud(cloud, data.localTransform());
data.id()?id_=data.id():++id_;
clouds_.insert(std::make_pair(id_, cloud));
if((int)clouds_.size() > maxClouds_)
{
this->removeCloud(uFormat("cloud%d", clouds_.begin()->first));
clouds_.erase(clouds_.begin());
}
if(clouds_.size())
{
this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
}
this->updateCameraPosition(data.pose());
this->setBackgroundColor(Qt::black);
this->render();
}
}
void OdometryViewer::handleEvent(UEvent * event)
{
if(!_aPause->isChecked())
{
if(event->getClassName().compare("OdometryEvent") == 0)
{
rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
if(odomEvent->isValid())
{
bool empty = false;
dataMutex_.lock();
if(buffer_.empty())
{
buffer_.push_back(odomEvent->data());
empty= true;
}
else
{
buffer_.back() = odomEvent->data();
}
dataMutex_.unlock();
if(empty)
{
QMetaObject::invokeMethod(this, "processData");
}
}
else
{
//UWARN("odom=%fs, Cannot compute odometry!!!", timer_.restart());
QMetaObject::invokeMethod(this, "setBackgroundColor", Q_ARG(QColor, Qt::darkRed));
QMetaObject::invokeMethod(this, "render");
}
}
}
}
void OdometryViewer::handleAction(QAction * a)
{
CloudViewer::handleAction(a);
if(a == _aSetVoxelSize)
{
bool ok;
double value = QInputDialog::getDouble(this, tr("Set voxel size"), tr("Size (0=disabled)"), voxelSize_, 0.0, 0.1, 2, &ok);
if(ok)
{
voxelSize_ = value;
}
}
else if(a == _aSetCloudHistorySize)
{
bool ok;
int value = QInputDialog::getInt(this, tr("Set cloud history size"), tr("Size (0=infinite)"), maxClouds_, 0, 100, 1, &ok);
if(ok)
{
maxClouds_ = value;
}
}
else if(a == _aSetDecimation)
{
bool ok;
int value = QInputDialog::getInt(this, tr("Set depth image decimation"), tr("Decimation (0=infinite)"), decimation_, 1, 8, 1, &ok);
if(ok)
{
decimation_ = value;
}
}
}
} /* namespace rtabmap */
guilib/src/PdfPlot.cpp
+8 -8
View File
@@ -19,6 +19,8 @@
#include "PdfPlot.h"
#include <rtabmap/utilite/ULogger.h>
#include "rtabmap/gui/UCv2Qt.h"
#include "rtabmap/core/util3d.h"
namespace rtabmap {
@@ -59,15 +61,13 @@ void PdfPlotItem::showDescription(bool shown)
if(!_img && _imagesRef)
{
QImage img;
QMap<int, QByteArray>::const_iterator iter = _imagesRef->find(int(this->data().x()));
QMap<int, std::vector<unsigned char> >::const_iterator iter = _imagesRef->find(int(this->data().x()));
if(iter != _imagesRef->constEnd())
{
if(img.loadFromData(iter.value(), "JPEG"))
{
QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
_img = new QGraphicsPixmapItem(scaled, this);
_img->setVisible(false);
}
img = uCvMat2QImage(util3d::uncompressImage(iter.value()));
QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
_img = new QGraphicsPixmapItem(scaled, this);
_img->setVisible(false);
}
}
@@ -103,7 +103,7 @@ void PdfPlotItem::showDescription(bool shown)
PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, QByteArray> * imagesMapRef = 0, QObject * parent) :
PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, std::vector<unsigned char> > * imagesMapRef = 0, QObject * parent) :
UPlotCurve(name, parent),
_imagesMapRef(imagesMapRef)
{
guilib/src/PdfPlot.h
+5 -4
View File
@@ -21,6 +21,7 @@
#define PDFPLOT_H_
#include <utilite/UPlot.h>
#include "opencv2/opencv.hpp"
namespace rtabmap {
@@ -31,7 +32,7 @@ public:
virtual ~PdfPlotItem();
void setLikelihood(int id, float value, int childCount);
void setImagesRef(const QMap<int, QByteArray> * imagesRef) {_imagesRef = imagesRef;}
void setImagesRef(const QMap<int, std::vector<unsigned char> > * imagesRef) {_imagesRef = imagesRef;}
float value() const {return this->data().y();}
int id() const {return this->data().x();}
@@ -42,7 +43,7 @@ protected:
private:
QGraphicsPixmapItem * _img;
int _childCount;
const QMap<int, QByteArray> * _imagesRef;
const QMap<int, std::vector<unsigned char> > * _imagesRef;
QGraphicsTextItem * _text;
};
@@ -52,14 +53,14 @@ class PdfPlotCurve : public UPlotCurve
Q_OBJECT
public:
PdfPlotCurve(const QString & name, const QMap<int, QByteArray> * imagesMapRef, QObject * parent = 0);
PdfPlotCurve(const QString & name, const QMap<int, std::vector<unsigned char> > * imagesMapRef, QObject * parent = 0);
virtual ~PdfPlotCurve();
virtual void clear();
void setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap);
private:
const QMap<int, QByteArray> * _imagesMapRef;
const QMap<int, std::vector<unsigned char> > * _imagesMapRef;
};
}
guilib/src/PreferencesDialog.cpp
+963 -149
View File
File diff suppressed because it is too large Load Diff
guilib/src/images/PauseOnLocalLoop.ico
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.4 KiB

guilib/src/qtipl.cpp
-53
View File
@@ -1,53 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/gui/qtipl.h"
#include "rtabmap/utilite/ULogger.h"
#include <opencv2/core/core_c.h>
namespace rtabmap {
// TODO : support only from gray 8bits ?
QImage Ipl2QImage(const IplImage *newImage, int alpha)
{
QImage qtemp;
if (newImage && newImage->depth == IPL_DEPTH_8U && cvGetSize(newImage).width>0)
{
int x;
int y;
char* data = newImage->imageData;
qtemp= QImage(newImage->width, newImage->height,QImage::Format_ARGB32 );
for( y = 0; y < newImage->height; y++, data +=newImage->widthStep )
{
for( x = 0; x < newImage->width; x++)
{
uint *p = (uint*)qtemp.scanLine (y) + x;
*p = qRgba(data[x * newImage->nChannels+2], data[x * newImage->nChannels+1],data[x * newImage->nChannels], alpha);
}
}
}
else
{
ULOGGER_ERROR("Wrong IplImage format");
}
return qtemp;
}
}
guilib/src/ui/DatabaseViewer.ui
+16 -4
View File
@@ -29,7 +29,7 @@
<number>0</number>
</property>
<item>
<widget class="QGraphicsView" name="graphicsView_A"/>
<widget class="rtabmap::ImageView" name="graphicsView_A"/>
</item>
<item>
<widget class="QScrollArea" name="scrollArea_2">
@@ -153,7 +153,7 @@
<number>0</number>
</property>
<item>
<widget class="QGraphicsView" name="graphicsView_B"/>
<widget class="rtabmap::ImageView" name="graphicsView_B"/>
</item>
<item>
<widget class="QScrollArea" name="scrollArea">
@@ -288,7 +288,7 @@
<x>0</x>
<y>0</y>
<width>736</width>
<height>22</height>
<height>21</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -309,7 +309,7 @@
<addaction name="actionClean_database"/>
<addaction name="actionClean_local_graph"/>
<addaction name="separator"/>
<addaction name="actionUpdate_base_ids"/>
<addaction name="actionGenerate_3D_map_pcd"/>
</widget>
<addaction name="menuFile"/>
<addaction name="menuEdit"/>
@@ -354,7 +354,19 @@
<string>Update base ids</string>
</property>
</action>
<action name="actionGenerate_3D_map_pcd">
<property name="text">
<string>Generate 3D map (.pcd) ...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
<class>rtabmap::ImageView</class>
<extends>QGraphicsView</extends>
<header>rtabmap/gui/ImageView.h</header>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>
guilib/src/ui/aboutDialog.ui
+23 -6
View File
@@ -82,10 +82,10 @@ p, li { white-space: pre-wrap; }
</item>
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="1" column="0">
<widget class="QLabel" name="label_2">
<item row="7" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Author :</string>
<string>Version :</string>
</property>
</widget>
</item>
@@ -96,10 +96,10 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLabel" name="label_6">
<item row="1" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Version :</string>
<string>Author :</string>
</property>
</widget>
</item>
@@ -174,6 +174,23 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>PCL version :</string>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_pcl_version">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
guilib/src/ui/loopClosureViewer.ui
+95
View File
@@ -0,0 +1,95 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>loopClosureViewer</class>
<widget class="QWidget" name="loopClosureViewer">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>515</width>
<height>390</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout" stretch="0,1">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<widget class="QLabel" name="label_idA">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_idB">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_transform">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QCheckBox" name="checkBox_rawCloud">
<property name="text">
<string>Raw cloud</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="rtabmap::CloudViewer" name="cloudViewerTransform" native="true"/>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>rtabmap::CloudViewer</class>
<extends>QWidget</extends>
<header>rtabmap/gui/CloudViewer.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>
guilib/src/ui/mainWindow.ui
+224 -54
View File
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>1056</width>
<height>642</height>
<width>809</width>
<height>661</height>
</rect>
</property>
<property name="windowTitle">
@@ -101,8 +101,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>1056</width>
<height>22</height>
<width>809</width>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -115,35 +115,25 @@
<property name="title">
<string>Edit</string>
</property>
<widget class="QMenu" name="menuAspect_ratio">
<property name="title">
<string>Aspect ratio</string>
</property>
<addaction name="action16_9"/>
<addaction name="action16_10"/>
<addaction name="action4_3"/>
<addaction name="separator"/>
<addaction name="action1080p"/>
<addaction name="action720p"/>
<addaction name="action480p"/>
<addaction name="action360p"/>
<addaction name="action240p"/>
</widget>
<addaction name="actionApply_settings_to_the_detector"/>
<addaction name="actionClear_cache"/>
<addaction name="actionDownload_all_clouds"/>
<addaction name="separator"/>
<addaction name="actionOpen_working_directory"/>
<addaction name="actionPrint_loop_closure_IDs_to_console"/>
<addaction name="separator"/>
<addaction name="actionReset_the_memory"/>
<addaction name="actionDelete_memory"/>
<addaction name="actionClear_cache"/>
<addaction name="separator"/>
<addaction name="actionDump_the_memory"/>
<addaction name="actionDump_the_prediction_matrix"/>
<addaction name="actionPrint_loop_closure_IDs_to_console"/>
<addaction name="actionGenerate_map"/>
<addaction name="actionGenerate_local_map"/>
<addaction name="actionReset_the_memory"/>
<addaction name="separator"/>
<addaction name="actionAuto_screen_capture"/>
<addaction name="menuAspect_ratio"/>
<addaction name="actionView_high_res_point_cloud"/>
<addaction name="actionSave_point_cloud"/>
<addaction name="actionView_point_cloud_as_mesh"/>
<addaction name="actionSave_mesh_ply_vtk_stl"/>
</widget>
<widget class="QMenu" name="menu6">
<property name="title">
@@ -171,6 +161,7 @@
</widget>
<addaction name="menuImage"/>
<addaction name="actionDatabase"/>
<addaction name="actionOpenni_RGBD"/>
</widget>
<addaction name="menuSelect_source"/>
<addaction name="separator"/>
@@ -180,6 +171,13 @@
<addaction name="separator"/>
<addaction name="actionPause_on_match"/>
<addaction name="actionPause_when_a_loop_hypothesis_is_rejected"/>
<addaction name="actionPause_on_local_loop_detection"/>
<addaction name="separator"/>
<addaction name="actionSLAM_mode"/>
<addaction name="actionLocalization_mode"/>
<addaction name="separator"/>
<addaction name="actionReset_Odometry"/>
<addaction name="actionTrigger_a_new_map"/>
</widget>
<widget class="QMenu" name="menuWindow">
<property name="title">
@@ -198,8 +196,24 @@
<addaction name="actionSave_state"/>
<addaction name="actionLoad_state"/>
</widget>
<widget class="QMenu" name="menuAspect_ratio_2">
<property name="title">
<string>Aspect ratio</string>
</property>
<addaction name="action16_9"/>
<addaction name="action16_10"/>
<addaction name="action4_3"/>
<addaction name="separator"/>
<addaction name="action1080p"/>
<addaction name="action720p"/>
<addaction name="action480p"/>
<addaction name="action360p"/>
<addaction name="action240p"/>
</widget>
<addaction name="menuShow_view"/>
<addaction name="menuFigures"/>
<addaction name="actionAuto_screen_capture"/>
<addaction name="menuAspect_ratio_2"/>
<addaction name="separator"/>
<addaction name="actionPreferences"/>
</widget>
@@ -250,6 +264,11 @@
<addaction name="separator"/>
<addaction name="actionPause_on_match"/>
<addaction name="actionPause_when_a_loop_hypothesis_is_rejected"/>
<addaction name="actionPause_on_local_loop_detection"/>
<addaction name="separator"/>
<addaction name="actionSLAM_mode"/>
<addaction name="actionLocalization_mode"/>
<addaction name="separator"/>
</widget>
<widget class="QDockWidget" name="dockWidget_statsV2">
<property name="floating">
@@ -277,6 +296,28 @@
<property name="verticalSpacing">
<number>2</number>
</property>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_stats_timeLimit">
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="suffix">
<string> ms</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
<property name="singleStep">
<double>50.000000000000000</double>
</property>
<property name="value">
<double>450.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_24">
<property name="text">
@@ -316,7 +357,7 @@
</property>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QLabel" name="label_45">
<property name="text">
<string>Elapsed time (hh:mm:ss)</string>
@@ -326,7 +367,7 @@
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_elapsedTime">
<property name="text">
<string>Unknown</string>
@@ -336,14 +377,14 @@
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QLabel" name="label_20">
<property name="text">
<string>Current image id</string>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_stats_imageNumber">
<property name="text">
<string>Unknown</string>
@@ -353,14 +394,14 @@
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Loop closures detected</string>
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_stats_loopClosuresDetected">
<property name="text">
<string>0</string>
@@ -370,7 +411,7 @@
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QLabel" name="label_38">
<property name="text">
<string>Loop closures detected
@@ -378,7 +419,7 @@
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_stats_loopClosuresReactivatedDetected">
<property name="text">
<string>0</string>
@@ -388,14 +429,14 @@
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QLabel" name="label_15">
<property name="text">
<string>Loop closures rejected</string>
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_stats_loopClosuresRejected">
<property name="text">
<string>0</string>
@@ -406,14 +447,14 @@
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label">
<widget class="QLabel" name="doubleSpinBox_stats_imgRate_label">
<property name="text">
<string>Image rate</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_2">
<item row="3" column="0">
<widget class="QLabel" name="label_timeLimit">
<property name="text">
<string>Time limit processing</string>
</property>
@@ -423,24 +464,31 @@
</widget>
</item>
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_stats_timeLimit">
<widget class="QDoubleSpinBox" name="doubleSpinBox_stats_detectionRate">
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="suffix">
<string> ms</string>
<string> Hz</string>
</property>
<property name="decimals">
<number>0</number>
<number>1</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>50.000000000000000</double>
<double>0.100000000000000</double>
</property>
<property name="value">
<double>450.000000000000000</double>
<double>2.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="doubleSpinBox_stats_imgRate_label_2">
<property name="text">
<string>RTAB-Map update rate</string>
</property>
</widget>
</item>
@@ -480,13 +528,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -537,6 +579,48 @@
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_cloudViewer">
<property name="windowTitle">
<string>3D Map</string>
</property>
<attribute name="dockWidgetArea">
<number>4</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_5">
<layout class="QVBoxLayout" name="verticalLayout_3">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="rtabmap::CloudViewer" name="widget_cloudViewer" native="true"/>
</item>
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_loopClosureViewer">
<property name="windowTitle">
<string>3D Loop closure</string>
</property>
<attribute name="dockWidgetArea">
<number>8</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_7">
<layout class="QVBoxLayout" name="verticalLayout_7">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="rtabmap::LoopClosureViewer" name="widget_loopClosureViewer" native="true"/>
</item>
</layout>
</widget>
</widget>
<action name="actionExit">
<property name="text">
<string>Exit</string>
@@ -593,7 +677,7 @@
<normaloff>:/images/PauseOnLoop.ico</normaloff>:/images/PauseOnLoop.ico</iconset>
</property>
<property name="text">
<string>Pause on match</string>
<string>Pause on loop closure detection</string>
</property>
</action>
<action name="actionStop">
@@ -612,7 +696,7 @@
</action>
<action name="actionApply_settings_to_the_detector">
<property name="text">
<string>Apply settings to the detector</string>
<string>Apply settings to detector</string>
</property>
</action>
<action name="actionDump_the_memory">
@@ -629,7 +713,7 @@
<normaloff>:/images/PauseLoopRejected.ico</normaloff>:/images/PauseLoopRejected.ico</iconset>
</property>
<property name="text">
<string>Pause when a loop hypothesis is rejected</string>
<string>Pause on loop closure rejection</string>
</property>
</action>
<action name="actionClear_cache">
@@ -662,7 +746,7 @@
</action>
<action name="actionGenerate_map">
<property name="text">
<string>Generate map...</string>
<string>Generate graph map (*.dot)...</string>
</property>
</action>
<action name="actionDelete_memory">
@@ -749,7 +833,7 @@
</action>
<action name="actionGenerate_local_map">
<property name="text">
<string>Generate local map...</string>
<string>Generate graph local map (*.dot)...</string>
</property>
</action>
<action name="actionPrint_loop_closure_IDs_to_console">
@@ -757,6 +841,80 @@
<string>Print loop closure IDs to console</string>
</property>
</action>
<action name="actionOpenni_RGBD">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Openni (RGBD)</string>
</property>
</action>
<action name="actionSave_point_cloud">
<property name="text">
<string>Save high-res point clouds (*.pcd *.ply *.vtk)...</string>
</property>
</action>
<action name="actionDownload_all_clouds">
<property name="text">
<string>Download all clouds (update cache)</string>
</property>
</action>
<action name="actionPause_on_local_loop_detection">
<property name="checkable">
<bool>true</bool>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/PauseOnLocalLoop.ico</normaloff>:/images/PauseOnLocalLoop.ico</iconset>
</property>
<property name="text">
<string>Pause on local loop closure detection</string>
</property>
</action>
<action name="actionSLAM_mode">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Mapping</string>
</property>
<property name="toolTip">
<string>Simultaneous Localization And Mapping (SLAM)</string>
</property>
</action>
<action name="actionLocalization_mode">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>Localization</string>
</property>
</action>
<action name="actionReset_Odometry">
<property name="text">
<string>Reset Odometry</string>
</property>
</action>
<action name="actionView_high_res_point_cloud">
<property name="text">
<string>View high-res point clouds</string>
</property>
</action>
<action name="actionView_point_cloud_as_mesh">
<property name="text">
<string>View meshes</string>
</property>
</action>
<action name="actionSave_mesh_ply_vtk_stl">
<property name="text">
<string>Save meshes (*.ply *.vtk)...</string>
</property>
</action>
<action name="actionTrigger_a_new_map">
<property name="text">
<string>Trigger a new map</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
@@ -782,6 +940,18 @@
<header>ConsoleWidget.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>rtabmap::CloudViewer</class>
<extends>QWidget</extends>
<header>../include/rtabmap/gui/CloudViewer.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>rtabmap::LoopClosureViewer</class>
<extends>QWidget</extends>
<header>../include/rtabmap/gui/LoopClosureViewer.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources>
<include location="../GuiLib.qrc"/>
guilib/src/ui/preferencesDialog.ui
+3468 -1504
View File
File diff suppressed because it is too large Load Diff
tools/CMakeLists.txt
+8 -2
View File
@@ -4,8 +4,14 @@ ADD_SUBDIRECTORY( ImagesJoiner )
ADD_SUBDIRECTORY( ImagesDbExtractor )
ADD_SUBDIRECTORY( VocabularyComparison )
IF(TARGET rtabmap_guilib)
ADD_SUBDIRECTORY( viewer )
#On linux, we use ros package as OpenniCamera
IF(WIN32 OR APPLE)
ADD_SUBDIRECTORY( OdometryViewer )
ADD_SUBDIRECTORY( DataRecorder )
ENDIF(WIN32 OR APPLE)
IF(TARGET rtabmap_gui)
ADD_SUBDIRECTORY( DatabaseViewer )
ADD_SUBDIRECTORY( EpipolarGeometry )
ELSE()
MESSAGE(STATUS "RTAB-Map GUI lib is not built, the databaseViewer and epipolarGeometry programs will not be built...")

Some files were not shown because too many files have changed in this diff Show More