i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dronecrew-g2o_prior' into devel

Browse Source
This commit is contained in:
matlabbe
2017-05-22 21:14:02 -04:00
parent e361abcecd e8af8f7792
commit c8e1060ea4
14 changed files with 401 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@@ -1,6 +1,8 @@
/lib /lib
.DS_Store .DS_Store
.settings/language.settings.xml .settings/language.settings.xml
.idea/
cmake-build-debug/
app/android/.classpath app/android/.classpath
app/android/.project app/android/.project
app/android/AndroidManifest.xml app/android/AndroidManifest.xml

130
CMakeLists.txt
View File

@@ -46,25 +46,25 @@ IF(NOT ANDROID)
ENDIF(NOT ANDROID) ENDIF(NOT ANDROID)
IF(WIN32 AND NOT MINGW) IF(WIN32 AND NOT MINGW)
ADD_DEFINITIONS("-DNOMINMAX") ADD_DEFINITIONS("-DNOMINMAX")
ADD_DEFINITIONS("-wd4100 -wd4127 -wd4150 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986") ADD_DEFINITIONS("-wd4100 -wd4127 -wd4150 -wd4191 -wd4242 -wd4244 -wd4251 -wd4305 -wd4365 -wd4512 -wd4514 -wd4548 -wd4571 -wd4619 -wd4625 -wd4626 -wd4628 -wd4668 -wd4710 -wd4711 -wd4738 -wd4820 -wd4946 -wd4986")
ELSE () ELSE ()
ADD_DEFINITIONS( "-Wall" ) ADD_DEFINITIONS( "-Wall" )
ADD_DEFINITIONS("-Wno-unknown-pragmas") ADD_DEFINITIONS("-Wno-unknown-pragmas")
ENDIF() ENDIF()
if(POLICY CMP0020) if(POLICY CMP0020)
cmake_policy(SET CMP0020 OLD) cmake_policy(SET CMP0020 OLD)
endif() endif()
if(POLICY CMP0043) if(POLICY CMP0043)
cmake_policy(SET CMP0043 OLD) cmake_policy(SET CMP0043 OLD)
endif() endif()
IF(MINGW) IF(MINGW)
# Hide the --enable-auto-import warning # Hide the --enable-auto-import warning
SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--enable-auto-import") SET(CMAKE_EXE_LINKER_FLAGS "-Wl,--enable-auto-import")
SET(CMAKE_MODULE_LINKER_FLAGS "-Wl,--enable-auto-import") SET(CMAKE_MODULE_LINKER_FLAGS "-Wl,--enable-auto-import")
SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import") SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--enable-auto-import")
ENDIF(MINGW) ENDIF(MINGW)
# GCC 4 required # GCC 4 required
@@ -125,7 +125,7 @@ set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
####### BUILD OPTIONS ####### ####### BUILD OPTIONS #######
IF(APPLE) IF(APPLE)
OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF) OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
ENDIF(APPLE) ENDIF(APPLE)
OPTION(BUILD_APP "Build main application" ON) OPTION(BUILD_APP "Build main application" ON)
OPTION(BUILD_TOOLS "Build tools" ON) OPTION(BUILD_TOOLS "Build tools" ON)
@@ -232,18 +232,18 @@ ENDIF(WITH_FREENECT)
IF(WITH_FREENECT2) IF(WITH_FREENECT2)
FIND_PACKAGE(freenect2 QUIET) FIND_PACKAGE(freenect2 QUIET)
IF(freenect2_FOUND) IF(freenect2_FOUND)
IF(NOT freenect2_INCLUDE_DIRS) IF(NOT freenect2_INCLUDE_DIRS)
SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR}) SET(freenect2_INCLUDE_DIRS ${freenect2_INCLUDE_DIR})
ENDIF(NOT freenect2_INCLUDE_DIRS) ENDIF(NOT freenect2_INCLUDE_DIRS)
MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}") MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}")
# Explicitly link to OpenCL (in case of CUDA installed) # Explicitly link to OpenCL (in case of CUDA installed)
FIND_PACKAGE(OpenCL QUIET) FIND_PACKAGE(OpenCL QUIET)
IF(OpenCL_FOUND) IF(OpenCL_FOUND)
SET(freenect2_LIBRARIES SET(freenect2_LIBRARIES
${OpenCL_LIBRARIES} ${OpenCL_LIBRARIES}
${freenect2_LIBRARIES} ${freenect2_LIBRARIES}
) )
ENDIF(OpenCL_FOUND) ENDIF(OpenCL_FOUND)
ENDIF(freenect2_FOUND) ENDIF(freenect2_FOUND)
ENDIF(WITH_FREENECT2) ENDIF(WITH_FREENECT2)
@@ -281,47 +281,47 @@ IF(WITH_FLYCAPTURE2)
ENDIF(WITH_FLYCAPTURE2) ENDIF(WITH_FLYCAPTURE2)
IF(WITH_CVSBA) IF(WITH_CVSBA)
FIND_PACKAGE(cvsba QUIET) FIND_PACKAGE(cvsba QUIET)
IF(cvsba_FOUND) IF(cvsba_FOUND)
MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}") MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
ENDIF(cvsba_FOUND) ENDIF(cvsba_FOUND)
ENDIF(WITH_CVSBA) ENDIF(WITH_CVSBA)
IF(WITH_ZED) IF(WITH_ZED)
IF(WIN32) # Windows IF(WIN32) # Windows
SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS}) SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
if (CMAKE_CL_64) # 64 bits if (CMAKE_CL_64) # 64 bits
SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64}) SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
else(CMAKE_CL_64) # 32 bits else(CMAKE_CL_64) # 32 bits
message("32bits compilation is no more available with CUDA7.0") message("32bits compilation is no more available with CUDA7.0")
endif(CMAKE_CL_64) endif(CMAKE_CL_64)
SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR}) SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS) IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
SET(ZED_FOUND TRUE) SET(ZED_FOUND TRUE)
LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR}) LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS) ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
ELSE() # Linux ELSE() # Linux
find_package(ZED 1 QUIET) find_package(ZED 1 QUIET)
ENDIF(WIN32) ENDIF(WIN32)
IF(ZED_FOUND) IF(ZED_FOUND)
MESSAGE(STATUS "Found ZED sdk: ${ZED_INCLUDE_DIRS}") MESSAGE(STATUS "Found ZED sdk: ${ZED_INCLUDE_DIRS}")
## look for CUDA ## look for CUDA
find_package(CUDA) find_package(CUDA)
IF(CUDA_FOUND) IF(CUDA_FOUND)
MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}") MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}")
ELSE() ELSE()
MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.") MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
ENDIF() ENDIF()
ENDIF(ZED_FOUND) ENDIF(ZED_FOUND)
ENDIF(WITH_ZED) ENDIF(WITH_ZED)
IF(WITH_REALSENSE) IF(WITH_REALSENSE)
IF(WITH_REALSENSE_SLAM) IF(WITH_REALSENSE_SLAM)
FIND_PACKAGE(RealSense QUIET COMPONENTS slam) FIND_PACKAGE(RealSense QUIET COMPONENTS slam)
ELSE() ELSE()
FIND_PACKAGE(RealSense QUIET) FIND_PACKAGE(RealSense QUIET)
ENDIF() ENDIF()
IF(RealSense_FOUND) IF(RealSense_FOUND)
MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}") MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}")
ENDIF(RealSense_FOUND) ENDIF(RealSense_FOUND)
@@ -331,14 +331,14 @@ IF(WITH_REALSENSE)
ENDIF(WITH_REALSENSE) ENDIF(WITH_REALSENSE)
IF(WITH_OCTOMAP) IF(WITH_OCTOMAP)
FIND_PACKAGE(OCTOMAP QUIET) FIND_PACKAGE(OCTOMAP QUIET)
IF(OCTOMAP_FOUND) IF(OCTOMAP_FOUND)
MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}") MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}")
ENDIF(OCTOMAP_FOUND) ENDIF(OCTOMAP_FOUND)
ENDIF(WITH_OCTOMAP) ENDIF(WITH_OCTOMAP)
IF(WITH_CPUTSDF) IF(WITH_CPUTSDF)
FIND_PACKAGE(CPUTSDF QUIET) FIND_PACKAGE(CPUTSDF QUIET)
IF(CPUTSDF_FOUND) IF(CPUTSDF_FOUND)
MESSAGE(STATUS "Found CPUTSDF: ${CPUTSDF_INCLUDE_DIRS}") MESSAGE(STATUS "Found CPUTSDF: ${CPUTSDF_INCLUDE_DIRS}")
ENDIF(CPUTSDF_FOUND) ENDIF(CPUTSDF_FOUND)
@@ -348,15 +348,15 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
#Newest versions require std11 #Newest versions require std11
IF(NOT MSVC) IF(NOT MSVC)
include(CheckCXXCompilerFlag) include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
IF(COMPILER_SUPPORTS_CXX11) IF(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
ELSEIF(COMPILER_SUPPORTS_CXX0X) ELSEIF(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
ELSE() ELSE()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).") message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
ENDIF() ENDIF()
ENDIF() ENDIF()
ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND) ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND)
@@ -392,7 +392,7 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
# CONF_DEPENDENCIES contains only dependencies not required by the headers # CONF_DEPENDENCIES contains only dependencies not required by the headers
SET(CONF_DEPENDENCIES SET(CONF_DEPENDENCIES
${ZLIB_LIBRARIES} ${ZLIB_LIBRARIES}
) )
IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)) IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
SET(NONFREE "//") SET(NONFREE "//")
@@ -520,8 +520,8 @@ file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "$
# ... for the build tree # ... for the build tree
set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include"
"${PROJECT_SOURCE_DIR}/guilib/include" "${PROJECT_SOURCE_DIR}/guilib/include"
"${PROJECT_SOURCE_DIR}/utilite/include") "${PROJECT_SOURCE_DIR}/utilite/include")
set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}") set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
IF(QT4_FOUND OR Qt5_FOUND) IF(QT4_FOUND OR Qt5_FOUND)
set(CONF_WITH_GUI ON) set(CONF_WITH_GUI ON)
@@ -646,8 +646,8 @@ MESSAGE(STATUS " BUILD_APP = ${BUILD_APP}")
MESSAGE(STATUS " BUILD_TOOLS = ${BUILD_TOOLS}") MESSAGE(STATUS " BUILD_TOOLS = ${BUILD_TOOLS}")
MESSAGE(STATUS " BUILD_EXAMPLES = ${BUILD_EXAMPLES}") MESSAGE(STATUS " BUILD_EXAMPLES = ${BUILD_EXAMPLES}")
IF(NOT WIN32) IF(NOT WIN32)
# see comment above for the BUILD_SHARED_LIBS option on Windows # see comment above for the BUILD_SHARED_LIBS option on Windows
MESSAGE(STATUS " BUILD_SHARED_LIBS = ${BUILD_SHARED_LIBS}") MESSAGE(STATUS " BUILD_SHARED_LIBS = ${BUILD_SHARED_LIBS}")
ENDIF(NOT WIN32) ENDIF(NOT WIN32)
IF(APPLE) IF(APPLE)
MESSAGE(STATUS " BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}") MESSAGE(STATUS " BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
@@ -807,3 +807,5 @@ MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of th
GTSAM (https://collab.cc.gatech.edu/borg/gtsam) GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
set -DWITH_TORO=ON") set -DWITH_TORO=ON")
ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO) ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
# vim: set et ft=cmake fenc=utf-8 ff=unix sts=0 sw=2 ts=2 :

3
corelib/include/rtabmap/core/Link.h
View File

@@ -46,7 +46,8 @@ public:
kUserClosure, kUserClosure,
kVirtualClosure, kVirtualClosure,
kNeighborMerged, kNeighborMerged,
kUndef}; kPosePrior,
kUndef = 99};
Link(); Link();
Link(int from, Link(int from,
int to, int to,

7
corelib/include/rtabmap/core/SensorData.h
View File

@@ -221,6 +221,10 @@ public:
void setGroundTruth(const Transform & pose) {groundTruth_ = pose;} void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
const Transform & groundTruth() const {return groundTruth_;} const Transform & groundTruth() const {return groundTruth_;}
void setGlobalPose(const Transform & pose, const cv::Mat & covariance) {globalPose_ = pose; globalPoseCovariance_ = covariance;}
const Transform & globalPose() const {return globalPose_;}
const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
long getMemoryUsed() const; // Return memory usage in Bytes long getMemoryUsed() const; // Return memory usage in Bytes
private: private:
@@ -258,6 +262,9 @@ private:
cv::Mat _descriptors; cv::Mat _descriptors;
Transform groundTruth_; Transform groundTruth_;
Transform globalPose_;
cv::Mat globalPoseCovariance_; // 6x6 double
}; };
} }

2
corelib/src/DBDriver.cpp
View File

@@ -1127,7 +1127,7 @@ void DBDriver::generateGraph(
weightNeighbor, weightNeighbor,
colorG); colorG);
} }
else else if(iter->first != id)
{ {
//child //child
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",

101
corelib/src/Graph.cpp
View File

@@ -1456,61 +1456,64 @@ std::list<std::pair<int, Transform> > computePath(
} }
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter) for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{ {
Transform nextPose = currentNode->pose()*iter->second.transform(); if(iter->second.from() != iter->second.to())
float cost = 0.0f;
if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
{ {
// use distance only Transform nextPose = currentNode->pose()*iter->second.transform();
cost = iter->second.transform().getNorm(); float cost = 0.0f;
} if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
else // use time
{
if(linearVelocity > 0.0f)
{ {
cost += iter->second.transform().getNorm()/linearVelocity; // use distance only
cost = iter->second.transform().getNorm();
} }
if(angularVelocity > 0.0f) else // use time
{ {
Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f); if(linearVelocity > 0.0f)
Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
float angle = pcl::getAngle3D(v1, v2);
cost += angle / angularVelocity;
}
}
std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
if(nodeIter == nodes.end())
{
Node n(iter->second.to(), currentNode->id(), nextPose);
n.setCostSoFar(currentNode->costSoFar() + cost);
nodes.insert(std::make_pair(iter->second.to(), n));
if(updateNewCosts)
{
pqmap.insert(std::make_pair(n.totalCost(), n.id()));
}
else
{
pq.push(Pair(n.id(), n.totalCost()));
}
}
else if(updateNewCosts && nodeIter->second.isOpened())
{
float newCostSoFar = currentNode->costSoFar() + cost;
if(nodeIter->second.costSoFar() > newCostSoFar)
{
// update pose with new link
nodeIter->second.setPose(nextPose);
// update the cost in the priority queue
for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
{ {
if(mapIter->second == nodeIter->first) cost += iter->second.transform().getNorm()/linearVelocity;
}
if(angularVelocity > 0.0f)
{
Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f);
Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
float angle = pcl::getAngle3D(v1, v2);
cost += angle / angularVelocity;
}
}
std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
if(nodeIter == nodes.end())
{
Node n(iter->second.to(), currentNode->id(), nextPose);
n.setCostSoFar(currentNode->costSoFar() + cost);
nodes.insert(std::make_pair(iter->second.to(), n));
if(updateNewCosts)
{
pqmap.insert(std::make_pair(n.totalCost(), n.id()));
}
else
{
pq.push(Pair(n.id(), n.totalCost()));
}
}
else if(updateNewCosts && nodeIter->second.isOpened())
{
float newCostSoFar = currentNode->costSoFar() + cost;
if(nodeIter->second.costSoFar() > newCostSoFar)
{
// update pose with new link
nodeIter->second.setPose(nextPose);
// update the cost in the priority queue
for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
{ {
pqmap.erase(mapIter); if(mapIter->second == nodeIter->first)
nodeIter->second.setCostSoFar(newCostSoFar); {
pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first)); pqmap.erase(mapIter);
break; nodeIter->second.setCostSoFar(newCostSoFar);
pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
break;
}
} }
} }
} }

89
corelib/src/Memory.cpp
View File

@@ -805,6 +805,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
if(!merge) if(!merge)
{ {
merge = iter->second.to() < s->id() && // should be a parent->child link merge = iter->second.to() < s->id() && // should be a parent->child link
iter->second.to() != iter->second.from() &&
iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged && iter->second.type() != Link::kNeighborMerged &&
iter->second.userDataCompressed().empty() && iter->second.userDataCompressed().empty() &&
@@ -958,6 +959,7 @@ std::map<int, Link> Memory::getLoopClosureLinks(
{ {
if(iter->second.type() != Link::kNeighbor && if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged && iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior &&
iter->second.type() != Link::kUndef) iter->second.type() != Link::kUndef)
{ {
loopClosures.insert(*iter); loopClosures.insert(*iter);
@@ -1843,27 +1845,30 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
const std::map<int, Link> & links = s->getLinks(); const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{ {
Signature * sTo = this->_getSignature(iter->first); if(iter->second.from() != iter->second.to())
// neighbor to s
UASSERT_MSG(sTo!=0,
uFormat("A neighbor (%d) of the deleted location %d is "
"not found in WM/STM! Are you deleting a location "
"outside the STM?", iter->first, s->id()).c_str());
if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
{ {
UWARN("Link %d of %d is newer, removing neighbor link " Signature * sTo = this->_getSignature(iter->first);
"may split the map!", // neighbor to s
iter->first, s->id()); UASSERT_MSG(sTo!=0,
} uFormat("A neighbor (%d) of the deleted location %d is "
"not found in WM/STM! Are you deleting a location "
"outside the STM?", iter->first, s->id()).c_str());
// child if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id()) {
{ UWARN("Link %d of %d is newer, removing neighbor link "
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight "may split the map!",
} iter->first, s->id());
}
sTo->removeLink(s->id()); // child
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
{
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
}
sTo->removeLink(s->id());
}
} }
s->removeLinks(); // remove all links s->removeLinks(); // remove all links
@@ -2089,6 +2094,7 @@ void Memory::removeLink(int oldId, int newId)
{ {
if(iter->second.type() != Link::kNeighbor && if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged && iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior &&
iter->first < newS->id()) iter->first < newS->id())
{ {
noChildrenAnymore = false; noChildrenAnymore = false;
@@ -2694,7 +2700,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
{ {
childIds.insert(*iter); childIds.insert(*iter);
} }
else else if(iter->second.from() != iter->second.to())
{ {
loopIds.insert(*iter); loopIds.insert(*iter);
} }
@@ -2724,8 +2730,7 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
void Memory::rehearsal(Signature * signature, Statistics * stats) void Memory::rehearsal(Signature * signature, Statistics * stats)
{ {
UTimer timer; UTimer timer;
if(signature->getLinks().size() != 1 || if(signature->isBadSignature())
signature->isBadSignature())
{ {
return; return;
} }
@@ -2790,7 +2795,8 @@ bool Memory::rehearsalMerge(int oldId, int newId)
std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id()); std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
if(iter != oldS->getLinks().end() && if(iter != oldS->getLinks().end() &&
iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged) iter->second.type() != Link::kNeighborMerged &&
iter->second.from() != iter->second.to())
{ {
// do nothing, already merged // do nothing, already merged
UWARN("already merged, old=%d, new=%d", oldId, newId); UWARN("already merged, old=%d, new=%d", oldId, newId);
@@ -2804,7 +2810,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
bool fullMerge; bool fullMerge;
bool intermediateMerge = false; bool intermediateMerge = false;
if(!newS->getLinks().begin()->second.transform().isNull()) if(!newS->getLinks().empty() && !newS->getLinks().begin()->second.transform().isNull())
{ {
// we are in metric SLAM mode: // we are in metric SLAM mode:
// 1) Normal merge if not moving AND has direct link // 1) Normal merge if not moving AND has direct link
@@ -2844,22 +2850,25 @@ bool Memory::rehearsalMerge(int oldId, int newId)
const std::map<int, Link> & links = oldS->getLinks(); const std::map<int, Link> & links = oldS->getLinks();
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter) for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{ {
Link link = iter->second; if(iter->second.from() != iter->second.to())
Link mergedLink = newToOldLink.merge(link, link.type());
UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
Signature * s = this->_getSignature(link.to());
if(s)
{ {
// modify neighbor "from" Link link = iter->second;
s->removeLink(oldS->id()); Link mergedLink = newToOldLink.merge(link, link.type());
s->addLink(mergedLink.inverse()); UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
newS->addLink(mergedLink); Signature * s = this->_getSignature(link.to());
} if(s)
else {
{ // modify neighbor "from"
UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id()); s->removeLink(oldS->id());
s->addLink(mergedLink.inverse());
newS->addLink(mergedLink);
}
else
{
UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
}
} }
} }
newS->setLabel(oldS->getLabel()); newS->setLabel(oldS->getLabel());
@@ -3847,6 +3856,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
} }
s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint); s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
// prior
if(!isIntermediateNode && !data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
{
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
}
return s; return s;
} }

270
corelib/src/OptimizerG2O.cpp
View File

@@ -47,6 +47,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "g2o/core/optimization_algorithm_levenberg.h" #include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/linear_solver.h" #include "g2o/core/linear_solver.h"
#include "g2o/types/sba/types_sba.h" #include "g2o/types/sba/types_sba.h"
#include "g2o/types/slam2d/types_slam2d.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/core/robust_kernel_impl.h" #include "g2o/core/robust_kernel_impl.h"
#ifdef G2O_HAVE_CSPARSE #ifdef G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h" #include "g2o/solvers/csparse/linear_solver_csparse.h"
@@ -56,10 +58,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "g2o/solvers/cholmod/linear_solver_cholmod.h" #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#endif #endif
#include "g2o/solvers/eigen/linear_solver_eigen.h" #include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/edge_se3.h" enum {
#include "g2o/types/slam2d/vertex_se2.h" PARAM_OFFSET=0,
#include "g2o/types/slam2d/edge_se2.h" };
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver; typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver; typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
@@ -165,6 +167,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::SparseOptimizer optimizer; g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug); optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
g2o::ParameterSE3Offset* odomOffset = new g2o::ParameterSE3Offset();
odomOffset->setId(PARAM_OFFSET);
optimizer.addParameter(odomOffset);
SlamBlockSolver * blockSolver = 0; SlamBlockSolver * blockSolver = 0;
@@ -211,6 +216,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
} }
UDEBUG("fill poses to g2o..."); UDEBUG("fill poses to g2o...");
std::map<int, std::pair<Transform, cv::Mat> > geoPoses; // pose / information matrix
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
UASSERT(!iter->second.isNull()); UASSERT(!iter->second.isNull());
@@ -255,127 +261,175 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::HyperGraph::Edge * edge = 0; g2o::HyperGraph::Edge * edge = 0;
#ifdef RTABMAP_VERTIGO if(id1 == id2)
VertexSwitchLinear * v = 0;
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{ {
// For loop closure links, add switchable edges if(isSlam2d())
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// a proper and convenient initial value for all switch variables would be
// sij = 1 when using the linear switch function"
v = new VertexSwitchLinear();
v->setEstimate(1.0);
v->setId(vertigoVertexId++);
UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
EdgeSwitchPrior * prior = new EdgeSwitchPrior();
prior->setMeasurement(1.0);
prior->setVertex(0, v);
UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
}
#endif
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{ {
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE2Switchable * e = new EdgeSE2Switchable();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); priorEdge->setVertex(0, v1);
UASSERT(v1 != 0); priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
UASSERT(v2 != 0); priorEdge->setParameterId(0, PARAM_OFFSET);
e->setVertex(0, v1); Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
e->setVertex(1, v2); if(!isCovarianceIgnored())
e->setVertex(2, v); {
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
e->setInformation(information); information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
edge = e; information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
priorEdge->setInformation(information);
edge = priorEdge;
} }
else else
#endif
{ {
g2o::EdgeSE2 * e = new g2o::EdgeSE2(); g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); priorEdge->setVertex(0, v1);
UASSERT(v1 != 0); Eigen::Affine3d a = iter->second.transform().toEigen3d();
UASSERT(v2 != 0); Eigen::Isometry3d pose;
e->setVertex(0, v1); pose = a.rotation();
e->setVertex(1, v2); pose.translation() = a.translation();
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); priorEdge->setMeasurement(pose);
e->setInformation(information); priorEdge->setParameterId(0, PARAM_OFFSET);
edge = e; Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
priorEdge->setInformation(information);
edge = priorEdge;
} }
} }
else else
{ {
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d constraint;
constraint = a.rotation();
constraint.translation() = a.translation();
#ifdef RTABMAP_VERTIGO #ifdef RTABMAP_VERTIGO
VertexSwitchLinear * v = 0;
if(this->isRobust() && if(this->isRobust() &&
iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged) iter->second.type() != Link::kNeighborMerged)
{ {
EdgeSE3Switchable * e = new EdgeSE3Switchable(); // For loop closure links, add switchable edges
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); // create new switch variable
UASSERT(v1 != 0); // Sunderhauf IROS 2012:
UASSERT(v2 != 0); // "Since it is reasonable to initially accept all loop closure constraints,
e->setVertex(0, v1); // a proper and convenient initial value for all switch variables would be
e->setVertex(1, v2); // sij = 1 when using the linear switch function"
e->setVertex(2, v); v = new VertexSwitchLinear();
e->setMeasurement(constraint); v->setEstimate(1.0);
e->setInformation(information); v->setId(vertigoVertexId++);
edge = e; UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
EdgeSwitchPrior * prior = new EdgeSwitchPrior();
prior->setMeasurement(1.0);
prior->setVertex(0, v);
UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
}
#endif
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE2Switchable * e = new EdgeSE2Switchable();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
}
else
#endif
{
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
}
} }
else else
#endif
{ {
g2o::EdgeSE3 * e = new g2o::EdgeSE3(); Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); if(!isCovarianceIgnored())
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); {
UASSERT(v1 != 0); memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
UASSERT(v2 != 0); }
e->setVertex(0, v1);
e->setVertex(1, v2); Eigen::Affine3d a = iter->second.transform().toEigen3d();
e->setMeasurement(constraint); Eigen::Isometry3d constraint;
e->setInformation(information); constraint = a.rotation();
edge = e; constraint.translation() = a.translation();
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE3Switchable * e = new EdgeSE3Switchable();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
else
#endif
{
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
} }
} }
@@ -384,6 +438,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
delete edge; delete edge;
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2); UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
} }
} }
UDEBUG("Initial optimization..."); UDEBUG("Initial optimization...");

5
corelib/src/OptimizerGTSAM.cpp
View File

@@ -134,6 +134,11 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{ {
int id1 = iter->second.from(); int id1 = iter->second.from();
int id2 = iter->second.to(); int id2 = iter->second.to();
if(id1 == id2)
{
// not supporting pose prior
continue;
}
UASSERT(!iter->second.transform().isNull()); UASSERT(!iter->second.transform().isNull());

36
corelib/src/OptimizerTORO.cpp
View File

@@ -126,15 +126,19 @@ std::map<int, Transform> OptimizerTORO::optimize(
int id1 = iter->second.from(); int id1 = iter->second.from();
int id2 = iter->second.to(); int id2 = iter->second.to();
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1); if(id1 != id2)
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph2::Transformation t(p);
if (!pg2.addEdge(v1, v2, t, inf))
{ {
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2); AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph2::Transformation t(p);
if (!pg2.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
} }
//else // not supporting pose prior
} }
} }
else else
@@ -154,15 +158,19 @@ std::map<int, Transform> OptimizerTORO::optimize(
int id1 = iter->second.from(); int id1 = iter->second.from();
int id2 = iter->second.to(); int id2 = iter->second.to();
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1); if(id1 != id2)
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg3.addEdge(v1, v2, t, inf))
{ {
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2); AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg3.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
} }
//else // not supporting pose prior
} }
} }
UDEBUG("buildMST... root=%d", rootId); UDEBUG("buildMST... root=%d", rootId);

41
corelib/src/Rtabmap.cpp
View File

@@ -1033,7 +1033,7 @@ bool Rtabmap::process(
// Minimum displacement required to add to Memory // Minimum displacement required to add to Memory
//============================================================ //============================================================
const std::map<int, Link> & links = signature->getLinks(); const std::map<int, Link> & links = signature->getLinks();
if(links.size() == 1) if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{ {
// don't do this if there are intermediate nodes // don't do this if there are intermediate nodes
const Signature * s = _memory->getSignature(links.begin()->second.to()); const Signature * s = _memory->getSignature(links.begin()->second.to());
@@ -1061,7 +1061,8 @@ bool Rtabmap::process(
// Update optimizedPoses with the newly added node // Update optimizedPoses with the newly added node
Transform newPose; Transform newPose;
if(_neighborLinkRefining && if(_neighborLinkRefining &&
signature->getLinks().size() == 1 && signature->getLinks().size() &&
signature->getLinks().begin()->second.type() == Link::kNeighbor &&
_memory->isIncremental() && // ignore pose matching in localization mode _memory->isIncremental() && // ignore pose matching in localization mode
rehearsedId == 0) // don't do it if rehearsal happened rehearsedId == 0) // don't do it if rehearsal happened
{ {
@@ -1166,7 +1167,7 @@ bool Rtabmap::process(
// Update Poses and Constraints // Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose)); _optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose _lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(signature->getLinks().size() == 1 && if(signature->getLinks().size() &&
signature->getLinks().begin()->second.type() == Link::kNeighbor) signature->getLinks().begin()->second.type() == Link::kNeighbor)
{ {
// link should be old to new // link should be old to new
@@ -1297,7 +1298,7 @@ bool Rtabmap::process(
//============================================================ //============================================================
// Bayes filter update // Bayes filter update
//============================================================ //============================================================
int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0; int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0;
// Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure // Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure
if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0)) if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
{ {
@@ -1383,7 +1384,7 @@ bool Rtabmap::process(
{ {
float loopThr = _loopThr; float loopThr = _loopThr;
if((_startNewMapOnLoopClosure || !_memory->isIncremental()) && if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
signature->getLinks().size() == 0 && // alone in the current map graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature) _memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() && (int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
_rgbdSlamMode) _rgbdSlamMode)
@@ -2154,14 +2155,16 @@ bool Rtabmap::process(
(_loopClosureHypothesis.first>0 || (_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() || statistics_.reducedIds().size() ||
signature->hasLink(signature->id()) || // prior edge
proximityDetectionsInTimeFound>0 || proximityDetectionsInTimeFound>0 ||
((_memory->isIncremental() || signature->getLinks().size()) && // In localization mode, the new node should be linked ((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kPosePrior).size()) && // In localization mode, the new node should be linked
signaturesRetrieved.size()))) // can be different map of the current one signaturesRetrieved.size()))) // can be different map of the current one
{ {
UASSERT(uContains(_optimizedPoses, signature->id())); UASSERT(uContains(_optimizedPoses, signature->id()));
//used in localization mode: filter virtual links //used in localization mode: filter virtual links
std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure); std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
localizationLinks = graph::filterLinks(localizationLinks, Link::kPosePrior);
// Note that in localization mode, we don't re-optimize the graph // Note that in localization mode, we don't re-optimize the graph
// if: // if:
@@ -2239,7 +2242,7 @@ bool Rtabmap::process(
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{ {
// ignore links with high variance // ignore links with high variance
if(iter->second.transVariance() <= 1.0) if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{ {
Transform t1 = uValue(poses, iter->second.from(), Transform()); Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform()); Transform t2 = uValue(poses, iter->second.to(), Transform());
@@ -2437,7 +2440,7 @@ bool Rtabmap::process(
Signature lastSignatureData(signature->id()); Signature lastSignatureData(signature->id());
Transform lastSignatureLocalizedPose; Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && signature->getLinks().size()) if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && graph::filterLinks(signature->getLinks(), Link::kPosePrior).size())
{ {
// only if localized set it // only if localized set it
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id()); lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
@@ -2466,7 +2469,7 @@ bool Rtabmap::process(
{ {
if(_startNewMapOnLoopClosure && if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode _memory->isIncremental() && // only in mapping mode
signature->getLinks().size() == 0 && // alone in the current map graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature) _memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
{ {
UWARN("Ignoring location %d because a global loop closure is required before starting a new map!", UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -3121,18 +3124,26 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
} }
} }
} }
int ignoredLinks = 0;
if(edgeConstraints.size() != linksOut.size()) if(edgeConstraints.size() != linksOut.size())
{ {
for(std::multimap<int, Link>::iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) for(std::multimap<int, Link>::iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{ {
if(graph::findLink(linksOut, iter->second.from(), iter->second.to()) == linksOut.end()) if(graph::findLink(linksOut, iter->second.from(), iter->second.to()) == linksOut.end())
{ {
UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to()); if(iter->second.type() == Link::kPosePrior)
{
++ignoredLinks;
}
else
{
UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to());
}
} }
} }
} }
UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size() == linksOut.size(), UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size()-ignoredLinks == linksOut.size(),
uFormat("nodes %d->%d, links %d->%d", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size()).c_str()); uFormat("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks).c_str());
} }
if(constraints) if(constraints)
@@ -3143,7 +3154,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
UASSERT(_graphOptimizer!=0); UASSERT(_graphOptimizer!=0);
if(_graphOptimizer->iterations() == 0) if(_graphOptimizer->iterations() == 0)
{ {
// Optimization desactivated! Return not optimized poses. // Optimization disabled! Return not optimized poses.
optimizedPoses = poses; optimizedPoses = poses;
} }
else else
@@ -3574,7 +3585,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{ {
// ignore links with high variance // ignore links with high variance
if(iter->second.transVariance() <= 1.0) if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{ {
UASSERT(optimizedPoses.find(iter->second.from())!=optimizedPoses.end()); UASSERT(optimizedPoses.find(iter->second.from())!=optimizedPoses.end());
UASSERT(optimizedPoses.find(iter->second.to())!=optimizedPoses.end()); UASSERT(optimizedPoses.find(iter->second.to())!=optimizedPoses.end());
@@ -3906,7 +3917,7 @@ bool Rtabmap::computePath(const Transform & targetPose)
for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter) for(std::map<int, Link>::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter)
{ {
// only add links for which poses are in "nodes" // only add links for which poses are in "nodes"
if(uContains(nodes, jter->second.to())) if(jter->second.from() != jter->second.to() && uContains(nodes, jter->second.to()))
{ {
links.insert(std::make_pair(jter->second.from(), jter->second.to())); links.insert(std::make_pair(jter->second.from(), jter->second.to()));
//links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> (commented: already added when iterating in nodes) //links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> (commented: already added when iterating in nodes)

2
corelib/src/Signature.cpp
View File

@@ -120,7 +120,7 @@ void Signature::addLink(const Link & link)
{ {
UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance()); UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str()); UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() != this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str()); UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link)); std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str()); UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
_linksModified = true; _linksModified = true;

2
guilib/src/DatabaseViewer.cpp
View File

@@ -4879,7 +4879,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{ {
// ignore links with high variance // ignore links with high variance
if(iter->second.transVariance() <= 1.0) if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{ {
UASSERT(poses.find(iter->second.from())!=poses.end()); UASSERT(poses.find(iter->second.from())!=poses.end());
UASSERT(poses.find(iter->second.to())!=poses.end()); UASSERT(poses.find(iter->second.to())!=poses.end());

2
guilib/src/MainWindow.cpp
View File

@@ -5137,7 +5137,7 @@ void MainWindow::postProcessing()
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{ {
// ignore links with high variance // ignore links with high variance
if(iter->second.transVariance() <= 1.0) if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{ {
UASSERT(poses.find(iter->second.from())!=poses.end()); UASSERT(poses.find(iter->second.from())!=poses.end());
UASSERT(poses.find(iter->second.to())!=poses.end()); UASSERT(poses.find(iter->second.to())!=poses.end());