i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 17:47:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap into gtest

Browse Source
This commit is contained in:
matlabbe
2025-12-21 11:59:54 -08:00
parent 301b6bf49c 92aff2129f
commit b8ecc36052
129 changed files with 11607 additions and 5269 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CMakeLists.txt
+42 -8
View File
@@ -41,6 +41,7 @@ SET(SRC_FILES
camera/CameraMyntEye.cpp
camera/CameraDepthAI.cpp
camera/CameraSeerSense.cpp
camera/CameraOrbbecSDK.cpp
EpipolarGeometry.cpp
VisualWord.cpp
@@ -98,9 +99,10 @@ SET(SRC_FILES
odometry/OdometryLOAM.cpp
odometry/OdometryFLOAM.cpp
odometry/OdometryMSCKF.cpp
odometry/OdometryVINS.cpp
odometry/OdometryVINSFusion.cpp
odometry/OdometryOpenVINS.cpp
odometry/OdometryOpen3D.cpp
odometry/OdometryCuVSLAM.cpp
IMU.cpp
IMUThread.cpp
@@ -163,10 +165,6 @@ IF(MSVC)
ENDIF(MSVC)
SET(INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/../include
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_BINARY_DIR}/include
${ZLIB_INCLUDE_DIRS}
)
@@ -217,11 +215,21 @@ IF(TORCH_FOUND)
${SRC_FILES}
superpoint_torch/SuperPoint.cc
)
SET(INCLUDE_DIRS
SET(INCLUDE_DIRS
${TORCH_INCLUDE_DIRS}
${CMAKE_CURRENT_SOURCE_DIR}/superpoint_torch
${INCLUDE_DIRS}
)
IF(WITH_PYTHON AND Python3_FOUND)
SET(SRC_FILES
${SRC_FILES}
superpoint_rpautrat/SuperpointRpautrat.cpp
)
SET(INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/superpoint_rpautrat
${INCLUDE_DIRS}
)
ENDIF(WITH_PYTHON AND Python3_FOUND)
ENDIF(TORCH_FOUND)
IF(WITH_PYTHON AND Python3_FOUND)
@@ -394,6 +402,13 @@ IF(xvsdk_FOUND)
)
ENDIF(xvsdk_FOUND)
IF(OrbbecSDK_FOUND)
SET(LIBRARIES
${LIBRARIES}
ob::OrbbecSDK
)
ENDIF(OrbbecSDK_FOUND)
IF(TARGET OpenMP::OpenMP_CXX)
SET(LIBRARIES
${LIBRARIES}
@@ -768,6 +783,13 @@ IF(ORB_SLAM_FOUND)
)
ENDIF(ORB_SLAM_FOUND)
IF(CUVSLAM_FOUND)
SET(LIBRARIES
${LIBRARIES}
cuvslam::cuvslam
)
ENDIF(CUVSLAM_FOUND)
IF(GTSAM_FOUND)
# Make sure GTSAM is built with system Eigen, not the included one in its package
IF(GTSAM_INCLUDE_DIR)
@@ -816,6 +838,7 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA
SET(RESOURCES
${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_22_0.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql
@@ -825,6 +848,13 @@ SET(RESOURCES
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_16_0.sql
)
IF(TORCH_FOUND AND WITH_PYTHON AND Python3_FOUND)
SET(RESOURCES
${RESOURCES}
${CMAKE_CURRENT_SOURCE_DIR}/superpoint_rpautrat/superpoint_to_torchscript.py
)
ENDIF()
foreach(arg ${RESOURCES})
get_filename_component(filename ${arg} NAME)
string(REPLACE "." "_" output ${filename})
@@ -857,8 +887,12 @@ generate_export_header(rtabmap_core
DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED)
target_include_directories(rtabmap_core PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include;${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
"$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR};${PUBLIC_INCLUDE_DIRS}>")
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR};${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR}>")
target_include_directories(rtabmap_core SYSTEM PUBLIC
"$<BUILD_INTERFACE:${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
"$<INSTALL_INTERFACE:${PUBLIC_INCLUDE_DIRS}>")
TARGET_LINK_LIBRARIES(rtabmap_core
PUBLIC
corelib/src/Camera.cpp
+12 -3
View File
@@ -39,7 +39,8 @@ namespace rtabmap
Camera::Camera(float imageRate, const Transform & localTransform) :
SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()),
imuFilter_(0),
publishInterIMU_(false)
publishInterIMU_(false),
imuBaseFrameConversion_(false)
{}
Camera::~Camera()
@@ -52,15 +53,23 @@ bool Camera::initFromFile(const std::string & calibrationPath)
return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
}
void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter)
void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter, bool baseFrameConversion)
{
publishInterIMU_ = enabled;
delete imuFilter_;
imuFilter_ = filter;
imuBaseFrameConversion_ = baseFrameConversion;
}
void Camera::postInterIMU(const IMU & imu, double stamp)
void Camera::postInterIMU(const IMU & imu_in, double stamp)
{
IMU imu = imu_in;
if(imuBaseFrameConversion_)
{
UASSERT(!imu.localTransform().isNull());
imu.convertToBaseFrame();
}
if(imuFilter_)
{
imuFilter_->update(
corelib/src/CameraModel.cpp
+1 -1
View File
@@ -554,7 +554,7 @@ unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int d
int iR = 8;
int iP = 9;
int iL = 10;
UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
//UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
unsigned int requiredDataSize = sizeof(int)*headerSize +
sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
sizeof(float)*header[iL];
corelib/src/DBDriver.cpp
+55 -8
View File
@@ -383,7 +383,7 @@ void DBDriver::asyncSave(Signature * s)
{
if(s)
{
UDEBUG("s=%d", s->id());
//UDEBUG("s=%d", s->id());
_trashesMutex.lock();
{
_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
@@ -531,17 +531,17 @@ void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan)
_dbSafeAccessMutex.unlock();
}
void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const
void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly) const
{
_dbSafeAccessMutex.lock();
this->loadQuery(dictionary, lastStateOnly);
_dbSafeAccessMutex.unlock();
}
void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
void DBDriver::loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly) const
{
_dbSafeAccessMutex.lock();
this->loadLastNodesQuery(signatures);
this->loadLastNodesQuery(signatures, loadWordIdsOnly);
_dbSafeAccessMutex.unlock();
}
@@ -564,7 +564,8 @@ Signature * DBDriver::loadSignature(int id, bool * loadedFromTrash)
}
void DBDriver::loadSignatures(const std::list<int> & signIds,
std::list<Signature *> & signatures,
std::set<int> * loadedFromTrash)
std::set<int> * loadedFromTrash,
bool loadWordIdsOnly)
{
UDEBUG("");
// look up in the trash before the database
@@ -609,7 +610,7 @@ void DBDriver::loadSignatures(const std::list<int> & signIds,
if(ids.size())
{
_dbSafeAccessMutex.lock();
this->loadSignaturesQuery(ids, signatures);
this->loadSignaturesQuery(ids, signatures, loadWordIdsOnly);
_dbSafeAccessMutex.unlock();
}
}
@@ -656,10 +657,10 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
}
}
void DBDriver::loadNodeData(Signature * signature, bool images, bool scan, bool userData, bool occupancyGrid) const
void DBDriver::loadNodeData(Signature & signature, bool images, bool scan, bool userData, bool occupancyGrid) const
{
std::list<Signature *> signatures;
signatures.push_back(signature);
signatures.push_back(&signature);
this->loadNodeData(signatures, images, scan, userData, occupancyGrid);
}
@@ -823,6 +824,45 @@ bool DBDriver::getNodeInfo(
return found;
}
void DBDriver::getLocalFeatures(
int signatureId,
std::multimap<int, int> & words,
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & points,
cv::Mat & descriptors) const
{
bool found = false;
// look in the trash
_trashesMutex.lock();
if(uContains(_trashSignatures, signatureId))
{
const Signature * s = _trashSignatures.at(signatureId);
UASSERT(s != 0);
found = true;
if(!s->getWords().empty())
{
words = s->getWords();
if(s->getWordsKpts().empty()){
found = false; // Force checking the database in case the local features were not loaded in RAM
}
else
{
words = s->getWords();
keypoints = s->getWordsKpts();
points = s->getWords3();
descriptors = s->getWordsDescriptors().clone();
}
}
}
_trashesMutex.unlock();
if(!found)
{
UScopeMutex lock(_dbSafeAccessMutex);
getLocalFeaturesQuery(signatureId, words, keypoints, points, descriptors);
}
}
void DBDriver::loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type) const
{
bool found = false;
@@ -1287,6 +1327,13 @@ cv::Mat DBDriver::loadOptimizedMesh(
return cloud;
}
void DBDriver::saveFlannIndex(const std::vector<unsigned char> & indexData) const
{
_dbSafeAccessMutex.lock();
saveFlannIndexQuery(indexData);
_dbSafeAccessMutex.unlock();
}
void DBDriver::generateGraph(
const std::string & fileName,
const std::set<int> & idsInput,
corelib/src/DBDriverSqlite3.cpp
+382 -183
View File
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Compression.h"
#include "DatabaseSchema_sql.h"
#include "DatabaseSchema_0_22_0_sql.h"
#include "DatabaseSchema_0_20_0_sql.h"
#include "DatabaseSchema_0_18_3_sql.h"
#include "DatabaseSchema_0_18_0_sql.h"
@@ -404,6 +405,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL));
schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
schemas.push_back(std::make_pair("0.20.0", DATABASESCHEMA_0_20_0_SQL));
schemas.push_back(std::make_pair("0.22.0", DATABASESCHEMA_0_22_0_SQL));
schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
for(size_t i=0; i<schemas.size(); ++i)
{
@@ -1296,8 +1298,8 @@ std::map<int, std::vector<int> > DBDriverSqlite3::getAllStatisticsWmStatesQuery(
void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
{
UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d",
(int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0);
//UDEBUG("load data for %d signatures images=%d scan=%d userData=%d, grid=%d",
// (int)signatures.size(), images?1:0, scan?1:0, userData?1:0, occupancyGrid?1:0);
if(!images && !scan && !userData && !occupancyGrid)
{
@@ -1445,7 +1447,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
UASSERT(*iter != 0);
ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
//ULOGGER_DEBUG("Loading data for %d...", (*iter)->id());
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -1874,7 +1876,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
//ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
}
@@ -2391,6 +2393,23 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
return found;
}
void DBDriverSqlite3::getLocalFeaturesQuery(
int signatureId,
std::multimap<int, int> & words,
std::vector<cv::KeyPoint> & keypoints,
std::vector<cv::Point3f> & points,
cv::Mat & descriptors) const
{
Signature s(signatureId);
std::list<Signature *> ids;
ids.push_back(&s);
this->loadWordsQuery(ids);
words = ids.front()->getWords();
keypoints = ids.front()->getWordsKpts();
points = ids.front()->getWords3();
descriptors = ids.front()->getWordsDescriptors().clone();
}
void DBDriverSqlite3::getLastNodeIdsQuery(std::set<int> & ids) const
{
if(_ppDb)
@@ -2987,7 +3006,7 @@ void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
}
//may be slower than the previous version but don't have a limit of words that can be loaded at the same time
void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes, bool loadWordIdsOnly) const
{
ULOGGER_DEBUG("count=%d", (int)ids.size());
if(_ppDb && ids.size())
@@ -3151,7 +3170,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
// create the node
if(id)
{
ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
//ULOGGER_DEBUG("Creating %d (map=%d, pose=%s)", *iter, mapId, pose.prettyPrint().c_str());
Signature * s = new Signature(
id,
mapId,
@@ -3190,175 +3209,17 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
ULOGGER_DEBUG("Time=%fs", timer.ticks());
// Prepare the query... Get the map from signature and visual words
std::stringstream query2;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Feature "
"WHERE node_id = ? ";
UDEBUG("Loading local features (ids only=%s)....", loadWordIdsOnly?"true":"false");
if(loadWordIdsOnly) {
this->loadWordIdsQuery(nodes);
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
else {
this->loadWordsQuery(nodes);
}
else if(uStrNumCmp(_version, "0.11.2") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
query2 << " ORDER BY word_id"; // Needed for fast insertion below
query2 << ";";
rc = sqlite3_prepare_v2(_ppDb, query2.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
float nanFloat = std::numeric_limits<float>::quiet_NaN ();
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int visualWordId = 0;
int descriptorSize = 0;
const void * descriptor = 0;
int dRealSize = 0;
cv::KeyPoint kpt;
std::multimap<int, int> visualWords;
std::vector<cv::KeyPoint> visualWordsKpts;
std::vector<cv::Point3f> visualWords3;
cv::Mat descriptors;
bool allWords3NaN = true;
cv::Point3f depth(0,0,0);
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
visualWordId = sqlite3_column_int(ppStmt, index++);
kpt.pt.x = sqlite3_column_double(ppStmt, index++);
kpt.pt.y = sqlite3_column_double(ppStmt, index++);
kpt.size = sqlite3_column_int(ppStmt, index++);
kpt.angle = sqlite3_column_double(ppStmt, index++);
kpt.response = sqlite3_column_double(ppStmt, index++);
if(uStrNumCmp(_version, "0.12.0") >= 0)
{
kpt.octave = sqlite3_column_int(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.x = nanFloat;
++index;
}
else
{
depth.x = sqlite3_column_double(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.y = nanFloat;
++index;
}
else
{
depth.y = sqlite3_column_double(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.z = nanFloat;
++index;
}
else
{
depth.z = sqlite3_column_double(ppStmt, index++);
}
visualWordsKpts.push_back(kpt);
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
visualWords3.push_back(depth);
if(allWords3NaN && util3d::isFinite(depth))
{
allWords3NaN = false;
}
if(uStrNumCmp(_version, "0.11.2") >= 0)
{
descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
if(descriptor && descriptorSize>0 && dRealSize>0)
{
cv::Mat d;
if(dRealSize == descriptorSize)
{
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
memcpy(d.data, descriptor, dRealSize);
descriptors.push_back(d);
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(visualWords.size()==0)
{
UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
}
else
{
if(allWords3NaN)
{
visualWords3.clear();
}
(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
}
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
UDEBUG("Loading local features.... done! (in %f s)", timer.ticks());
this->loadLinksQuery(nodes);
ULOGGER_DEBUG("Time load links=%fs", timer.ticks());
ULOGGER_DEBUG("Time loading links=%fs", timer.ticks());
for(std::list<Signature*>::iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
{
@@ -3626,7 +3487,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
}
void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes, bool loadWordIdsOnly) const
{
ULOGGER_DEBUG("");
if(_ppDb)
@@ -3672,15 +3533,15 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Loading %d signatures...", ids.size());
this->loadSignaturesQuery(ids, nodes);
this->loadSignaturesQuery(ids, nodes, loadWordIdsOnly);
ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
}
}
void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) const
{
ULOGGER_DEBUG("");
if(_ppDb && dictionary)
if(_ppDb)
{
std::string type;
UTimer timer;
@@ -3743,11 +3604,11 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) c
memcpy(d.data, descriptor, dRealSize);
VisualWord * vw = new VisualWord(id, d);
vw->setSaved(true);
dictionary->addWord(vw);
dictionary.addWord(vw);
if(++count % 5000 == 0)
{
ULOGGER_DEBUG("Loaded %d words...", count);
//ULOGGER_DEBUG("Loaded %d words...", count);
}
rc = sqlite3_step(ppStmt); // next result...
}
@@ -3758,9 +3619,50 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) c
// Get Last word id
getLastWordId(id);
dictionary->setLastWordId(id);
dictionary.setLastWordId(id);
ULOGGER_DEBUG("Time=%fs", timer.ticks());
if(uStrNumCmp(_version, "0.23.0") >= 0) {
// load dictionary index
std::stringstream query3;
query3 << "SELECT dictionary_index "
<< "FROM Admin "
<< "WHERE version='" << _version.c_str()
<<"';";
rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query3.str().c_str()).c_str());
if(rc == SQLITE_ROW)
{
const void * data = 0;
int dataSize = 0;
int index = 0;
//opt_poses
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>4 && data)
{
UDEBUG("A flann index was saved in the database (size=%ld).", dataSize);
dictionary.deserializeIndex((const unsigned char*)data, dataSize);
}
else {
UDEBUG("No flann index was saved in the database.");
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
ULOGGER_DEBUG("Loaded %d words... time=%fs", count, timer.ticks());
}
}
@@ -3860,6 +3762,262 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
}
}
void DBDriverSqlite3::loadWordIdsQuery(std::list<Signature *> & signatures) const
{
if(_ppDb)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT word_id "
"FROM Feature "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
query << "SELECT word_id "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.11.2") >= 0)
{
query << "SELECT word_id "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else
{
query << "SELECT word_id "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
query << " ORDER BY word_id"; // Needed for fast insertion below
query << ";";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature*>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int visualWordId = 0;
std::multimap<int, int> visualWords;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
visualWordId = sqlite3_column_int(ppStmt, index++);
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, -1));
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(visualWords.size()==0)
{
UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
}
else
{
(*iter)->setWords(visualWords, std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
//ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
}
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
void DBDriverSqlite3::loadWordsQuery(std::list<Signature *> & signatures) const
{
if(_ppDb)
{
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Feature "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
query << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.11.2") >= 0)
{
query << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
else
{
query << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
"FROM Map_Node_Word "
"WHERE node_id = ? ";
}
query << " ORDER BY word_id"; // Needed for fast insertion below
query << ";";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
float nanFloat = std::numeric_limits<float>::quiet_NaN ();
for(std::list<Signature*>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
//ULOGGER_DEBUG("Loading words of %d...", (*iter)->id());
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int visualWordId = 0;
int descriptorSize = 0;
const void * descriptor = 0;
int dRealSize = 0;
cv::KeyPoint kpt;
std::multimap<int, int> visualWords;
std::vector<cv::KeyPoint> visualWordsKpts;
std::vector<cv::Point3f> visualWords3;
cv::Mat descriptors;
bool allWords3NaN = true;
cv::Point3f depth(0,0,0);
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
visualWordId = sqlite3_column_int(ppStmt, index++);
kpt.pt.x = sqlite3_column_double(ppStmt, index++);
kpt.pt.y = sqlite3_column_double(ppStmt, index++);
kpt.size = sqlite3_column_int(ppStmt, index++);
kpt.angle = sqlite3_column_double(ppStmt, index++);
kpt.response = sqlite3_column_double(ppStmt, index++);
if(uStrNumCmp(_version, "0.12.0") >= 0)
{
kpt.octave = sqlite3_column_int(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.x = nanFloat;
++index;
}
else
{
depth.x = sqlite3_column_double(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.y = nanFloat;
++index;
}
else
{
depth.y = sqlite3_column_double(ppStmt, index++);
}
if(sqlite3_column_type(ppStmt, index) == SQLITE_NULL)
{
depth.z = nanFloat;
++index;
}
else
{
depth.z = sqlite3_column_double(ppStmt, index++);
}
visualWordsKpts.push_back(kpt);
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
visualWords3.push_back(depth);
if(allWords3NaN && util3d::isFinite(depth))
{
allWords3NaN = false;
}
if(uStrNumCmp(_version, "0.11.2") >= 0)
{
descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
if(descriptor && descriptorSize>0 && dRealSize>0)
{
cv::Mat d;
if(dRealSize == descriptorSize)
{
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
memcpy(d.data, descriptor, dRealSize);
descriptors.push_back(d);
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(visualWords.size()==0)
{
UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
}
else
{
if(allWords3NaN)
{
visualWords3.clear();
}
(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
//ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
}
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
void DBDriverSqlite3::loadLinksQuery(
int signatureId,
std::multimap<int, Link> & links,
@@ -4174,7 +4332,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
//UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
}
// Finalize (delete) the statement
@@ -5117,8 +5275,8 @@ std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * la
Transform t(serializedPoses.at<float>(i*12), serializedPoses.at<float>(i*12+1), serializedPoses.at<float>(i*12+2), serializedPoses.at<float>(i*12+3),
serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
poses.insert(std::make_pair(serializedIds.at<int>(i), t));
UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
poses.insert(poses.end(), std::make_pair(serializedIds.at<int>(i), t));
//UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
}
}
@@ -5591,6 +5749,47 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
return cloud;
}
void DBDriverSqlite3::saveFlannIndexQuery(const std::vector<unsigned char> & data) const
{
UDEBUG("");
if(_ppDb && uStrNumCmp(_version, "0.23.0") >= 0)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Update table Admin
query = uFormat("UPDATE Admin SET dictionary_index=? WHERE version='%s';", _version.c_str());
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int index = 1;
if(data.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, data.data(), data.size(), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
std::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
@@ -6598,7 +6797,7 @@ void DBDriverSqlite3::stepLink(
{
UFATAL("");
}
UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
//UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
// Don't save virtual links
if(link.type()==Link::kVirtualClosure)
corelib/src/DBReader.cpp
+35 -15
View File
@@ -260,7 +260,7 @@ bool DBReader::init(
else
{
Signature * s = _dbDriver->loadSignature(*_ids.begin());
_dbDriver->loadNodeData(s);
_dbDriver->loadNodeData(*s);
if( s->sensorData().imageCompressed().empty() &&
s->getWords().empty() &&
!s->sensorData().laserScanCompressed().empty())
@@ -510,22 +510,41 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
}
else
{
// if localization data saved in database, covariance will be set in a prior link
_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
infMatrix = links.begin()->second.infMatrix();
_previousInfMatrix = infMatrix;
}
else
{
if(_previousInfMatrix.empty())
// In case the graph was reduced, look for forward neighbor link from previous id
bool covAdded = false;
if(_currentId != _ids.begin()) {
std::set<int>::iterator previousId = _currentId;
--previousId;
std::multimap<int, Link> previousLinks;
_dbDriver->loadLinks(*previousId, previousLinks, Link::kNeighbor);
if(previousLinks.size() && previousLinks.rbegin()->first == *_currentId)
{
_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
// assume the last is the forward neighbor pointing to current ID, take its covariance
infMatrix = previousLinks.rbegin()->second.infMatrix();
_previousInfMatrix = infMatrix;
covAdded = true;
}
}
if(!covAdded) {
// if localization data saved in database, covariance will be set in a prior link
_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
infMatrix = links.begin()->second.infMatrix();
_previousInfMatrix = infMatrix;
}
else
{
if(_previousInfMatrix.empty())
{
_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
}
// we have a node not linked to map, use last variance
UWARN("The node loaded (%d) doesn't have neighbor, re-using the covariance of the previous link for odometry.", s->id());
infMatrix = _previousInfMatrix;
}
// we have a node not linked to map, use last variance
infMatrix = _previousInfMatrix;
}
}
}
@@ -837,6 +856,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
if(info)
{
info->odomPose = pose;
UASSERT(!infMatrix.empty());
info->odomCovariance = infMatrix.inv();
info->odomVelocity = s->getVelocity();
UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
corelib/src/Features2d.cpp
+114 -4
View File
@@ -47,6 +47,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifdef RTABMAP_TORCH
#include "superpoint_torch/SuperPoint.h"
#endif
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
#include "superpoint_rpautrat/SuperpointRpautrat.h"
#endif
#ifdef RTABMAP_PYTHON
#include "python/PyDetector.h"
@@ -730,9 +733,14 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
feature2D = new ORBOctree(parameters);
break;
#ifdef RTABMAP_TORCH
case Feature2D::kFeatureSuperPointTorch:
feature2D = new SuperPointTorch(parameters);
break;
case Feature2D::kFeatureSuperPointTorch:
feature2D = new SuperPointTorch(parameters);
break;
#endif
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
case Feature2D::kFeatureSuperPointRpautrat:
feature2D = new SuperPointRpautrat(parameters);
break;
#endif
case Feature2D::kFeatureSurfFreak:
feature2D = new SURF_FREAK(parameters);
@@ -831,7 +839,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
std::vector<cv::KeyPoint> subKeypoints;
subKeypoints = this->generateKeypointsImpl(image, roi, mask);
if (this->getType() != Feature2D::Type::kFeaturePyDetector)
if (this->getType() != Feature2D::Type::kFeaturePyDetector && this->getType() != Feature2D::Type::kFeatureSuperPointRpautrat)
{
limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC());
}
@@ -2615,6 +2623,108 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
}
//////////////////////////
//SuperPointRpautrat
//////////////////////////
SuperPointRpautrat::SuperPointRpautrat(const ParametersMap & parameters) :
superpointWeightsPath_(Parameters::defaultSuperPointRpautratWeightsPath()),
superpointModelPath_(Parameters::defaultSuperPointRpautratModelPath()),
outputDir_(""),
threshold_(Parameters::defaultSuperPointRpautratThreshold()),
nms_(Parameters::defaultSuperPointRpautratNMS()),
minDistance_(Parameters::defaultSuperPointRpautratNMSRadius()),
cuda_(Parameters::defaultSuperPointRpautratCuda())
{
parseParameters(parameters);
}
SuperPointRpautrat::~SuperPointRpautrat()
{
}
void SuperPointRpautrat::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
std::string previousWeightsPath = superpointWeightsPath_;
std::string previousModelPath = superpointModelPath_;
bool previousCuda = cuda_;
float previousThreshold = threshold_;
bool previousNms = nms_;
int previousMinDistance = minDistance_;
Parameters::parse(parameters, Parameters::kSuperPointRpautratWeightsPath(), superpointWeightsPath_);
Parameters::parse(parameters, Parameters::kSuperPointRpautratModelPath(), superpointModelPath_);
Parameters::parse(parameters, Parameters::kSuperPointRpautratThreshold(), threshold_);
Parameters::parse(parameters, Parameters::kSuperPointRpautratNMS(), nms_);
Parameters::parse(parameters, Parameters::kSuperPointRpautratNMSRadius(), minDistance_);
Parameters::parse(parameters, Parameters::kSuperPointRpautratCuda(), cuda_);
Parameters::parse(parameters, Parameters::kRtabmapWorkingDirectory(), outputDir_);
// If working directory is not set, use the default
if(outputDir_.empty())
{
outputDir_ = Parameters::createDefaultWorkingDirectory();
}
// Reinitialize detector if model-affecting parameters changed
if(superPoint_.get() == 0 ||
superpointWeightsPath_.compare(previousWeightsPath) != 0 ||
superpointModelPath_.compare(previousModelPath) != 0 ||
previousCuda != cuda_ ||
previousThreshold != threshold_ ||
previousNms != nms_ ||
previousMinDistance != minDistance_)
{
superPoint_ = cv::Ptr<SPDetectorRpautrat>(new SPDetectorRpautrat(superpointWeightsPath_, superpointModelPath_, outputDir_, threshold_, nms_, minDistance_, cuda_, this->getMaxFeatures(), this->getSSC()));
}
else if(superPoint_.get() != 0)
{
// Update post-processing parameters without reinitializing
superPoint_->setMaxFeatures(this->getMaxFeatures());
superPoint_->setSSC(this->getSSC());
}
#else
UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
#endif
}
std::vector<cv::KeyPoint> SuperPointRpautrat::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
if(roi.x!=0 || roi.y !=0)
{
UERROR("SuperPoint Rpautrat: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
roi.x, roi.y, roi.width, roi.height,
Parameters::kKpRoiRatios().c_str(),
Parameters::kVisRoiRatios().c_str(),
Parameters::kVisGridRows().c_str(),
Parameters::kVisGridCols().c_str(),
Parameters::kKpGridRows().c_str(),
Parameters::kKpGridCols().c_str());
return std::vector<cv::KeyPoint>();
}
return superPoint_->detect(image, mask);
#else
UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
return std::vector<cv::KeyPoint>();
#endif
}
cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
return superPoint_->compute(keypoints);
#else
UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
return cv::Mat();
#endif
}
//////////////////////////
//GFTT-DAISY
//////////////////////////
corelib/src/FlannIndex.cpp
+330 -119
View File
@@ -27,8 +27,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/FlannIndex.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Version.h>
#ifdef WIN32
#include <rtabmap/core/Parameters.h>
#endif
#include "rtflann/flann.hpp"
#include <boost/crc.hpp>
namespace rtabmap {
@@ -37,7 +45,6 @@ FlannIndex::FlannIndex():
nextIndex_(0),
featuresType_(0),
featuresDim_(0),
isLSH_(false),
useDistanceL1_(false),
rebalancingFactor_(2.0f)
{
@@ -49,9 +56,9 @@ FlannIndex::~FlannIndex()
void FlannIndex::release()
{
UDEBUG("");
if(index_)
{
UDEBUG("Clearing flann index...");
if(featuresType_ == CV_8UC1)
{
delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
@@ -72,12 +79,139 @@ void FlannIndex::release()
}
}
index_ = 0;
UDEBUG("Clearing flann index... done!");
}
nextIndex_ = 0;
isLSH_ = false;
addedDescriptors_.clear();
removedIndexes_.clear();
UDEBUG("");
}
#define FLANN_INDEX_HEADER_SIZE 12
std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) const {
if(index_ && !addedDescriptors_.empty())
{
#ifdef WIN32
UERROR("FLANN index serialization is not yet implemented on Windows. Parameter \"%s\" cannot be used.", Parameters::kKpFlannIndexSaved().c_str());
#else
UTimer timer;
const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
std::vector<unsigned char> indexData(1024*1024*100 + headerSizeBytes); // Max 100 MB
FILE* indexDataPtr = fmemopen(indexData.data()+headerSizeBytes, indexData.size() - headerSizeBytes, "wb");
long bytes_written = 0;
if (indexDataPtr) {
if(featuresType_ == CV_8UC1)
{
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->save(indexDataPtr);
}
else
{
if(useDistanceL1_)
{
((rtflann::Index<rtflann::L1<float> >*)index_)->save(indexDataPtr);;
}
else if(featuresDim_ <= 3)
{
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->save(indexDataPtr);;
}
else
{
((rtflann::Index<rtflann::L2<float> >*)index_)->save(indexDataPtr);;
}
}
bytes_written = ftell(indexDataPtr);
fclose(indexDataPtr);
}
if(bytes_written < long(indexData.size()-headerSizeBytes))
{
//Expected data size and type
int dataRows = 0;
int dataCols = 0;
int dataType = -1;
cv::Mat dataset;
std::set<int> removedDescriptors;
if(computeChecksum){
removedDescriptors.insert(removedIndexes_.begin(), removedIndexes_.end());
}
for(const auto & iter: addedDescriptors_)
{
UASSERT(!iter.second.empty());
dataRows += iter.second.rows;
if(dataCols <= 0) {
dataCols = iter.second.cols;
}
else {
UASSERT(dataCols == iter.second.cols);
}
if(dataType < 0) {
dataType = iter.second.type();
}
else {
UASSERT(dataType == iter.second.type());
}
if(computeChecksum){
if(removedDescriptors.find(iter.first) == removedDescriptors.end()) {
if(dataset.empty()) {
dataset = iter.second.clone();
}
else {
dataset.push_back(iter.second);
}
}
else {
dataRows -= iter.second.rows;
}
}
}
if(!computeChecksum) {
for(const auto & index: removedIndexes_)
{
dataRows -= addedDescriptors_.at(index).rows;
}
}
unsigned int crcValue = 0;
if(computeChecksum) {
boost::crc_32_type result;
result.process_bytes(dataset.data, dataset.total()*dataset.elemSize());
crcValue = result.checksum();
}
indexData.resize(bytes_written+headerSizeBytes);
indexData.shrink_to_fit();
int rebalancingFactorAsInt;
memcpy(&rebalancingFactorAsInt, &rebalancingFactor_, sizeof(rebalancingFactor_));
int crcValueAsInt;
memcpy(&crcValueAsInt, &crcValue, sizeof(crcValue));
int header[FLANN_INDEX_HEADER_SIZE] = {
RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
algorithm_, // 3,
featuresDim_, // 4,
useDistanceL1_?1:0, // 5,
rebalancingFactorAsInt, // 6,
dataRows, // 7,
dataCols, // 8,
dataType, // 9,
crcValueAsInt, // 10
(int)bytes_written}; // 11
UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d",
header[0],header[1],header[2],
header[3],
header[4],
header[5],
rebalancingFactor_,
header[7], header[8], header[9], crcValueAsInt,
header[11]);
memcpy(indexData.data(), header, headerSizeBytes);
return indexData;
}
else {
UERROR("Target buffer too small to serialize index, aborting.");
}
UDEBUG("Flann serialization: %fs", timer.ticks());
#endif
}
return std::vector<unsigned char>();
}
size_t FlannIndex::indexedFeatures() const
@@ -139,12 +273,13 @@ size_t FlannIndex::memoryUsed() const
return memoryUsage;
}
void FlannIndex::buildLinearIndex(
void FlannIndex::buildIndex(
flann_algorithm_t algorithm,
const cv::Mat & features,
bool useDistanceL1,
float rebalancingFactor)
{
UDEBUG("");
UDEBUG("algorithm=%d", (int)algorithm);
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -152,8 +287,29 @@ void FlannIndex::buildLinearIndex(
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
algorithm_ = algorithm;
rtflann::LinearIndexParams params;
rtflann::IndexParams params;
switch (algorithm)
{
case FLANN_INDEX_LINEAR:
params = rtflann::LinearIndexParams();
break;
case FLANN_INDEX_KDTREE:
params = rtflann::KDTreeIndexParams(4);
break;
case FLANN_INDEX_KDTREE_SINGLE:
params = rtflann::KDTreeSingleIndexParams(10, true);
break;
case FLANN_INDEX_LSH:
UASSERT(features.type() == CV_8UC1);
params = rtflann::LshIndexParams(12, 20, 2);
break;
default:
UFATAL("The flann algorithm type %d is not supported!", (int)algorithm);
break;
}
if(featuresType_ == CV_8UC1)
{
@@ -199,13 +355,140 @@ void FlannIndex::buildLinearIndex(
UDEBUG("");
}
void FlannIndex::buildKDTreeIndex(
const cv::Mat & features,
int trees,
bool useDistanceL1,
float rebalancingFactor)
bool FlannIndex::loadIndex(
const std::vector<unsigned char> & indexData,
flann_algorithm_t algorithm,
const cv::Mat & features,
bool useDistanceL1,
float rebalancingFactor,
std::string * error)
{
UDEBUG("");
return loadIndex(
indexData.data(),
indexData.size(),
algorithm,
features,
useDistanceL1,
rebalancingFactor),
error;
}
bool FlannIndex::loadIndex(
const unsigned char * indexData,
size_t indexDataSize,
flann_algorithm_t algorithm,
const cv::Mat & features,
bool useDistanceL1,
float rebalancingFactor,
std::string * error)
{
UASSERT(indexData!=NULL);
if(indexDataSize == 0) {
UWARN("Trying to load empty index....");
return false;
}
#ifdef WIN32
UERROR("FLANN index deserialization is not yet implemented on Windows. Index cannot be loaded from memory buffer.");
return false;
#else
// Check if the features match the expected data from the index
size_t headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
if(indexDataSize < headerSizeBytes) {
if(error) {
*error = uFormat("Wrong header size detected (%ld vs expected %ld).", indexDataSize, headerSizeBytes);
}
return false;
}
const int * header = (const int *)indexData;
int savedAlgorithm = header[3];
int savedDim = header[4];
bool savedDistanceL1 = header[5]==1;
float savedRebalancingFactor;
memcpy(&savedRebalancingFactor, &header[6], sizeof(header[6]));
int savedRows = header[7];
int savedCols = header[8];
int savedType = header[9];
unsigned int savedCrc;
memcpy(&savedCrc, &header[10], sizeof(header[10]));
int savedIndexSize = header[11];
UDEBUG("Header: \"%d.%d.%d\" alg=%d dim=%d L1=%d factor=%f data(%dx%d type=%d, crc=%X) %d",
header[0],header[1],header[2],
header[3],
header[4],
header[5],
savedRebalancingFactor,
header[7], header[8], header[9], savedCrc,
header[11]);
if(savedAlgorithm != algorithm) {
if(error) {
*error = uFormat("Serialized flann algorithm (%d) doesn't match the expected one (%d).", savedAlgorithm, algorithm);
}
return false;
}
if(savedDim != features.cols) {
if(error) {
*error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedDim, features.cols);
}
return false;
}
if(savedDistanceL1 != useDistanceL1) {
if(error) {
*error = uFormat("Serialized \"use distance L1\" (%s) doesn't match the expected one (%s).", savedDistanceL1?"true":"false", useDistanceL1?"true":"false");
}
return false;
}
if(savedRebalancingFactor != rebalancingFactor) {
if(error) {
*error = uFormat("Serialized \"rebalancing factor\" (%f) doesn't match the expected one (%f).", savedRebalancingFactor, rebalancingFactor);
}
return false;
}
if(savedRows != features.rows) {
if(error) {
*error = uFormat("Serialized feature count (%d) doesn't match the expected one (%d).", savedRows, features.rows);
}
return false;
}
if(savedCols != features.cols) {
if(error) {
*error = uFormat("Serialized feature dimension (%d) doesn't match the expected one (%d).", savedCols, features.cols);
}
return false;
}
if(savedType != features.type()) {
if(error) {
*error = uFormat("Serialized feature type (%d) doesn't match the expected one (%d).", savedType, features.type());
}
return false;
}
if(savedCrc != 0) {
// Compute checksum and compare
boost::crc_32_type result;
result.process_bytes(features.data, features.total()*features.elemSize());
if(savedCrc != result.checksum()) {
if(error) {
*error = uFormat("Serialized feature crc (%X) doesn't match the expected one (%X).", savedCrc, result.checksum());
}
return false;
}
}
if(savedIndexSize != int(indexDataSize - headerSizeBytes)) {
if(error) {
*error = uFormat("Serialized flann index size (%ld) doesn't match the expected one (%ld).", savedIndexSize, indexDataSize - headerSizeBytes);
}
return false;
}
if(savedIndexSize == 0) {
if(error) {
*error = "Serialized flann index is empty.";
}
return false;
}
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -213,14 +496,39 @@ void FlannIndex::buildKDTreeIndex(
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
algorithm_ = algorithm;
rtflann::KDTreeIndexParams params(trees);
UDEBUG("algorithm=%d", (int)algorithm);
rtflann::IndexParams params;
switch (algorithm)
{
case FLANN_INDEX_LINEAR:
params = rtflann::LinearIndexParams();
break;
case FLANN_INDEX_KDTREE:
params = rtflann::KDTreeIndexParams(4);
break;
case FLANN_INDEX_KDTREE_SINGLE:
params = rtflann::KDTreeSingleIndexParams(10, true);
break;
case FLANN_INDEX_LSH:
UASSERT(features.type() == CV_8UC1);
params = rtflann::LshIndexParams(12, 20, 2);
break;
default:
UFATAL("The flann algorithm type %d is not supported!", (int)algorithm);
break;
}
FILE* indexDataPtr = fmemopen((void*)(indexData+headerSizeBytes), indexDataSize - headerSizeBytes, "r");
if(featuresType_ == CV_8UC1)
{
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->load_saved_index(indexDataPtr);
}
else
{
@@ -228,22 +536,24 @@ void FlannIndex::buildKDTreeIndex(
if(useDistanceL1_)
{
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
((rtflann::Index<rtflann::L1<float> >*)index_)->load_saved_index(indexDataPtr);
}
else if(featuresDim_ <=3)
{
index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->load_saved_index(indexDataPtr);
}
else
{
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
((rtflann::Index<rtflann::L2<float> >*)index_)->load_saved_index(indexDataPtr);
}
}
fclose(indexDataPtr);
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
@@ -257,107 +567,8 @@ void FlannIndex::buildKDTreeIndex(
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
}
void FlannIndex::buildKDTreeSingleIndex(
const cv::Mat & features,
int leafMaxSize,
bool reorder,
bool useDistanceL1,
float rebalancingFactor)
{
UDEBUG("");
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
if(featuresType_ == CV_8UC1)
{
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
}
else
{
rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
if(useDistanceL1_)
{
index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
}
else if(featuresDim_ <=3)
{
index_ = new rtflann::Index<rtflann::L2_Simple<float> >(dataset, params);
((rtflann::Index<rtflann::L2_Simple<float> >*)index_)->buildIndex();
}
else
{
index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
}
}
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
{
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
}
else
{
// tree won't ever be rebalanced, so just keep only one header for the data
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
}
void FlannIndex::buildLSHIndex(
const cv::Mat & features,
unsigned int table_number,
unsigned int key_size,
unsigned int multi_probe_level,
float rebalancingFactor)
{
UDEBUG("");
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_8UC1);
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = true;
rebalancingFactor_ = rebalancingFactor;
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
{
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
}
else
{
// tree won't ever be rebalanced, so just keep only one header for the data
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
return true;
#endif
}
bool FlannIndex::isBuilt()
corelib/src/GlobalMap.cpp
+24 -8
View File
@@ -99,15 +99,12 @@ unsigned long GlobalMap::getMemoryUsed() const
return memoryUsage;
}
bool GlobalMap::update(const std::map<int, Transform> & poses)
bool GlobalMap::fullUpdateNeeded(const std::map<int, Transform> & poses) const
{
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
float updateErrorSqrd = updateError_*updateError_;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
for(std::map<int, Transform>::const_iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
if(jter != poses.end())
@@ -125,7 +122,15 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
}
}
if(graphOptimized || graphChanged)
return graphOptimized || graphChanged;
}
bool GlobalMap::update(const std::map<int, Transform> & poses)
{
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
if(fullUpdateNeeded(poses))
{
// clear all but keep cache
clear();
@@ -134,14 +139,25 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
std::list<std::pair<int, Transform> > orderedPoses;
// add old poses that were not in the current map (they were just retrieved from LTM)
int nodesNotAssembled = 0;
int nodesNotInCache = 0;
for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
{
if(!isNodeAssembled(iter->first))
{
UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
orderedPoses.push_back(*iter);
if(uContains(cache(), iter->first))
{
++nodesNotAssembled;
//UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
orderedPoses.push_back(*iter);
}
else
{
++nodesNotInCache;
}
}
}
UDEBUG("%d nodes will be assembled in the map and %d nodes won't (no local grids in cache for them)", nodesNotAssembled, nodesNotInCache);
// insert zero after
if(poses.find(0) != poses.end())
corelib/src/Graph.cpp
+21 -4
View File
@@ -196,7 +196,7 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints, // optional for formats 3 and 4
std::map<int, double> * stamps) // optional for format 1 and 9
@@ -440,7 +440,7 @@ bool importPoses(
UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
}
}
else if(format == 1 || format==10 || format==11) // rgbd-slam format
else if(format == 1 || format==10 || format==11 || format==12) // rgbd-slam format
{
std::list<std::string> strList = uSplit(str);
if((strList.size() >= 8 && format!=11) || (strList.size() == 9 && format==11))
@@ -451,9 +451,12 @@ bool importPoses(
}
double stamp = uStr2Double(strList.front());
strList.pop_front();
if(format==11)
if(strList.size() == 8 && (format==10 || format==11 || format==12))
{
id = uStr2Int(strList.back());
if(format==11)
{
id = uStr2Int(strList.back());
}
strList.pop_back();
}
str = uJoin(strList, " ");
@@ -481,6 +484,20 @@ bool importPoses(
1, 0, 0, 0);
pose = t*pose;
}
else if(format == 12)
{
// See https://www.ipb.uni-bonn.de/data/rgbd-dynamic-dataset/index.html
Transform T_ros(-1, 0, 0, 0,
0, 0, 1, 0,
0, 1, 0, 0);
Transform T_m(
1.0157, 0.1828, -0.2389, 0.0113,
0.0009, -0.8431, -0.6413, -0.00980,
-0.3009, 0.6147, -0.8085, 0.0111);
// we remove the optical rotation
pose = T_ros*pose*T_ros*T_m*CameraModel::opticalRotation().inverse();
}
poses.insert(std::make_pair(id, pose));
}
}
corelib/src/LocalGrid.cpp
+2 -2
View File
@@ -64,8 +64,8 @@ void LocalGridCache::add(int nodeId,
void LocalGridCache::add(int nodeId, const LocalGrid & localGrid)
{
UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
nodeId, localGrid.groundCells.cols, localGrid.groundCells.channels(), localGrid.obstacleCells.cols, localGrid.obstacleCells.channels(), localGrid.emptyCells.cols, localGrid.emptyCells.channels());
//UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
// nodeId, localGrid.groundCells.cols, localGrid.groundCells.channels(), localGrid.obstacleCells.cols, localGrid.obstacleCells.channels(), localGrid.emptyCells.cols, localGrid.emptyCells.channels());
if(nodeId < 0)
{
UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
corelib/src/Memory.cpp
+169 -56
View File
@@ -76,6 +76,7 @@ Memory::Memory(const ParametersMap & parameters) :
_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
_binDataKept(Parameters::defaultMemBinDataKept()),
_rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
_loadVisualLocalFeaturesOnInit(Parameters::defaultMemLoadVisualLocalFeaturesOnInit()),
_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
@@ -83,6 +84,7 @@ Memory::Memory(const ParametersMap & parameters) :
_depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()),
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
_flannIndexSaved(Parameters::defaultKpFlannIndexSaved()),
_reduceGraph(Parameters::defaultMemReduceGraph()),
_maxStMemSize(Parameters::defaultMemSTMSize()),
_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
@@ -129,6 +131,7 @@ Memory::Memory(const ParametersMap & parameters) :
_linksChanged(false),
_signaturesAdded(0),
_allNodesInWM(true),
_receivingOdometryFeatures(false),
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
@@ -245,13 +248,13 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
std::set<int> ids;
_dbDriver->getAllNodeIds(ids, true);
_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures, 0, !_loadVisualLocalFeaturesOnInit);
}
else
{
// load previous session working memory
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
_dbDriver->loadLastNodes(dbSignatures);
_dbDriver->loadLastNodes(dbSignatures, !_loadVisualLocalFeaturesOnInit);
}
for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
{
@@ -417,20 +420,22 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
}
else
{
_dbDriver->load(_vwd, false);
_dbDriver->load(*_vwd, false);
}
}
else
{
UDEBUG("load words");
// load the last dictionary
_dbDriver->load(_vwd, _vwd->isIncremental());
_dbDriver->load(*_vwd, _vwd->isIncremental());
}
UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
_vwd->update();
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
UDEBUG("Adding word references...");
UTimer timer;
// Enable loaded signatures
const std::map<int, Signature *> & signatures = this->getSignatures();
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
@@ -441,7 +446,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
const std::multimap<int, int> & words = s->getWords();
if(words.size())
{
UDEBUG("node=%d, word references=%d", s->id(), words.size());
//UDEBUG("node=%d, word references=%d", s->id(), words.size());
for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
{
if(iter->first > 0)
@@ -458,7 +463,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
{
UWARN("_vwd->getUnusedWordsSize() must be empty... size=%d", _vwd->getUnusedWordsSize());
}
UDEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
UDEBUG("Total word references added = %d (in %f s)", _vwd->getTotalActiveReferences(), timer.ticks());
if(_lastSignature == 0)
{
@@ -482,6 +487,37 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
UDEBUG("map ids start with %d", _idMapCount);
}
void Memory::saveFlannIndex(bool postInitClosingEvents)
{
if(!_dbDriver) {
return;
}
if(uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.0") >= 0) {
if(_flannIndexSaved && !_incrementalMemory) {
if(_vwd->isModified()) {
UINFO("Saving flann index to database... (%s=true)", Parameters::kKpFlannIndexSaved().c_str());
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving flann index to database..."));
_dbDriver->saveFlannIndex(_vwd->serializeIndex());
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving flann index to database, done!"));
}
else
{
UDEBUG("The dictionary didn't change since loaded, do not need to save again to database.");
}
}
else {
// clear if exists
_dbDriver->saveFlannIndex(std::vector<unsigned char>());
}
}
else if(_flannIndexSaved)
{
UWARN("Parameter %s is enabled, but database version is too old (%s < 0.23). Flann index cannot be saved.",
Parameters::kKpFlannIndexSaved().c_str(),
_dbDriver->getDatabaseVersion().c_str());
}
}
void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath)
{
UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
@@ -493,6 +529,8 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
databaseNameChanged = ouputDatabasePath.size() && _dbDriver->getUrl().size() && _dbDriver->getUrl().compare(ouputDatabasePath) != 0?true:false;
}
UDEBUG("_memoryChanged=%d _linksChanged=%d databaseNameChanged=%d", _memoryChanged?1:0, _linksChanged?1:0, databaseNameChanged?1:0);
if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged))
{
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
@@ -500,6 +538,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
UINFO("No changes added to database.");
if(_dbDriver)
{
saveFlannIndex(postInitClosingEvents);
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
_dbDriver->closeConnection(false, ouputDatabasePath);
delete _dbDriver;
@@ -514,11 +553,15 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
{
UINFO("Saving memory...");
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
if(!_memoryChanged && _linksChanged && _dbDriver)
if(!_memoryChanged && _dbDriver)
{
// don't update the time stamps!
UDEBUG("");
_dbDriver->setTimestampUpdateEnabled(false);
saveFlannIndex(postInitClosingEvents);
if(_linksChanged) {
// don't update the time stamps!
UDEBUG("");
_dbDriver->setTimestampUpdateEnabled(false);
}
}
this->clear();
if(_dbDriver)
@@ -565,6 +608,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(params, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
Parameters::parse(params, Parameters::kMemLoadVisualLocalFeaturesOnInit(), _loadVisualLocalFeaturesOnInit);
Parameters::parse(params, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(params, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
@@ -618,6 +662,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance);
Parameters::parse(params, Parameters::kMarkerVarianceOrientationIgnored(), _markerOrientationIgnored);
Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved);
Parameters::parse(params, Parameters::kKpFlannIndexSaved(), _flannIndexSaved);
if(_markerAngVariance>=9999)
{
@@ -654,10 +699,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
// Keypoint stuff
if(_vwd)
{
_vwd->parseParameters(params);
}
_vwd->parseParameters(params);
Parameters::parse(params, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(params, Parameters::kKpParallelized(), _parallelized);
@@ -856,7 +898,7 @@ void Memory::preUpdate()
{
this->cleanUnusedWords();
}
if(_vwd && !_parallelized)
if(!_parallelized)
{
//When parallelized, it is done in CreateSignature
_vwd->update();
@@ -1114,10 +1156,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
}
++_signaturesAdded;
if(_vwd)
{
UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
}
UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
if(signature->getWords().size())
{
signature->setEnabled(true);
@@ -1224,16 +1263,17 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
std::multimap<int, Link> linksCopy = links;
for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
if(iter->second.type() == Link::kNeighborMerged)
{
// Removing only merged neighbor links, we keep original neighbor
// links to be able to reprocess databases with correct odometry covariance.
s->removeLink(iter->first);
if(iter->second.type() == Link::kNeighbor)
}
if(iter->second.type() == Link::kNeighbor)
{
if(_lastGlobalLoopClosureId == s->id())
{
if(_lastGlobalLoopClosureId == s->id())
{
_lastGlobalLoopClosureId = iter->first;
}
_lastGlobalLoopClosureId = iter->first;
}
}
}
@@ -1836,13 +1876,24 @@ void Memory::clear()
UDEBUG("");
//Get the tree root (parents)
std::map<int, Signature*> mem = _signatures;
for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
{
if(i->second)
if(!_dbDriver) {
// We are not saving to database anyway, just delete now.
for(std::map<int, Signature *>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
{
UDEBUG("deleting from the working and the short-term memory: %d", i->first);
this->moveToTrash(i->second);
delete iter->second;
}
_workingMem.clear();
_signatures.clear();
}
else {
std::map<int, Signature*> mem = _signatures;
for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
{
if(i->second)
{
//UDEBUG("deleting from the working and the short-term memory: %d", i->first);
this->moveToTrash(i->second);
}
}
}
@@ -1866,6 +1917,7 @@ void Memory::clear()
UDEBUG("");
_lastSignature = 0;
_lastGlobalLoopClosureId = 0;
_signaturesAdded = 0;
_idCount = kIdStart;
_idMapCount = kIdStart;
_memoryChanged = false;
@@ -1879,6 +1931,7 @@ void Memory::clear()
_landmarksIndex.clear();
_landmarksSize.clear();
_allNodesInWM = true;
_receivingOdometryFeatures = false;
if(_dbDriver)
{
@@ -1886,14 +1939,7 @@ void Memory::clear()
cleanUnusedWords();
_dbDriver->emptyTrashes();
}
else
{
cleanUnusedWords();
}
if(_vwd)
{
_vwd->clear();
}
_vwd->clear(_dbDriver!=NULL);
UDEBUG("");
}
@@ -2428,7 +2474,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
*/
void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> * deletedWords)
{
UDEBUG("id=%d", s?s->id():0);
//UDEBUG("id=%d", s?s->id():0);
if(s)
{
// Cleanup landmark indexes
@@ -2921,6 +2967,37 @@ Transform Memory::computeTransform(
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
// Load word descriptors and keypoints on-demand if necessary
if( !_reextractLoopClosureFeatures &&
(_registrationPipeline->isImageRequired() || guess.isNull()) &&
!fromS.getWords().empty() && fromS.getWordsKpts().empty() &&
_dbDriver)
{
// We assume "toS" has already features in RAM, so just lookup "fromS"
UDEBUG("Loading local visual features for signature %d", fromS.id());
std::multimap<int, int> words;
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points;
cv::Mat descriptors;
UTimer timer;
_dbDriver->getLocalFeatures(fromS.id(), words, keypoints, points, descriptors);
if(!words.empty() && !keypoints.empty()) {
UASSERT(words.size() == fromS.getWords().size());
std::map<int, int> wordsChanged = fromS.getWordsChanged();
bool wasEnabled = fromS.isEnabled();
fromS.setWords(words, keypoints, points, descriptors);
for(const auto & iter: wordsChanged) {
fromS.changeWordsRef(iter.first, iter.second);
}
fromS.setEnabled(wasEnabled);
UDEBUG("Loaded %ld local visual features for signature %d! (in %f s)", words.size(), fromS.id(), timer.ticks());
}
else
{
UDEBUG("Failed to load local visual features for signature %d.", fromS.id());
}
}
// compute transform fromId -> toId
std::vector<int> inliersV;
@@ -2980,8 +3057,10 @@ Transform Memory::computeTransform(
!_invertedReg &&
!tmpTo.getWordsDescriptors().empty() &&
!tmpTo.getWords().empty() &&
!tmpTo.getWordsKpts().empty() &&
!tmpFrom.getWordsDescriptors().empty() &&
!tmpFrom.getWords().empty() &&
!tmpFrom.getWordsKpts().empty() &&
!tmpFrom.getWords3().empty() &&
fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle
{
@@ -3015,8 +3094,12 @@ Transform Memory::computeTransform(
if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map
{
const Signature * s = this->getSignature(id);
if(s && !s->getWords3().empty())
if(s)
{
if(s->getWordsKpts().empty() && s->getWords3().empty() && s->getWordsDescriptors().empty()) {
UDEBUG("Signature %d doesn't have features set. Cannot be added in the local feature map.", s->id());
continue;
}
const std::map<int, int> & wordsTo = uMultimapToMapUnique(s->getWords());
for(std::map<int, int>::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter)
{
@@ -3115,6 +3198,11 @@ Transform Memory::computeTransform(
bundlePoses.insert(std::make_pair(id, iter->second.transform()));
}
if(s->getWordsKpts().empty())
{
UDEBUG("Signature %d doesn't have features set. Keypoints won't be added in local bundle adjustment.", s->id());
continue;
}
const std::map<int,int> & words = uMultimapToMapUnique(s->getWords());
for(std::map<int, int>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
{
@@ -3590,7 +3678,7 @@ void Memory::removeAllVirtualLinks()
void Memory::removeVirtualLinks(int signatureId)
{
UDEBUG("");
//UDEBUG("");
Signature * s = this->_getSignature(signatureId);
if(s)
{
@@ -3629,10 +3717,7 @@ void Memory::dumpMemory(std::string directory) const
void Memory::dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const
{
if(_vwd)
{
_vwd->exportDictionary(fileNameRef, fileNameDesc);
}
_vwd->exportDictionary(fileNameRef, fileNameDesc);
}
void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
@@ -3753,10 +3838,7 @@ unsigned long Memory::getMemoryUsed() const
{
memoryUsage += iter->second->getMemoryUsed(true);
}
if(_vwd)
{
memoryUsage += _vwd->getMemoryUsed();
}
memoryUsage += _vwd->getMemoryUsed();
memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::set<int>::iterator)) + sizeof(std::set<int>);
memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::map<int, double>::iterator)) + sizeof(std::map<int, double>);
memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
@@ -4194,6 +4276,29 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
words3 = s->getWords3();
wordsDescriptors = s->getWordsDescriptors();
globalDescriptors = s->sensorData().globalDescriptors();
if(!words.empty() && wordsKpts.empty() && _dbDriver)
{
std::multimap<int, int> tmpWords;
_dbDriver->getLocalFeatures(nodeId, tmpWords, wordsKpts, words3, wordsDescriptors);
if(!tmpWords.empty() && !wordsKpts.empty())
{
UASSERT(tmpWords.size() == words.size());
std::map<int, int> wordsChanged = s->getWordsChanged();
for(const auto & iter: wordsChanged) {
std::list<int> subwords = uValues(tmpWords, iter.first); // old id
if(subwords.size())
{
tmpWords.erase(iter.first);
for(std::list<int>::const_iterator jter=subwords.begin(); jter!=subwords.end(); ++jter)
{
tmpWords.insert(std::pair<int, int>(iter.second, (*jter))); // new id
}
}
}
words = tmpWords;
}
}
}
else if(_dbDriver)
{
@@ -4765,6 +4870,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
}
else if(_useOdometryFeatures) {
// To not detect first image as bad signature if odometry
// is using less features than feature2D->getMaxFeatures()
meanWordsPerLocation = 0;
}
if(_parallelized && !isIntermediateNode)
{
@@ -4868,10 +4978,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
SensorData decimatedData;
UDEBUG("Received kpts=%d kpts3D=%d, descriptors=%d _useOdometryFeatures=%s",
(int)data.keypoints().size(), (int)data.keypoints3D().size(), data.descriptors().rows, _useOdometryFeatures?"true":"false");
// TODO: do we still need the third and fouth comparisons?
// TODO: there is significant repetitive code between the if and the else, could we combine them?!
if(!_useOdometryFeatures ||
data.keypoints().empty() ||
(!_receivingOdometryFeatures && data.keypoints().empty()) ||
(int)data.keypoints().size() != data.descriptors().rows ||
(_feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty()))
(!_receivingOdometryFeatures && _feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty()))
{
if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
{
@@ -5208,6 +5320,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
{
_receivingOdometryFeatures = true;
UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
(int)data.keypoints().size(),
(int)data.keypoints3D().size(),
@@ -6330,7 +6443,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
void Memory::disableWordsRef(int signatureId)
{
UDEBUG("id=%d", signatureId);
//UDEBUG("id=%d", signatureId);
Signature * ss = this->_getSignature(signatureId);
if(ss && ss->isEnabled())
@@ -6346,7 +6459,7 @@ void Memory::disableWordsRef(int signatureId)
count -= _vwd->getTotalActiveReferences();
ss->setEnabled(false);
UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
//UDEBUG("%d words total ref removed from signature %d... (total active ref = %d)", count, ss->id(), _vwd->getTotalActiveReferences());
}
}
@@ -6409,7 +6522,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
// the words were deleted, so try to math it with an active word
// the words were deleted, so try to match it with an active word
std::list<VisualWord *> vws;
if(oldWordIds.size() && _dbDriver)
{
corelib/src/Odometry.cpp
+24 -7
View File
@@ -36,9 +36,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/odometry/OdometryLOAM.h"
#include "rtabmap/core/odometry/OdometryFLOAM.h"
#include "rtabmap/core/odometry/OdometryMSCKF.h"
#include "rtabmap/core/odometry/OdometryVINS.h"
#include "rtabmap/core/odometry/OdometryVINSFusion.h"
#include "rtabmap/core/odometry/OdometryOpenVINS.h"
#include "rtabmap/core/odometry/OdometryOpen3D.h"
#include "rtabmap/core/odometry/OdometryCuVSLAM.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_mapping.h"
@@ -103,8 +104,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
case Odometry::kTypeMSCKF:
odometry = new OdometryMSCKF(parameters);
break;
case Odometry::kTypeVINS:
odometry = new OdometryVINS(parameters);
case Odometry::kTypeVINSFusion:
odometry = new OdometryVINSFusion(parameters);
break;
case Odometry::kTypeOpenVINS:
odometry = new OdometryOpenVINS(parameters);
@@ -112,6 +113,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
case Odometry::kTypeOpen3D:
odometry = new OdometryOpen3D(parameters);
break;
case Odometry::kTypeCuVSLAM:
odometry = new OdometryCuVSLAM(parameters);
break;
default:
UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
odometry = new OdometryF2M(parameters);
@@ -625,6 +629,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
if(!guessIn.isNull())
{
guess = guessIn;
UDEBUG("Using provided guess %s", guessIn.prettyPrint().c_str());
}
else if(!imus_.empty())
{
@@ -641,12 +646,16 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
guess = guess.to3DoF();
}
UDEBUG("Adjusting guess from motion with IMU %s", guess.prettyPrint().c_str());
}
else if(!imuLastTransform_.isNull())
{
UWARN("Could not find imu transform at %f", data.stamp());
}
}
else if(!guess.isNull()) {
UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
}
UTimer time;
@@ -1011,21 +1020,28 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
--_resetCurrentCount;
if(_resetCurrentCount == 0)
{
UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose);
if(!guess.isNull() && !guessIn.isNull()) {
UWARN("Odometry automatically reset to latest pose (%s) + guess (%s)!", _pose.prettyPrint().c_str(), guess.prettyPrint().c_str());
this->reset(_pose * guess);
}
else {
UWARN("Odometry automatically reset to latest pose (%s)!", _pose.prettyPrint().c_str());
this->reset(_pose);
}
_resetCurrentCount = _resetCountdown;
if(info)
{
*info = OdometryInfo();
}
return this->computeTransform(data, Transform(), info);
this->computeTransform(data, Transform(), info);
return _pose;
}
}
previousVelocities_.clear();
velocityGuess_.setNull();
previousStamp_ = 0;
}
return Transform();
}
@@ -1055,6 +1071,7 @@ void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float v
0, 0, 0, 0, 0, 0.17 } };
static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
{ { 0.05, 0, 0, 0, 0, 0,
}
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.09, 0, 0,
corelib/src/OdometryInfo.cpp
+4 -4
View File
@@ -126,10 +126,10 @@ std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", reg.icpStructuralComplexity));
stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", reg.icpStructuralDistribution));
stats.insert(std::make_pair("Odometry/ICPCorrespondences/", reg.icpCorrespondences));
stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)reg.covariance.at<double>(0,0))));
stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)reg.covariance.at<double>(5,5))));
stats.insert(std::make_pair("Odometry/VarianceLin/", (float)reg.covariance.at<double>(0,0)));
stats.insert(std::make_pair("Odometry/VarianceAng/", (float)reg.covariance.at<double>(5,5)));
stats.insert(std::make_pair("Odometry/StdDevLin/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at<double>(0,0))));
stats.insert(std::make_pair("Odometry/StdDevAng/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at<double>(5,5))));
stats.insert(std::make_pair("Odometry/VarianceLin/", reg.covariance.empty()?0:(float)reg.covariance.at<double>(0,0)));
stats.insert(std::make_pair("Odometry/VarianceAng/", reg.covariance.empty()?0:(float)reg.covariance.at<double>(5,5)));
stats.insert(std::make_pair("Odometry/TimeEstimation/ms", timeEstimation*1000.0f));
stats.insert(std::make_pair("Odometry/TimeFiltering/ms", timeParticleFiltering*1000.0f));
stats.insert(std::make_pair("Odometry/LocalMapSize/", localMapSize));
corelib/src/OdometryThread.cpp
+50 -27
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
namespace rtabmap {
@@ -63,7 +64,7 @@ bool OdometryThread::handleEvent(UEvent * event)
SensorEvent * sensorEvent = (SensorEvent*)event;
if(sensorEvent->getCode() == SensorEvent::kCodeData)
{
this->addData(sensorEvent->data());
this->addData(*sensorEvent);
}
}
else if(event->getClassName().compare("IMUEvent") == 0)
@@ -112,31 +113,51 @@ void OdometryThread::mainLoop()
_imuBuffer.clear();
_oldestAsyncImuStamp = 0.0;
_newestAsyncImuStamp = 0.0;
_previousGuessPose.setNull();
}
SensorData data;
if(getData(data))
SensorEvent event;
if(getData(event))
{
OdometryInfo info;
UDEBUG("Processing data...");
Transform pose = _odometry->process(data, &info);
Transform guess;
UDEBUG("event.info().odomPose=%s", event.info().odomPose.prettyPrint().c_str());
if(!_previousGuessPose.isNull() && !event.info().odomPose.isNull()) {
guess = _previousGuessPose.inverse() * event.info().odomPose;
}
SensorData data = event.data();
Transform pose = _odometry->process(data, guess , &info);
if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (pose.isNull() && data.imu().empty()))
{
UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
if(!pose.isNull()) {
_previousGuessPose = event.info().odomPose;
UASSERT(event.info().odomPose.isNull() || !info.reg.covariance.empty());
if(!event.info().odomPose.isNull() && info.reg.covariance.at<double>(0,0) >= 9999 &&
(pose.x() != 0.0f || pose.y() != 0.0f || pose.z() != 0.0f)) // not the first frame
{
// In case of external guess and auto reset, keep reporting lost till we
// process the second frame with valid covariance. This way it
// won't trigger a new map.
pose = Transform();
}
}
// a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, pose, info));
}
}
}
void OdometryThread::addData(const SensorData & data)
void OdometryThread::addData(const SensorEvent & event)
{
if(data.imu().empty())
if(event.data().imu().empty())
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0)
{
if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) &&
data.laserScanRaw().empty())
if((event.data().imageRaw().empty() || event.data().depthOrRightRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty())) &&
event.data().laserScanRaw().empty())
{
ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
return;
@@ -145,7 +166,7 @@ void OdometryThread::addData(const SensorData & data)
else
{
// Mono can accept RGB only
if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty()))
if(event.data().imageRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty()))
{
ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
return;
@@ -156,30 +177,32 @@ void OdometryThread::addData(const SensorData & data)
bool notify = true;
_dataMutex.lock();
{
if( !data.imageRaw().empty() ||
!data.imageCompressed().empty() ||
!data.laserScanRaw().isEmpty() ||
!data.laserScanCompressed().empty() ||
data.imu().empty())
if( !event.data().imageRaw().empty() ||
!event.data().imageCompressed().empty() ||
!event.data().laserScanRaw().isEmpty() ||
!event.data().laserScanCompressed().empty() ||
event.data().imu().empty())
{
if(_oldestAsyncImuStamp > 0.0 && data.stamp() < _oldestAsyncImuStamp) {
if(_oldestAsyncImuStamp > 0.0 && event.data().stamp() < _oldestAsyncImuStamp) {
UWARN("Received image/lidar with stamp (%f) older than oldest received imu "
"(%f), skipping that frame (imu buffer size=%ld). "
"When using async IMU, make sure IMU is published faster "
"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
data.stamp(), _oldestAsyncImuStamp, _imuBuffer.size());
"than camera/lidar (assuming IMU latency is very small compared to camera/lidar)."
"Current camera/lidar delay is %fs.",
event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
notify = false;
}
else if(_newestAsyncImuStamp > 0.0 && data.stamp()>=_newestAsyncImuStamp) {
else if(_newestAsyncImuStamp > 0.0 && event.data().stamp()>=_newestAsyncImuStamp) {
UWARN("Received image/lidar with stamp (%f) newer than latest received imu "
"(%f), skipping that frame (imu buffer size=%ld). "
"When using async IMU, make sure IMU is published faster "
"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
data.stamp(), _newestAsyncImuStamp, _imuBuffer.size());
"than camera/lidar (assuming IMU latency is very small compared to camera/lidar). "
"Current camera/lidar delay is %fs.",
event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
notify = false;
}
else {
_dataBuffer.push_back(data);
_dataBuffer.push_back(event);
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
{
UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
@@ -190,11 +213,11 @@ void OdometryThread::addData(const SensorData & data)
}
else
{
_imuBuffer.push_back(data);
_imuBuffer.push_back(event.data());
if(_oldestAsyncImuStamp == 0) {
_oldestAsyncImuStamp = data.stamp();
_oldestAsyncImuStamp = event.data().stamp();
}
_newestAsyncImuStamp = data.stamp();
_newestAsyncImuStamp = event.data().stamp();
}
}
_dataMutex.unlock();
@@ -205,7 +228,7 @@ void OdometryThread::addData(const SensorData & data)
}
}
bool OdometryThread::getData(SensorData & data)
bool OdometryThread::getData(SensorEvent & event)
{
bool dataFilled = false;
_dataAdded.acquire();
@@ -219,12 +242,12 @@ bool OdometryThread::getData(SensorData & data)
_odometry->process(_imuBuffer.front());
double stamp =_imuBuffer.front().stamp();
_imuBuffer.pop_front();
if(stamp > _dataBuffer.front().stamp()) {
if(stamp > _dataBuffer.front().data().stamp()) {
break;
}
}
data = _dataBuffer.front();
event = _dataBuffer.front();
_dataBuffer.pop_front();
dataFilled = true;
}
corelib/src/Optimizer.cpp
+88 -51
View File
@@ -185,6 +185,48 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
return optimizer;
}
class LinkIdKey
{
public:
LinkIdKey(int id, Link::Type type) :
id_(id),
type_(type) {}
bool operator<(const LinkIdKey & k) const
{
// landmark, sort by smallest to largest landmark id, after normal links
if(id_ < 0 && k.id_ < 0)
{
return id_ > k.id_;
}
else if(id_ < 0) {
return false;
}
else if(k.id_ < 0) {
return true;
}
if(type_ == Link::kNeighbor && k.type_ != Link::kNeighbor)
{
return true;
}
else if(type_ != Link::kNeighbor && k.type_ == Link::kNeighbor)
{
return false;
}
else if(type_ == Link::kNeighborMerged && k.type_ != Link::kNeighbor && k.type_ != Link::kNeighborMerged)
{
return true;
}
else
{
// normal link, sort by smallest to largest id
return id_ < k.id_;
}
}
int id_;
Link::Type type_;
};
void Optimizer::getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
@@ -199,8 +241,8 @@ void Optimizer::getConnectedGraph(
posesOut.clear();
linksOut.clear();
std::set<int> nextPoses;
nextPoses.insert(fromId);
std::map<LinkIdKey, Transform> nextPoses;
nextPoses.insert(std::make_pair(LinkIdKey(fromId, Link::kUndef), posesIn.find(fromId)->second));
std::multimap<int, std::pair<int, Link::Type> > biLinks;
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
{
@@ -216,20 +258,25 @@ void Optimizer::getConnectedGraph(
while(nextPoses.size())
{
int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
nextPoses.erase(*nextPoses.rbegin());
// Fill up all nodes before landmarks
// For nodes, fill up all neightbor nodes before loop closure ones
int currentId = nextPoses.begin()->first.id_;
Transform currentPose = nextPoses.begin()->second;
nextPoses.erase(nextPoses.begin());
if(posesOut.empty())
if(posesOut.find(currentId) != posesOut.end()) {
// Already added from priority list
continue;
}
posesOut.insert(std::make_pair(currentId, currentPose));
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
{
posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
{
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
{
linksOut.insert(*pter);
}
linksOut.insert(*pter);
}
}
@@ -240,52 +287,42 @@ void Optimizer::getConnectedGraph(
if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
{
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
if(nextPoses.find(toId) == nextPoses.end())
UASSERT(kter!=linksIn.end());
if(!uContains(posesOut, toId))
{
if(!uContains(posesOut, toId))
const Transform & poseToIn = posesIn.at(toId);
Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
Transform pose;
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
{
const Transform & poseToIn = posesIn.at(toId);
Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
if(poseToIn.is3DoF())
{
if(poseToIn.is3DoF())
{
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
}
else
{
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
}
pose = (posesOut.at(currentId) * t).to3DoF();
}
else
{
posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
pose = (posesOut.at(currentId) * t).to4DoF();
}
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
{
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
{
linksOut.insert(*pter);
}
}
nextPoses.insert(toId);
}
else
{
pose = posesOut.at(currentId)* t;
}
// only add unique links
if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose));
}
// only add unique links
if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
{
if(kter->second.to() < 0)
{
if(kter->second.to() < 0)
{
// For landmarks, make sure fromId is the landmark
linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
}
else
{
linksOut.insert(*kter);
}
// For landmarks, make sure fromId is the landmark
linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
}
else
{
linksOut.insert(*kter);
}
}
}
@@ -608,8 +645,8 @@ void Optimizer::computeBACorrespondences(
}
}
if(sFrom.getWords().size() &&
sTo.getWords().size() &&
if(sFrom.getWordsKpts().size() &&
sTo.getWordsKpts().size() &&
sFrom.getWords3().size())
{
if(!rematchFeatures)
corelib/src/Parameters.cpp
+14 -3
View File
@@ -168,6 +168,7 @@ bool Parameters::isFeatureParameter(const std::string & parameter)
group.compare("BRISK") == 0 ||
group.compare("KAZE") == 0 ||
group.compare("SuperPoint") == 0 ||
group.compare("SuperPointRpautrat") == 0 ||
group.compare("PyDetector") == 0;
}
@@ -182,6 +183,7 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
(stereo && group.compare("Stereo") == 0) ||
(icp && group.compare("Icp") == 0) ||
(vis && Parameters::isFeatureParameter(iter->first)) ||
group.compare("OdomCuVSLAM") == 0 ||
group.compare("Reg") == 0 ||
group.compare("Optimizer") == 0 ||
group.compare("g2o") == 0 ||
@@ -236,6 +238,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.23.1
removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath", std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
// 0.21.13
removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly", std::make_pair(false, "")));
@@ -658,6 +663,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With SuperPoint Rpautrat:";
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With Python3:";
#ifdef RTABMAP_PYTHON
@@ -936,7 +947,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With VINS-Fusion:";
#ifdef RTABMAP_VINS
#ifdef RTABMAP_VINS_FUSION
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
@@ -1109,8 +1120,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
ignore = true;
}
#endif
#ifndef RTABMAP_ORBSLAM2
if(group.compare("OdomORBSLAM2") == 0)
#ifndef RTABMAP_ORB_SLAM
if(group.compare("OdomORBSLAM") == 0)
{
ignore = true;
}
corelib/src/RegistrationVis.cpp
+19 -11
View File
@@ -286,6 +286,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
}
if(uContains(parameters, Parameters::kRtabmapWorkingDirectory()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), parameters.at(Parameters::kRtabmapWorkingDirectory())));
}
delete _detectorFrom;
delete _detectorTo;
@@ -329,11 +333,12 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("Feature Detector = %d", (int)_detectorFrom->getType());
UDEBUG("guess=%s", guess.prettyPrint().c_str());
UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d words kpts, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
fromSignature.id(),
(int)fromSignature.getWords().size(),
(int)fromSignature.getWords3().size(),
(int)fromSignature.getWordsDescriptors().rows,
(int)fromSignature.getWordsKpts().size(),
(int)fromSignature.sensorData().keypoints().size(),
(int)fromSignature.sensorData().keypoints3D().size(),
fromSignature.sensorData().descriptors().rows,
@@ -342,11 +347,12 @@ Transform RegistrationVis::computeTransformationImpl(
(int)fromSignature.sensorData().cameraModels().size(),
(int)fromSignature.sensorData().stereoCameraModels().size());
UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d words kpts, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
toSignature.id(),
(int)toSignature.getWords().size(),
(int)toSignature.getWords3().size(),
(int)toSignature.getWordsDescriptors().rows,
(int)toSignature.getWordsKpts().size(),
(int)toSignature.sensorData().keypoints().size(),
(int)toSignature.sensorData().keypoints3D().size(),
toSignature.sensorData().descriptors().rows,
@@ -375,6 +381,9 @@ Transform RegistrationVis::computeTransformationImpl(
{
UDEBUG("");
// just some checks to make sure that input data are ok
UASSERT(fromSignature.getWords().empty() ||
fromSignature.getWordsKpts().empty() ||
(fromSignature.getWords().size() == fromSignature.getWordsKpts().size()));
UASSERT(fromSignature.getWords().empty() ||
fromSignature.getWords3().empty() ||
(fromSignature.getWords().size() == fromSignature.getWords3().size()));
@@ -382,8 +391,11 @@ Transform RegistrationVis::computeTransformationImpl(
(int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
fromSignature.sensorData().descriptors().empty() ||
fromSignature.getWordsDescriptors().empty() == 0);
UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
(toSignature.getWords().size() && toSignature.getWords3().empty())||
UASSERT(toSignature.getWords().empty() ||
toSignature.getWordsKpts().empty() ||
(toSignature.getWords().size() == toSignature.getWordsKpts().size()));
UASSERT(toSignature.getWords().empty() ||
toSignature.getWords3().empty() ||
(toSignature.getWords().size() == toSignature.getWords3().size()));
UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
(int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||
@@ -1660,14 +1672,10 @@ Transform RegistrationVis::computeTransformationImpl(
UINFO(msg.c_str());
}
}
else if(fromSignature.getWords().size() == 0)
else
{
msg = uFormat("No enough features (%d)", (int)fromSignature.getWords().size());
UWARN(msg.c_str());
}
else
{
msg = uFormat("No camera model");
msg = uFormat("No enough features < %s=%d (from=%d to=%d)",
Parameters::kVisMinInliers().c_str(), _minInliers, (int)fromSignature.getWords().size(), (int)toSignature.getWords().size());
UWARN(msg.c_str());
}
}
corelib/src/Rtabmap.cpp
+45 -52
View File
@@ -1502,8 +1502,8 @@ bool Rtabmap::process(
float angleToClosestNodeInTheGraph = 0;
if(_rgbdSlamMode)
{
double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
double linVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
double angVar = odomCovariance.empty()?0.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar);
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar);
@@ -1544,9 +1544,10 @@ bool Rtabmap::process(
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
bool isMoving = fabs(x) > _rgbdLinearUpdate ||
fabs(y) > _rgbdLinearUpdate ||
fabs(z) > _rgbdLinearUpdate ||
bool isMoving = (_rgbdLinearUpdate > 0.0f && (
fabs(x) > _rgbdLinearUpdate ||
fabs(y) > _rgbdLinearUpdate ||
fabs(z) > _rgbdLinearUpdate)) ||
(_rgbdAngularUpdate>0.0f && (
fabs(roll) > _rgbdAngularUpdate ||
fabs(pitch) > _rgbdAngularUpdate ||
@@ -1607,6 +1608,7 @@ bool Rtabmap::process(
Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
if(!t.isNull())
{
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
oldId,
signature->id(),
@@ -1614,7 +1616,6 @@ bool Rtabmap::process(
info.covariance.at<double>(5,5),
guess.prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));
if(_optimizeFromGraphEnd)
@@ -1894,7 +1895,7 @@ bool Rtabmap::process(
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance))))
{
++proximityDetectionsInTimeFound;
@@ -2395,24 +2396,22 @@ bool Rtabmap::process(
distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
}
if(distanceSoFar <= _localRadius)
if(_memory->getSignature(_path[i].first) != 0)
{
if(_memory->getSignature(_path[i].first) != 0)
if(immunizedLocations.insert(_path[i].first).second)
{
if(immunizedLocations.insert(_path[i].first).second)
{
++immunizedLocally;
}
UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
}
else if(retrievalLocalIds.size() < _maxLocalRetrieved)
{
UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
retrievalLocalIds.push_back(_path[i].first);
// retrieved locations are automatically immunized
++immunizedLocally;
}
UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
}
else
else if(retrievalLocalIds.size() < _maxLocalRetrieved)
{
UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
retrievalLocalIds.push_back(_path[i].first);
// retrieved locations are automatically immunized
}
if(distanceSoFar > _localRadius)
{
UDEBUG("Stop on node %d (dist=%fm > %fm)",
_path[i].first, distanceSoFar, _localRadius);
@@ -2784,7 +2783,7 @@ bool Rtabmap::process(
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
//for statistics
loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
@@ -2995,7 +2994,7 @@ bool Rtabmap::process(
}
// set Identify covariance for laser scan matching only
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
@@ -3083,7 +3082,7 @@ bool Rtabmap::process(
if(!rejectedLoopClosure)
{
// Make the new one the parent of the old one
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
@@ -3158,17 +3157,7 @@ bool Rtabmap::process(
UASSERT(uContains(_optimizedPoses, signature->id()));
UASSERT_MSG(uContains(_optimizedPoses, _path[_pathCurrentIndex].first), uFormat("id=%d", _path[_pathCurrentIndex].first).c_str());
Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius)
{
_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
}
else
{
UERROR("Virtual link larger than local radius (%fm > %fm). Aborting the plan!",
virtualLoop.getNorm(), _localRadius);
this->clearPath(-1);
}
_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
}
}
@@ -3269,6 +3258,7 @@ bool Rtabmap::process(
{
constraints.insert(std::make_pair(iter->second.from(), iter->second));
}
cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
@@ -3276,6 +3266,7 @@ bool Rtabmap::process(
if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
{
poses.insert(*iterPose);
// make the poses in the map fixed
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
@@ -3285,11 +3276,14 @@ bool Rtabmap::process(
std::map<int, Transform> posesOut;
std::multimap<int, Link> edgeConstraintsOut;
bool priorsIgnored = _graphOptimizer->priorsIgnored();
UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
// If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
if(ULogger::level() == ULogger::kDebug)
{
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
@@ -5644,6 +5638,7 @@ int Rtabmap::detectMoreLoopClosures(
const ProgressState * processState,
float clusterRadiusMin)
{
UDEBUG("");
UASSERT(iterations>0);
if(_graphOptimizer->iterations() <= 0)
@@ -6326,6 +6321,7 @@ bool Rtabmap::addLink(const Link & link)
std::map<int, Transform> poses = _odomCachePoses;
std::multimap<int, Link> constraints = _odomCacheConstraints;
constraints.insert(std::make_pair(link.from(), link));
cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
@@ -6333,7 +6329,7 @@ bool Rtabmap::addLink(const Link & link)
{
poses.insert(*iterPose);
// make the poses in the map fixed
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*999999)));
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
}
}
@@ -6927,24 +6923,24 @@ void Rtabmap::updateGoalIndex()
{
distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
}
if(distanceSoFar <= _localRadius)
if(_path[i].first != _path[i-1].first)
{
if(_path[i].first != _path[i-1].first)
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
{
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}
}
else
if(distanceSoFar > _localRadius)
{
UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
break;
}
}
@@ -7004,11 +7000,8 @@ void Rtabmap::updateGoalIndex()
if((goalIndex == _pathCurrentIndex && i == _path.size()-1) ||
_pathUnreachableNodes.find(i) == _pathUnreachableNodes.end())
{
if(distanceFromCurrentNode <= _localRadius)
{
goalIndex = i;
}
else
goalIndex = i;
if(distanceFromCurrentNode > _localRadius)
{
break;
}
corelib/src/RtabmapThread.cpp
+3 -2
View File
@@ -506,9 +506,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
ignoreFrame = true;
}
}
UASSERT(!odomEvent.info().reg.covariance.empty());
if(!lastPose_.isIdentity() &&
(odomEvent.pose().isIdentity() ||
odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
(odomEvent.pose().isIdentity() ||
odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
{
if(odomEvent.pose().isIdentity())
{
corelib/src/SensorCaptureThread.cpp
-1
View File
@@ -960,7 +960,6 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i
{
UASSERT(!data.imu().localTransform().isNull());
imu.convertToBaseFrame();
}
_imuFilter->update(
imu.angularVelocity()[0],
corelib/src/SensorData.cpp
+3 -3
View File
@@ -548,7 +548,7 @@ void SensorData::setOccupancyGrid(
float cellSize,
const cv::Point3f & viewPoint)
{
UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
//UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
(!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
(!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
@@ -649,7 +649,7 @@ void SensorData::uncompressData(
cv::Mat * emptyCellsRaw,
cv::Mat * depthConfidenceRaw)
{
UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)",
/*UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)",
this->id(),
imageRaw?1:0,
depthRaw?1:0,
@@ -658,7 +658,7 @@ void SensorData::uncompressData(
groundCellsRaw?1:0,
obstacleCellsRaw?1:0,
emptyCellsRaw?1:0,
depthConfidenceRaw?1:0);
depthConfidenceRaw?1:0);*/
if(imageRaw == 0 &&
depthRaw == 0 &&
laserScanRaw == 0 &&
corelib/src/Signature.cpp
+3 -3
View File
@@ -118,7 +118,7 @@ void Signature::addLinks(const std::map<int, Link> & links)
}
void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
//UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), 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.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
@@ -318,7 +318,7 @@ void Signature::setWords(const std::multimap<int, int> & words,
UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
UASSERT_MSG(points.empty() || points.size() == words.size(), uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
//UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
_invalidWordsCount = 0;
for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
@@ -328,7 +328,7 @@ void Signature::setWords(const std::multimap<int, int> & words,
++_invalidWordsCount;
}
// make sure indexes are all valid!
UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
UASSERT_MSG(iter->second<0 || iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
}
_enabled = false;
corelib/src/VWDictionary.cpp
+187 -45
View File
@@ -51,7 +51,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <fstream>
#include <string>
#define KDTREE_SIZE 4
#define KNN_CHECKS 32
namespace rtabmap
@@ -69,9 +68,11 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
_nndrRatio(Parameters::defaultKpNndrRatio()),
_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
_serializeWithChecksum(Parameters::defaultKpSerializeWithChecksum()),
_lastWordId(0),
useDistanceL1_(false),
_flannIndex(new FlannIndex()),
_modified(true),
_strategy(kNNBruteForce)
{
this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy());
@@ -89,6 +90,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
Parameters::parse(parameters, Parameters::kKpSerializeWithChecksum(), _serializeWithChecksum);
Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
bool byteToFloat = _byteToFloat;
@@ -160,7 +162,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
DBDriver * driver = DBDriver::create();
if(driver->openConnection(dictionaryPath, false))
{
driver->load(this, false);
driver->load(*this, false);
for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
{
iter->second->setSaved(true);
@@ -289,6 +291,11 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
_newDictionaryPath = dictionaryPath;
}
bool VWDictionary::isModified() const
{
return _modified;
}
bool VWDictionary::setNNStrategy(NNStrategy strategy)
{
#if CV_MAJOR_VERSION < 3
@@ -484,7 +491,13 @@ void VWDictionary::update()
if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
{
if(_incrementalFlann &&
_modified = true;
bool firstUpdate = _removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size();
UDEBUG("firstUpdate=%s (_removedIndexedWords=%ld, _visualWords=%ld, _notIndexedWords=%ld)",
firstUpdate?"true":"false", _removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
if(!firstUpdate &&
_incrementalFlann &&
_strategy < kNNBruteForce &&
_visualWords.size())
{
@@ -501,7 +514,9 @@ void VWDictionary::update()
if(_notIndexedWords.size())
{
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
UTimer timer;
timer.start();
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size(), _byteToFloat?"true":"false");
for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
{
VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
@@ -528,24 +543,13 @@ void VWDictionary::update()
int index = 0;
if(!_flannIndex->isBuilt())
{
UDEBUG("Building FLANN index...");
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(descriptor, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannKdTree:
UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannLSH:
UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(descriptor, 12, 20, 2, _rebalancingFactor);
break;
default:
UFATAL("Not supposed to be here!");
break;
}
UDEBUG("Building FLANN index... (strategy=%s, byteToFloat=%s, useDistanceL1=%s, rebalancingFactor=%f)",
nnStrategyName(_strategy).c_str(), _byteToFloat?"true":"false", useDistanceL1_?"true":"false", _rebalancingFactor);
_flannIndex->buildIndex(
_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
descriptor, useDistanceL1_, _rebalancingFactor);
UDEBUG("Building FLANN index... done!");
}
else
@@ -561,7 +565,7 @@ void VWDictionary::update()
inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
UASSERT(inserted.second);
}
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done! (in %f s)", (int)_notIndexedWords.size(), timer.ticks());
}
}
else if(_strategy >= kNNBruteForce &&
@@ -657,23 +661,13 @@ void VWDictionary::update()
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
case kNNFlannKdTree:
UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
case kNNFlannLSH:
UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
default:
break;
}
_flannIndex->buildIndex(
_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
_dataTree,
useDistanceL1_,
_incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
}
}
@@ -689,6 +683,146 @@ void VWDictionary::update()
UDEBUG("");
}
std::vector<unsigned char> VWDictionary::serializeIndex() const
{
if(_strategy >= kNNBruteForce) {
UINFO("Not flann strategy, ignoring serialization...");
return std::vector<unsigned char>();
}
if(!_flannIndex->isBuilt() || !_removedIndexedWords.empty() || !_notIndexedWords.empty() || _visualWords.empty()) {
UWARN("Flann index is not buit, or there are words not indexed, cannot do serialization.");
return std::vector<unsigned char>();
}
return _flannIndex->serializeIndex(_serializeWithChecksum);
}
void VWDictionary::deserializeIndex(const std::vector<unsigned char> & data)
{
deserializeIndex(data.data(), data.size());
}
void VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
{
if(data== NULL || size == 0)
{
UWARN("Trying to deserialize empty data, aborting.");
return;
}
UDEBUG("Loading flann index... (data size=%ld bytes)", size);
if(_strategy >= kNNBruteForce) {
//ignore
return;
}
if(_flannIndex->isBuilt()) {
UERROR("Flann index is already built, cannot deserialize data!");
return;
}
if(_visualWords.empty()) {
UERROR("Descriptors should be added before deserializing flann index! See VWDictionary::addWord()");
return;
}
if(!(_removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size())) {
UERROR("State of dictionary not as expected before deserializing. (removed words=%ld, words=%ld, not indexed=%ld)",
_removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
return;
}
std::map<int, int> mapIndexId;
std::map<int, int> mapIdIndex;
cv::Mat dataTree;
UTimer timer;
timer.start();
int dim = _visualWords.begin()->second->getDescriptor().cols;
int type;
if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
{
useDistanceL1_ = true;
if(_strategy == kNNFlannKdTree)
{
type = CV_32F;
if(!_byteToFloat)
{
dim *= 8;
}
}
else
{
type = _visualWords.begin()->second->getDescriptor().type();
}
}
else
{
type = _visualWords.begin()->second->getDescriptor().type();
}
UASSERT(type == CV_32F || type == CV_8U);
UASSERT(dim > 0);
// Create the data matrix
dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
{
cv::Mat descriptor;
if(iter->second->getDescriptor().type() == CV_8U)
{
if(_strategy == kNNFlannKdTree)
{
descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
}
else
{
descriptor = iter->second->getDescriptor();
}
}
else
{
descriptor = iter->second->getDescriptor();
}
UASSERT_MSG(descriptor.type() == type, uFormat("%d vs %d", descriptor.type(), type).c_str());
UASSERT_MSG(descriptor.cols == dim, uFormat("%d vs %d", descriptor.cols, dim).c_str());
descriptor.copyTo(dataTree.row(i));
mapIndexId.insert(mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
mapIdIndex.insert(mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
}
ULOGGER_DEBUG("mapIndexId.size() = %d, words.size()=%d, dim=%d", mapIndexId.size(), _visualWords.size(), dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
std::string errorMsg;
if(_flannIndex->loadIndex(
data,
size,
_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
FlannIndex::FLANN_INDEX_KDTREE,
dataTree,
useDistanceL1_,
_incrementalDictionary && _incrementalFlann ? _rebalancingFactor:1,
&errorMsg))
{
_mapIndexId = mapIndexId;
_mapIdIndex = mapIdIndex;
_dataTree = dataTree;
_notIndexedWords.clear();
_modified = false;
}
else {
UWARN("Failed deserializing flann index data (error: %s), the index will be rebuilt on next update.", errorMsg.c_str());
_flannIndex->release(); // reset to initial state
}
ULOGGER_DEBUG("Time to load flann index = %f s", timer.ticks());
}
void VWDictionary::clear(bool printWarningsIfNotEmpty)
{
ULOGGER_DEBUG("");
@@ -718,6 +852,7 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty)
_unusedWords.clear();
_flannIndex->release();
useDistanceL1_ = false;
_modified = true;
}
int VWDictionary::getNextId()
@@ -784,14 +919,21 @@ std::list<int> VWDictionary::addNewWords(
type = _visualWords.begin()->second->getDescriptor().type();
UASSERT(type == CV_32F || type == CV_8U);
}
static std::string moreInfo = uFormat(
"This could happen if the computer doesn't have access to same "
"feature detectors than when the database was created. This could "
"also happen if we enabled \"%s\" but the first frame received "
"was empty, thus features were re-extracted with a different detector "
"than the one used by the odometry.",
Parameters::kMemUseOdomFeatures().c_str());
if(dim && dim != descriptorsIn.cols)
{
UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim);
UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary (size=%d). %s", descriptorsIn.cols, dim, moreInfo.c_str());
return wordIds;
}
if(type>=0 && type != descriptorsIn.type())
{
UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type);
UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary (type=%d). %s", descriptorsIn.type(), type, moreInfo.c_str());
return wordIds;
}
@@ -1394,15 +1536,15 @@ void VWDictionary::addWord(VisualWord * vw)
{
if(vw)
{
_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
_notIndexedWords.insert(vw->id());
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
if(vw->getReferences().size())
{
_totalActiveReferences += uSum(uValues(vw->getReferences()));
}
else
{
_unusedWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
_unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
}
if(_lastWordId < vw->id())
{
corelib/src/VisualWord.cpp
+1 -1
View File
@@ -57,7 +57,7 @@ void VisualWord::addRef(int signatureId)
}
else
{
_references.insert(std::pair<int, int>(signatureId, 1));
_references.insert(_references.end(), std::pair<int, int>(signatureId, 1));
}
++_totalReferences;
}
corelib/src/camera/CameraDepthAI.cpp
+4 -3
View File
@@ -370,9 +370,10 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
matrix[2][0], matrix[2][1], matrix[2][2]);
std::vector<float> coeffs = calibHandler.getDistortionCoefficients(cameraId);
if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective)
distCoeffs = (cv::Mat_<double>(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]);
if(calibHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective) {
UASSERT(coeffs.size()>=14);
distCoeffs = (cv::Mat_<double>(1,14) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7], coeffs[8], coeffs[9], coeffs[10], coeffs[11], coeffs[12], coeffs[13]);
}
if(alphaScaling_>-1.0f)
newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
else
corelib/src/camera/CameraImages.cpp
+3 -3
View File
@@ -523,19 +523,19 @@ bool CameraImages::readPoses(
UERROR("Cannot read pose file \"%s\".", filePath.c_str());
return false;
}
else if((format != 1 && format != 10 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
else if((format != 1 && format != 10 && format != 12 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
{
UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
"the pose file path if you don't want to use it (current file path=%s).",
(int)poses.size(), this->imagesCount(), filePath.c_str());
return false;
}
else if((format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
else if((format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9) && (inOutStamps.empty() && stamps.size()!=poses.size()))
{
UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
return false;
}
else if(format == 1 || format == 10 || format == 5 || format == 6 || format == 7 || format == 9)
else if(format == 1 || format == 10 || format == 12 || format == 5 || format == 6 || format == 7 || format == 9)
{
UDEBUG("");
//Match ground truth values with images
corelib/src/camera/CameraOrbbecSDK.cpp
+742
View File
@@ -0,0 +1,742 @@
/*
Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/camera/CameraOrbbecSDK.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UThread.h>
#ifdef RTABMAP_ORBBEC_SDK
#include <libobsensor/ObSensor.hpp>
#endif
namespace rtabmap
{
#ifdef RTABMAP_ORBBEC_SDK
Transform obToRtabmap(const OBExtrinsic & t)
{
return Transform(t.rot[0], t.rot[1], t.rot[2], t.trans[0]/1000.0f,
t.rot[3], t.rot[4], t.rot[5], t.trans[1]/1000.0f,
t.rot[6], t.rot[7], t.rot[8], t.trans[2]/1000.0f);
}
cv::Mat obColorFrameToCv(const ob::VideoFrame & videoFrame)
{
cv::Mat rgb;
switch(videoFrame.getFormat()) {
case OB_FORMAT_MJPG: {
cv::Mat rawMat(1, videoFrame.getDataSize(), CV_8UC1, videoFrame.getData());
rgb = cv::imdecode(rawMat, 1);
} break;
case OB_FORMAT_NV21: {
cv::Mat rawMat(videoFrame.getHeight() * 3 / 2, videoFrame.getWidth(), CV_8UC1, videoFrame.getData());
cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_NV21);
} break;
case OB_FORMAT_YUYV:
case OB_FORMAT_YUY2: {
cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC2, videoFrame.getData());
cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_YUY2);
} break;
case OB_FORMAT_BGR: {
cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC3, videoFrame.getData());
cv::cvtColor(rawMat, rgb, cv::COLOR_BGR2RGB);
} break;
case OB_FORMAT_RGB: {
cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC3, videoFrame.getData());
cv::cvtColor(rawMat, rgb, cv::COLOR_RGB2BGR);
} break;
case OB_FORMAT_RGBA: {
cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC4, videoFrame.getData());
cv::cvtColor(rawMat, rgb, cv::COLOR_RGBA2BGR);
} break;
case OB_FORMAT_BGRA: {
cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC4, videoFrame.getData());
cv::cvtColor(rawMat, rgb, cv::COLOR_BGRA2RGB);
} break;
case OB_FORMAT_UYVY: {
cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC2, videoFrame.getData());
cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_UYVY);
} break;
case OB_FORMAT_I420: {
cv::Mat rawMat(videoFrame.getHeight() * 3 / 2, videoFrame.getWidth(), CV_8UC1, videoFrame.getData());
cv::cvtColor(rawMat, rgb, cv::COLOR_YUV2BGR_I420);
} break;
case OB_FORMAT_Y8: {
rgb = cv::Mat(videoFrame.getHeight(), videoFrame.getWidth(), CV_8UC1, videoFrame.getData()).clone();
} break;
case OB_FORMAT_Y16: {
cv::Mat rawMat(videoFrame.getHeight(), videoFrame.getWidth(), CV_16UC1, videoFrame.getData());
rawMat.convertTo(rgb, CV_8UC1, 255.0 / 65535.0);
} break;
default:
break;
}
return rgb;
}
cv::Mat obDepthFrameToCv(const ob::DepthFrame & depthFrame)
{
cv::Mat depth;
if(depthFrame.getFormat() == OB_FORMAT_Y16 || depthFrame.getFormat() == OB_FORMAT_Z16 || depthFrame.getFormat() == OB_FORMAT_Y12C4) {
cv::Mat rawMat = cv::Mat(depthFrame.getHeight(), depthFrame.getWidth(), CV_16UC1, depthFrame.getData());
float scale = depthFrame.getValueScale() / 1000.0f;
rawMat.convertTo(depth, CV_32F, scale);
}
return depth;
}
cv::Mat obIntrinsicToK(const OBCameraIntrinsic & intrinsics)
{
cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
K.at<double>(0,0) = intrinsics.fx;
K.at<double>(1,1) = intrinsics.fy;
K.at<double>(0,2) = intrinsics.cx;
K.at<double>(1,2) = intrinsics.cy;
return K;
}
cv::Mat obIntrinsicToP(const OBCameraIntrinsic & intrinsics)
{
cv::Mat P = cv::Mat::eye(3,4,CV_64FC1);
obIntrinsicToK(intrinsics).copyTo(P.colRange(0,3));
return P;
}
cv::Mat obDistortionToD(const OBCameraDistortion & distortion)
{
cv::Mat D = cv::Mat(1,8,CV_64FC1);
D.at<double>(0,0) = distortion.k1;
D.at<double>(0,1) = distortion.k2;
D.at<double>(0,2) = distortion.p1;
D.at<double>(0,3) = distortion.p2;
D.at<double>(0,4) = distortion.k3;
D.at<double>(0,5) = distortion.k4;
D.at<double>(0,6) = distortion.k5;
D.at<double>(0,7) = distortion.k6;
if(distortion.k4 == 0 && distortion.k5 == 0 && distortion.k6 == 0)
{
D = D.colRange(0,5);
}
return D;
}
#endif
bool CameraOrbbecSDK::available()
{
#ifdef RTABMAP_ORBBEC_SDK
return true;
#else
return false;
#endif
}
CameraOrbbecSDK::CameraOrbbecSDK(
std::string deviceId,
unsigned int colorWidth,
unsigned int colorHeight,
unsigned int depthWidth,
unsigned int depthHeight,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform)
#ifdef RTABMAP_ORBBEC_SDK
, deviceId_(deviceId),
colorWidth_(colorWidth),
colorHeight_(colorHeight),
depthWidth_(depthWidth),
depthHeight_(depthHeight),
pipeline_(nullptr),
imuPipeline_(nullptr),
alignFilter_(nullptr),
imuLocalTransformInitialized_(false),
lastAccStamp_(0),
lastImageStamp_(0),
globalTimestampAvailable_(false),
rectifyColor_(false),
convertDepthToMM_(true),
imuPublished_(true)
#endif
{
}
CameraOrbbecSDK::~CameraOrbbecSDK()
{
#ifdef RTABMAP_ORBBEC_SDK
this->close();
#endif
}
void CameraOrbbecSDK::close()
{
#ifdef RTABMAP_ORBBEC_SDK
if(imuPipeline_) {
imuPipeline_->stop();
delete imuPipeline_;
imuPipeline_=nullptr;
}
if(pipeline_) {
pipeline_->stop();
delete pipeline_;
pipeline_=nullptr;
}
delete alignFilter_;
alignFilter_ = nullptr;
imuLocalTransform_ = Transform();
imuLocalTransformInitialized_ = false;
lastAccStamp_ = 0;
lastImageStamp_ = 0;
globalTimestampAvailable_ = false;
model_ = CameraModel();
imuBuffer_.clear();
#endif
}
bool CameraOrbbecSDK::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_ORBBEC_SDK
this->close();
std::shared_ptr<ob::Device> device;
ob::Context context;
auto devices = context.queryDeviceList();
UINFO("%d device(s) found", devices->getCount());
for(uint32_t i=0; i<devices->getCount(); ++i)
{
auto currentDevice = devices->getDevice(i);
auto info = currentDevice->getDeviceInfo();
if(deviceId_.find('-') != std::string::npos)
{
// UID
if(deviceId_.compare(info->getUid()) == 0) {
device = currentDevice;
}
}
else if(uSplitNumChar(deviceId_).size() > 1)
{
// Serial
if(deviceId_.compare(info->getSerialNumber()) == 0) {
device = currentDevice;
}
}
else if((deviceId_.empty() && i==0) ||
(!deviceId_.empty() && uStr2Int(deviceId_) == (int)i)) {
// Index
device = currentDevice;
}
std::string type = "Unknown";
switch(info->getDeviceType())
{
case OB_STRUCTURED_LIGHT_MONOCULAR_CAMERA:
type = "Structured Light Monocular Camera";
break;
case OB_STRUCTURED_LIGHT_BINOCULAR_CAMERA:
type = "Structured Light Binocular Camera";
break;
case OB_TOF_CAMERA:
type = "TOF Camera";
break;
default:
break;
}
UINFO("Device %ld:", i);
UINFO(" Name: %s", info->getName());
UINFO(" Type: %s", type.c_str());
UINFO(" Serial: %s", info->getSerialNumber());
UINFO(" UID: %s", info->getUid());
UINFO(" Chip: %s", info->getAsicName());
UINFO(" Hardware version: %s", info->getHardwareVersion());
UINFO(" Firmware version: %s", info->getFirmwareVersion());
}
if(device.get() == nullptr) {
if(deviceId_.empty()) {
UERROR( "Could not find any orbbec compatible devices! Verify that the "
"camera is correctly connected and the udev rules are installed.");
}
else {
UERROR("Could not find an orbbec device with ID \"%s\"! Verify that the "
"camera is correctly connected and the udev rules are installed. "
"Unset the ID to choose the first camera found.");
}
return false;
}
bool hasGyro = false;
bool hasAccel = false;
auto sensors = device->getSensorList();
if(device->isGlobalTimestampSupported())
{
UINFO("Global (host time sync) timestamp is supported.");
device->enableGlobalTimestamp(true);
globalTimestampAvailable_ = true;
}
else
{
UWARN("Global (host time sync) timestamp is not supported! We will use device timestamp, so the camera frames won't be synchronizable with other sensors.");
}
uint32_t maxColorFps = 0;
uint32_t maxDepthFps = 0;
for(uint32_t i=0; i<sensors->getCount(); ++i)
{
if(sensors->getSensorType(i) == OB_SENSOR_GYRO)
{
hasGyro = true;
}
if(sensors->getSensorType(i) == OB_SENSOR_ACCEL)
{
hasAccel = true;
}
if( sensors->getSensorType(i) == OB_SENSOR_DEPTH ||
sensors->getSensorType(i) == OB_SENSOR_COLOR)
{
auto profiles = sensors->getSensor(i)->getStreamProfileList();
UINFO("Supported %s profiles:", sensors->getSensorType(i) == OB_SENSOR_DEPTH?"depth":"color");
for(uint32_t j=0; j<profiles->getCount(); ++j)
{
auto profile = profiles->getProfile(j)->as<ob::VideoStreamProfile>();
UINFO("Resolution: %ldx%ld, FPS: %ld, Format: %d",
profile->getWidth(), profile->getHeight(), profile->getFps(), profile->getFormat(), j==0?" (default)":"");
// Get maximum frame rate based on resolution selected
if(sensors->getSensorType(i) == OB_SENSOR_DEPTH) {
if( profile->getFps() > maxDepthFps &&
depthWidth_ == profile->getWidth() &&
depthHeight_ == profile->getHeight())
{
maxDepthFps = profile->getFps();
}
}
else
{
if( profile->getFps() > maxColorFps &&
colorWidth_ == profile->getWidth() &&
colorHeight_ == profile->getHeight())
{
maxColorFps = profile->getFps();
}
}
}
}
}
// Note that for TOF camera, we want maximum frame rate to better
// sync rgb and depth. For stereo cameras, use the specified frame rate.
if(this->getImageRate()!=0.0f && device->getDeviceInfo()->getDeviceType() != OB_TOF_CAMERA)
{
maxColorFps = maxDepthFps = (unsigned int)this->getImageRate();
this->setImageRate(0);
}
std::shared_ptr<ob::Config> imuConfig;
if(imuPublished_)
{
if(hasGyro && hasAccel)
{
imuPipeline_ = new ob::Pipeline(device);
imuConfig = std::make_shared<ob::Config>();
imuConfig->enableGyroStream();
imuConfig->enableAccelStream();
try {
UINFO("Starting imu pipeline");
imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::FrameSet> frameSet) {
if(frameSet->getCount() != 2)
{
return;
}
if(!imuLocalTransformInitialized_)
{
return;
}
UASSERT(frameSet->getFrame(OB_FRAME_ACCEL) != nullptr &&
frameSet->getFrame(OB_FRAME_GYRO) != nullptr);
auto accel = frameSet->getFrame(OB_FRAME_ACCEL)->as<const ob::AccelFrame>();
auto gyro = frameSet->getFrame(OB_FRAME_GYRO)->as<const ob::GyroFrame>();
uint64_t accelStampUs = globalTimestampAvailable_?accel->getGlobalTimeStampUs():accel->getTimeStampUs();
uint64_t gyroStampUs = globalTimestampAvailable_?gyro->getGlobalTimeStampUs():gyro->getTimeStampUs();
if(accelStampUs != gyroStampUs)
{
UWARN("Received accel and gyro frames with different timestamps (%llu vs %llu), skipping.",
accelStampUs, gyroStampUs);
return;
}
double accStamp = double(accelStampUs)/1e6;
if(accelStampUs <= lastAccStamp_) {
return;
}
lastAccStamp_ = accelStampUs;
auto accelValue = accel->getValue();
auto gyroValue = gyro->getValue();
if(isInterIMUPublishing())
{
IMU imu(cv::Vec3f(gyroValue.x, gyroValue.y, gyroValue.z), cv::Mat::eye(3,3,CV_64FC1),
cv::Vec3f(accelValue.x, accelValue.y, accelValue.z), cv::Mat::eye(3,3,CV_64FC1),
imuLocalTransform_);
this->postInterIMU(imu, accStamp);
}
else
{
UScopeMutex lock(imuMutex_);
imuBuffer_.emplace_hint(imuBuffer_.end(), accStamp, cv::Vec6f(gyroValue.x, gyroValue.y, gyroValue.z, accelValue.x, accelValue.y, accelValue.z));
if(imuBuffer_.size()>1000) {
imuBuffer_.erase(imuBuffer_.begin());
}
}
});
}
catch(const ob::Error & e) {
UERROR("Unexpected error when configuring IMU stream: %s", e.what());
}
}
else
{
UWARN("IMU option is enabled but the camera doesn't have an IMU, ignoring.");
}
}
pipeline_ = new ob::Pipeline(device);
auto config = std::make_shared<ob::Config>();
// Set highest frame rate possible to reduce color/depth sync diff
config->enableVideoStream(OB_STREAM_COLOR, colorWidth_, colorHeight_, maxColorFps, OB_FORMAT_RGB);
config->enableVideoStream(OB_STREAM_DEPTH, depthWidth_, depthHeight_, maxDepthFps, OB_FORMAT_Y16);
UINFO("Using color profile: %dx%d", colorWidth_, colorHeight_);
UINFO("Using depth profile: %dx%d", depthWidth_, depthHeight_);
config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE);
config->setAlignMode(ALIGN_DISABLE);
config->setDepthScaleRequire(true);
pipeline_->enableFrameSync();
try {
UINFO("Starting camera pipeline");
pipeline_->start(config);
auto enabledStreams = pipeline_->getConfig()->getEnabledStreamProfileList();
if(imuPipeline_ != nullptr) {
for(uint32_t i=0; i<enabledStreams->getCount() && !imuLocalTransformInitialized_; ++i)
{
if(enabledStreams->getProfile(i)->getType() == OB_STREAM_COLOR)
{
auto enabledImuStreams = imuPipeline_->getConfig()->getEnabledStreamProfileList();
for(uint32_t j=0; j<enabledImuStreams->getCount(); ++j)
{
if(enabledImuStreams->getProfile(j)->getType() == OB_STREAM_ACCEL)
{
auto extrinsics = enabledStreams->getProfile(i)->as<ob::VideoStreamProfile>()->getExtrinsicTo(enabledImuStreams->getProfile(j)->as<ob::AccelStreamProfile>());
// base -> color -> imu
imuLocalTransform_ = this->getLocalTransform() * obToRtabmap(extrinsics);
UINFO("IMU local transform: %s", imuLocalTransform_.prettyPrint().c_str());
imuLocalTransformInitialized_ = true;
break;
}
}
}
}
}
std::shared_ptr<ob::StreamProfile> colorProfile;
std::shared_ptr<ob::StreamProfile> depthProfile;
for(uint32_t i=0; i<enabledStreams->getCount(); ++i)
{
if(enabledStreams->getProfile(i)->getType() == OB_STREAM_COLOR) {
colorProfile = enabledStreams->getProfile(i);
}
else if(enabledStreams->getProfile(i)->getType() == OB_STREAM_DEPTH) {
depthProfile = enabledStreams->getProfile(i);
}
}
bool currentSelectionSupportsHwD2C = false;
auto hwD2CSupportedDepthStreamProfiles = pipeline_->getD2CDepthProfileList(colorProfile, ALIGN_D2C_HW_MODE);
if(hwD2CSupportedDepthStreamProfiles->count() == 0) {
UWARN("Current color profile selected doesn't support any hardware depth to color registration. Software registration is done instead.");
}
else
{
auto depthVsp = depthProfile->as<ob::VideoStreamProfile>();
auto count = hwD2CSupportedDepthStreamProfiles->getCount();
for(uint32_t i = 0; i < count; i++) {
auto vsp = hwD2CSupportedDepthStreamProfiles->getProfile(i)->as<ob::VideoStreamProfile>();
UINFO("Supported depth to color format: Resolution: %ldx%ld, FPS: %ld, Format: %d", vsp->getWidth(), vsp->getHeight(), vsp->getFps(), vsp->getFormat(), i==0?" (default)":"");
if(vsp->getWidth() == depthVsp->getWidth() && vsp->getHeight() == depthVsp->getHeight() && vsp->getFormat() == depthVsp->getFormat()
&& vsp->getFps() == depthVsp->getFps()) {
currentSelectionSupportsHwD2C = true;
}
}
}
if(!currentSelectionSupportsHwD2C) {
UWARN("Hardware depth to color registration cannot be done with the selected color and depth profiles. "
"Software registration is done instead, so more CPU will be needed on the host computer. "
"Set logger level to info to see comptible depth formats for the selected color profile.");
alignFilter_ = new ob::Align(OB_STREAM_COLOR);
alignFilter_->setMatchTargetResolution(false);
}
else {
UINFO("Enabling hardware depth to color registration!");
config->setAlignMode(ALIGN_D2C_HW_MODE);
config->setDepthScaleRequire(false);
pipeline_->stop();
pipeline_->start(config);
}
}
catch(const ob::Error & e)
{
UERROR("Configuration not supported! Exception: %s", e.what());
UERROR("Supported formats:");
for(uint32_t i=0; i<sensors->getCount(); ++i)
{
if( sensors->getSensorType(i) == OB_SENSOR_DEPTH ||
sensors->getSensorType(i) == OB_SENSOR_COLOR)
{
auto profiles = sensors->getSensor(i)->getStreamProfileList();
for(uint32_t j=0; j<profiles->getCount(); ++j)
{
auto profile = profiles->getProfile(j)->as<ob::VideoStreamProfile>();
UERROR("%sResolution: %ldx%ld, FPS: %ld, Format: %d",
sensors->getSensorType(i) == OB_SENSOR_DEPTH?"Depth":"Color",
profile->getWidth(),
profile->getHeight(),
profile->getFps(),
profile->getFormat(),
j==0?" (default)":"");
}
}
}
return false;
}
return true;
#else
UERROR("CameraOrbbecSDK: RTAB-Map is not built with Orbbec SDK support!");
return false;
#endif
}
bool CameraOrbbecSDK::isCalibrated() const
{
return true;
}
std::string CameraOrbbecSDK::getSerial() const
{
#ifdef RTABMAP_ORBBEC_SDK
if(pipeline_) {
return pipeline_->getDevice()->getDeviceInfo()->getSerialNumber();
}
#endif
return "";
}
void CameraOrbbecSDK::enableColorRectification(bool enabled)
{
#ifdef RTABMAP_ORBBEC_SDK
rectifyColor_ = enabled;
#endif
}
void CameraOrbbecSDK::enableImu(bool enabled)
{
#ifdef RTABMAP_ORBBEC_SDK
imuPublished_ = enabled;
#endif
}
void CameraOrbbecSDK::enableDepthMM(bool enabled)
{
#ifdef RTABMAP_ORBBEC_SDK
convertDepthToMM_ = enabled;
#endif
}
SensorData CameraOrbbecSDK::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_ORBBEC_SDK
if(!pipeline_) {
UERROR("Camera is not initialized!");
return data;
}
auto frameset = pipeline_->waitForFrameset();
if(frameset == nullptr || frameset->getCount() == 0) {
UWARN("No frame received!");
return data;
}
if(frameset->getCount() != 2) {
UWARN("Received %s frames, expecting 2!", frameset->getCount());
return data;
}
if(alignFilter_ != nullptr) {
// Software depth to color registration
frameset = alignFilter_->process(frameset)->as<ob::FrameSet>();
UASSERT(frameset != nullptr);
}
auto colorFrame = frameset->getFrame(OB_FRAME_COLOR);
UASSERT(colorFrame != nullptr);
auto depthFrame = frameset->getFrame(OB_FRAME_DEPTH);
UASSERT(depthFrame != nullptr);
auto colorVideoFrame = colorFrame->as<const ob::VideoFrame>();
auto depthVideoFrame = depthFrame->as<const ob::DepthFrame>();
cv::Mat rgb = obColorFrameToCv(*colorVideoFrame);
cv::Mat depth = obDepthFrameToCv(*depthVideoFrame);
if(rgb.empty()) {
UERROR("Could not convert the color frame! Type=%d Format=%d", colorFrame->getType(), colorVideoFrame->getFormat());
}
else if(depth.empty()) {
UERROR("Could not convert the depth frame! Type=%d Format=%d", depthFrame->getType(), depthVideoFrame->getFormat());
}
else if(!rgb.empty() && !depth.empty())
{
if(!model_.isValidForProjection())
{
auto streamProfile = colorFrame->getStreamProfile();
auto videoStreamProfile = streamProfile->as<ob::VideoStreamProfile>();
auto intrinsics = videoStreamProfile->getIntrinsic();
model_ = CameraModel(
getSerial(),
cv::Size(intrinsics.width, intrinsics.height),
obIntrinsicToK(intrinsics),
obDistortionToD(videoStreamProfile->getDistortion()),
cv::Mat::eye(3,3,CV_64FC1),
obIntrinsicToP(intrinsics),
this->getLocalTransform());
if(rectifyColor_ && !model_.initRectificationMap()) {
UWARN("Could not initialize rectification map, color images won't be rectified.");
}
}
if(rectifyColor_ && model_.isValidForRectification())
{
rgb = model_.rectifyImage(rgb);
}
if(convertDepthToMM_)
{
depth = util2d::cvtDepthFromFloat(depth);
}
uint64_t colorStampUs = globalTimestampAvailable_?colorFrame->getGlobalTimeStampUs():colorFrame->getTimeStampUs();
uint64_t depthStampUs = globalTimestampAvailable_?depthFrame->getGlobalTimeStampUs():depthFrame->getTimeStampUs();
double colorStamp = double(colorStampUs) / 1e6;
double depthStamp = double(depthStampUs) / 1e6;
if(fabs(colorStamp - depthStamp) > 0.018) {
// The difference seems varying between 0 and 17 ms normally
UWARN("Large timestamp difference (%fs) between color (%f) and depth (%f) frames. "
"Depth registration would be wrong on fast motion.",
colorStamp - depthStamp, colorStamp, depthStamp);
}
uint64_t stampUs = colorStampUs < depthStampUs ? colorStampUs : depthStampUs;
#ifdef WIN32
// On Windows, there is an issue that timestamps are not populated by default without following instructions from:
// https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.md
// Detect if the consecutive timestamps are identical, then send error!
if (stampUs <= lastImageStamp_)
{
UERROR("We detected non-consecutive timestamps, make sure you applied the fix from https://github.com/orbbec/OrbbecSDK_v2/blob/main/scripts/env_setup/obsensor_metadata_win10.md .");
}
lastImageStamp_ = stampUs;
#endif
double stamp = double(stampUs)/1e6;
data = SensorData(rgb, depth, model_, this->getNextSeqID(), stamp);
if(imuPublished_ && !imuBuffer_.empty() && !this->isInterIMUPublishing())
{
cv::Vec6f imuVec;
std::map<double, cv::Vec6f>::const_iterator iterA, iterB;
imuMutex_.lock();
int maximumTries = 10;
while(imuBuffer_.rbegin()->first < stamp && maximumTries-- > 0)
{
imuMutex_.unlock();
uSleep(1);
imuMutex_.lock();
}
if(imuBuffer_.rbegin()->first < stamp)
{
UWARN("Could not get IMU data at request image stamp %f after waiting 10 ms, latest imu stamp is %f", stamp, imuBuffer_.rbegin()->first);
imuMutex_.unlock();
}
else
{
// Interpolate imu data on image stamp
iterB = imuBuffer_.lower_bound(stamp);
iterA = iterB;
if(iterA != imuBuffer_.begin())
iterA = --iterA;
if(iterA == iterB || stamp == iterB->first)
{
imuVec = iterB->second;
}
else if(stamp > iterA->first && stamp < iterB->first)
{
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
imuVec = iterA->second + t*(iterB->second - iterA->second);
}
imuBuffer_.erase(imuBuffer_.begin(), iterB);
imuMutex_.unlock();
data.setIMU(IMU(cv::Vec3d(imuVec[0], imuVec[1], imuVec[2]), cv::Mat::eye(3, 3, CV_64FC1), cv::Vec3d(imuVec[3], imuVec[4], imuVec[5]), cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
}
}
}
#else
UERROR("CameraOrbbecSDK: RTAB-Map is not built with Orbbec SDK support!");
#endif
return data;
}
} // namespace rtabmap
corelib/src/camera/CameraRealSense2.cpp
+35 -26
View File
@@ -712,14 +712,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
for (auto& profile : profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string(
video_profile.format()),
video_profile.width(),
video_profile.height(),
video_profile.fps(),
video_profile.stream_index(),
video_profile.stream_name().c_str(),
video_profile.stream_type());
UINFO("%s %d %d %d %d %s type=%d",
rs2_format_to_string(profile.format()),
video_profile.get()?video_profile.width():-1,
video_profile.get()?video_profile.height():-1,
profile.fps(),
profile.stream_index(),
profile.stream_name().c_str(),
profile.stream_type());
}
}
int pi = 0;
@@ -728,7 +728,8 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
auto video_profile = profile.as<rs2::video_stream_profile>();
if(!stereo)
{
if( (video_profile.width() == cameraWidth_ &&
if( (video_profile.get() &&
video_profile.width() == cameraWidth_ &&
video_profile.height() == cameraHeight_ &&
video_profile.fps() == cameraFps_) ||
(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 &&
@@ -778,7 +779,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
}
}
else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF)
{
//D435i:
//MOTION_XYZ32F 0 0 200 (gyro)
@@ -817,6 +818,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
{
//T265:
if(!dualMode_ &&
video_profile.get() &&
video_profile.format() == RS2_FORMAT_Y8 &&
video_profile.width() == 848 &&
video_profile.height() == 800 &&
@@ -865,7 +867,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
added = true;
}
else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF)
{
//MOTION_XYZ32F 0 0 200
//MOTION_XYZ32F 0 0 62
@@ -884,14 +886,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
for (auto& profile : profiles)
{
auto video_profile = profile.as<rs2::video_stream_profile>();
UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string(
video_profile.format()),
video_profile.width(),
video_profile.height(),
video_profile.fps(),
video_profile.stream_index(),
video_profile.stream_name().c_str(),
video_profile.stream_type());
UERROR("%s %d %d %d %d %s type=%d",
rs2_format_to_string(profile.format()),
video_profile.get()?video_profile.width():-1,
video_profile.get()?video_profile.height():-1,
profile.fps(),
profile.stream_index(),
profile.stream_name().c_str(),
profile.stream_type());
}
return false;
}
@@ -1075,13 +1077,13 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
{
auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>();
UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string(
video_profile.format()),
video_profile.width(),
video_profile.height(),
video_profile.fps(),
video_profile.stream_index(),
video_profile.stream_name().c_str(),
video_profile.stream_type());
profilesPerSensor[i][j].format()),
video_profile.get()?video_profile.width():-1,
video_profile.get()?video_profile.height():-1,
profilesPerSensor[i][j].fps(),
profilesPerSensor[i][j].stream_index(),
profilesPerSensor[i][j].stream_name().c_str(),
profilesPerSensor[i][j].stream_type());
}
if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
{
@@ -1525,6 +1527,13 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
else
{
UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);
if(frameset.size()>0)
{
for (auto it = frameset.begin(); it != frameset.end(); ++it)
{
UERROR("Received frame only from %s", (*it).get_profile().stream_name().c_str());
}
}
}
}
catch(const std::exception& ex)
corelib/src/odometry/OdometryCuVSLAM.cpp
+1101
View File
File diff suppressed because it is too large Load Diff
corelib/src/odometry/OdometryF2F.cpp
+2
View File
@@ -176,6 +176,8 @@ Transform OdometryF2F::computeTransform(
if(info && this->isInfoDataFilled())
{
std::list<std::pair<int, std::pair<int, int> > > pairs;
UASSERT(tmpRefFrame.getWords().size() == tmpRefFrame.getWordsKpts().size());
UASSERT(newFrame.getWords().size() == newFrame.getWordsKpts().size());
EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
info->refCorners.resize(pairs.size());
info->newCorners.resize(pairs.size());
corelib/src/odometry/OdometryF2M.cpp
+5
View File
@@ -793,6 +793,7 @@ Transform OdometryF2M::computeTransform(
if(!lastFrameModels.empty())
{
UASSERT(lastFrame_->getWordsKpts().size() == lastFrame_->getWords().size());
for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
{
const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
@@ -1559,6 +1560,10 @@ Transform OdometryF2M::computeTransform(
{
info->reg = regInfo.copyWithoutData();
}
if(output.isNull())
{
info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0; // Lost
}
}
UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance:lin=%f, ang=%f local_map=%d local_scan_map=%d",
corelib/src/odometry/OdometryORBSLAM3.cpp
+54 -21
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UFile.h"
#include <pcl/common/transforms.h>
#include <opencv2/imgproc/types_c.h>
#include <rtabmap/core/odometry/OdometryORBSLAM3.h>
@@ -116,6 +117,13 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
}
//Load ORB Vocabulary
vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
if(!UFile::exists(vocabularyPath))
{
UERROR("ORB_SLAM vocabulary path \"%s\" doesn't exist! (Parameter name=\"%s\")",
vocabularyPath.c_str(),
rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
return false;
}
UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
// Create configuration file
@@ -240,7 +248,7 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
//# IMU Parameters TODO: hard-coded, not used
//#--------------------------------------------------------------------------------------------
// Transformation from camera 0 to body-frame (imu)
rtabmap::Transform camImuT = model1.localTransform()*imuLocalTransform_;
rtabmap::Transform camImuT = imuLocalTransform_.inverse()*model1.localTransform();
ofs << "IMU.T_b_c1: !!opencv-matrix" << std::endl;
ofs << " rows: 4" << std::endl;
ofs << " cols: 4" << std::endl;
@@ -340,14 +348,16 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
ofs.close();
ORB_SLAM3::System::eSensor sensor =
stereo?(withIMU?ORB_SLAM3::System::IMU_STEREO:ORB_SLAM3::System::STEREO):
(withIMU?ORB_SLAM3::System::IMU_RGBD:ORB_SLAM3::System::RGBD);
UINFO("Initializing ORB_SLAM3 system with sensor %d...", (int)sensor);
orbslam_ = new ORB_SLAM3::System(
vocabularyPath,
configPath,
stereo && withIMU?ORB_SLAM3::System::IMU_STEREO:
stereo?ORB_SLAM3::System::STEREO:
withIMU?ORB_SLAM3::System::IMU_RGBD:
ORB_SLAM3::System::RGBD,
sensor,
false);
UINFO("Initializing ORB_SLAM3 system with sensor %d... done!", (int)sensor);
return true;
#else
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
@@ -373,6 +383,7 @@ Transform OdometryORBSLAM3::computeTransform(
{
if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp())
{
UDEBUG("Adding IMU %f", data.stamp());
orbslamImus_.push_back(ORB_SLAM3::IMU::Point(
data.imu().linearAcceleration().val[0],
data.imu().linearAcceleration().val[1],
@@ -432,6 +443,7 @@ Transform OdometryORBSLAM3::computeTransform(
if(lastImageStamp_ == 0.0)
{
lastImageStamp_ = data.stamp();
UDEBUG("Waiting for another image to initialize...");
return t;
}
@@ -457,6 +469,7 @@ Transform OdometryORBSLAM3::computeTransform(
rightMono = cv::Mat();
cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY);
}
UDEBUG("Adding Stereo Frame %f", data.stamp());
Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_);
orbslamImus_.clear();
}
@@ -472,15 +485,22 @@ Transform OdometryORBSLAM3::computeTransform(
{
depth = util2d::cvtDepthToFloat(data.depthRaw());
}
UDEBUG("Adding RGBD Frame %f", data.stamp());
Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_);
orbslamImus_.clear();
}
Transform previousPoseInv = previousPose_.inverse();
std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetTrackedMapPoints();
if(orbslam_->isLost() || mapPoints.empty())
std::vector<ORB_SLAM3::MapPoint*> trackedMapPoints = orbslam_->GetTrackedMapPoints();
if(orbslam_->isLost() || trackedMapPoints.empty())
{
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
if(!imuLocalTransform_.isNull()) {
UWARN("ORBSLAM lost tracking! If it is on initialization, try moving the sensor in a circle for a couple of seconds.");
}
else {
UWARN("ORBSLAM lost tracking!");
}
}
else
{
@@ -490,14 +510,16 @@ Transform OdometryORBSLAM3::computeTransform(
if(!p.isNull())
{
if(!localTransform.isNull())
if(!imuLocalTransform_.isNull())
{
if(originLocalTransform_.isNull())
{
originLocalTransform_ = localTransform;
}
// transform in base frame
p = originLocalTransform_ * p.inverse() * localTransform.inverse();
// Transform p from optical-imu system (x->left, y->back and z->up) to ros system, then remove camera local transform
p = Transform(0,0,0,0,0,-M_PI/2) * p.inverse() * localTransform.inverse();
}
else
{
UASSERT(!localTransform.isNull());
// Transform p from optical system (x->right, y->down and z->forward) to ros system, then remove camera local transform
p = CameraModel::opticalRotation() * p.inverse() * localTransform.inverse();
}
t = previousPoseInv*p;
}
@@ -534,12 +556,14 @@ Transform OdometryORBSLAM3::computeTransform(
}
}
size_t mapPointsSize = 0;
if(info)
{
info->lost = t.isNull();
info->type = (int)kTypeORBSLAM;
info->reg.covariance = covariance;
info->localMapSize = mapPoints.size();
std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetAllMapPoints();
info->localMapSize = mapPointsSize = mapPoints.size();
info->localKeyFrames = 0;
if(this->isInfoDataFilled())
@@ -549,20 +573,20 @@ Transform OdometryORBSLAM3::computeTransform(
info->reg.inliersIDs.resize(kpts.size());
int oi = 0;
UASSERT(mapPoints.size() == kpts.size());
UASSERT(trackedMapPoints.size() == kpts.size());
for (unsigned int i = 0; i < kpts.size(); ++i)
{
int wordId;
if(mapPoints[i] != 0)
if(trackedMapPoints[i] != 0)
{
wordId = mapPoints[i]->mnId;
wordId = trackedMapPoints[i]->mnId;
}
else
{
wordId = -(i+1);
}
info->words.insert(std::make_pair(wordId, kpts[i]));
if(mapPoints[i] != 0)
if(trackedMapPoints[i] != 0)
{
info->reg.matchesIDs[oi] = wordId;
info->reg.inliersIDs[oi] = wordId;
@@ -574,7 +598,15 @@ Transform OdometryORBSLAM3::computeTransform(
info->reg.inliers = oi;
info->reg.matches = oi;
Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
Eigen::Affine3f fixRot;
if(!imuLocalTransform_.isNull())
{
fixRot = (this->getPose()*previousPoseInv*Transform(0,0,0,0,0,-M_PI/2)).toEigen3f();
}
else
{
fixRot = (this->getPose()*previousPoseInv*CameraModel::opticalRotation()).toEigen3f();
}
for (unsigned int i = 0; i < mapPoints.size(); ++i)
{
if(mapPoints[i])
@@ -587,7 +619,8 @@ Transform OdometryORBSLAM3::computeTransform(
}
}
UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false");
UINFO("Odom update time = %fs, tracked points=%ld, map points=%ld, lost=%s",
timer.elapsed(), trackedMapPoints.size(), mapPointsSize, t.isNull()?"true":"false");
#else
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
corelib/src/odometry/OdometryVINS.cpp
-516
View File
@@ -1,516 +0,0 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/odometry/OdometryVINS.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UThread.h"
#include "rtabmap/utilite/UDirectory.h"
#include <opencv2/imgproc/types_c.h>
#ifdef RTABMAP_VINS
#include <estimator/estimator.h>
#include <estimator/parameters.h>
#include <camodocal/camera_models/PinholeCamera.h>
#include <camodocal/camera_models/EquidistantCamera.h>
#include <utility/visualization.h>
#endif
namespace rtabmap {
#ifdef RTABMAP_VINS
class VinsEstimator: public Estimator
{
public:
VinsEstimator(
const Transform & imuLocalTransform,
const StereoCameraModel & model,
bool rectified) : Estimator()
{
MULTIPLE_THREAD = 0;
setParameter();
//overwrite camera calibration only if received model is radtan, otherwise use config
UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2);
if( (NUM_OF_CAM == 2 && model.left().D_raw().cols == 4 && model.right().D_raw().cols == 4) ||
(NUM_OF_CAM == 1 && model.left().D_raw().cols == 4))
{
UWARN("Overwriting VINS camera calibration config with received pinhole model... rectified=%d", rectified?1:0);
featureTracker.m_camera.clear();
camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
camodocal::PinholeCamera::Parameters params(
model.name(),
model.left().imageWidth(), model.left().imageHeight(),
rectified?0:model.left().D_raw().at<double>(0,0),
rectified?0:model.left().D_raw().at<double>(0,1),
rectified?0:model.left().D_raw().at<double>(0,2),
rectified?0:model.left().D_raw().at<double>(0,3),
rectified?model.left().fx():model.left().K_raw().at<double>(0,0),
rectified?model.left().fy():model.left().K_raw().at<double>(1,1),
rectified?model.left().cx():model.left().K_raw().at<double>(0,2),
rectified?model.left().cy():model.left().K_raw().at<double>(1,2));
camera->setParameters(params);
featureTracker.m_camera.push_back(camera);
if(NUM_OF_CAM == 2)
{
camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
camodocal::PinholeCamera::Parameters params(
model.name(),
model.right().imageWidth(), model.right().imageHeight(),
rectified?0:model.right().D_raw().at<double>(0,0),
rectified?0:model.right().D_raw().at<double>(0,1),
rectified?0:model.right().D_raw().at<double>(0,2),
rectified?0:model.right().D_raw().at<double>(0,3),
rectified?model.right().fx():model.right().K_raw().at<double>(0,0),
rectified?model.right().fy():model.right().K_raw().at<double>(1,1),
rectified?model.right().cx():model.right().K_raw().at<double>(0,2),
rectified?model.right().cy():model.right().K_raw().at<double>(1,2));
camera->setParameters(params);
featureTracker.m_camera.push_back(camera);
}
}
else if(rectified)
{
UWARN("Images are rectified but received calibration cannot be "
"used, make sure calibration in config file doesn't have "
"distortion or send raw images to VINS odometry.");
if(!featureTracker.m_camera.empty())
{
if(featureTracker.m_camera.front()->imageWidth() != model.left().imageWidth() ||
featureTracker.m_camera.front()->imageHeight() != model.left().imageHeight())
{
UERROR("Received images don't have same size (%dx%d) than in the config file (%dx%d)!",
model.left().imageWidth(),
model.left().imageHeight(),
featureTracker.m_camera.front()->imageWidth(),
featureTracker.m_camera.front()->imageHeight());
}
}
}
Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform();
tic[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z());
ric[0] = imuCam0.toEigen4d().block<3,3>(0,0);
if(NUM_OF_CAM == 2)
{
Transform cam0cam1;
if(rectified)
{
cam0cam1 = Transform(
1, 0, 0, model.baseline(),
0, 1, 0, 0,
0, 0, 1, 0);
}
else
{
cam0cam1 = model.stereoTransform().inverse();
}
UASSERT(!cam0cam1.isNull());
Transform imuCam1 = imuCam0 * cam0cam1;
tic[1] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z());
ric[1] = imuCam1.toEigen4d().block<3,3>(0,0);
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
cout << " exitrinsic cam " << i << endl << ric[i] << endl << tic[i].transpose() << endl;
}
f_manager.setRic(ric);
ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
td = TD;
g = G;
cout << "set g " << g.transpose() << endl;
}
// Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled.
void inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
inputImageCnt++;
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
TicToc featureTrackerTime;
if(_img1.empty())
featureFrame = featureTracker.trackImage(t, _img);
else
featureFrame = featureTracker.trackImage(t, _img, _img1);
//printf("featureTracker time: %f\n", featureTrackerTime.toc());
//if(MULTIPLE_THREAD)
//{
// if(inputImageCnt % 2 == 0)
// {
// mBuf.lock();
// featureBuf.push(make_pair(t, featureFrame));
// mBuf.unlock();
// }
//}
//else
{
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
TicToc processTime;
processMeasurements();
UDEBUG("VINS process time: %f", processTime.toc());
}
}
// Copy of original inputIMU() but with publisher commented
void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)
{
mBuf.lock();
accBuf.push(make_pair(t, linearAcceleration));
gyrBuf.push(make_pair(t, angularVelocity));
//printf("input imu with time %f \n", t);
mBuf.unlock();
fastPredictIMU(t, linearAcceleration, angularVelocity);
//if (solver_flag == NON_LINEAR)
// pubLatestOdometry(latest_P, latest_Q, latest_V, t);
}
// Copy of original processMeasurements() but with publishers commented and threading disabled
void processMeasurements()
{
//while (1)
{
//printf("process measurments\n");
pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
if(!featureBuf.empty())
{
feature = featureBuf.front();
curTime = feature.first + td;
//while(1)
//{
if (!((!USE_IMU || IMUAvailable(feature.first + td))))
//if ((!USE_IMU || IMUAvailable(feature.first + td)))
// break;
//else
{
printf("wait for imu ... \n");
//if (! MULTIPLE_THREAD)
return;
//std::chrono::milliseconds dura(5);
//std::this_thread::sleep_for(dura);
}
//}
mBuf.lock();
if(USE_IMU)
getIMUInterval(prevTime, curTime, accVector, gyrVector);
featureBuf.pop();
mBuf.unlock();
if(USE_IMU)
{
if(!initFirstPoseFlag)
initFirstIMUPose(accVector);
UDEBUG("accVector.size() = %d", accVector.size());
for(size_t i = 0; i < accVector.size(); i++)
{
double dt;
if(i == 0)
dt = accVector[i].first - prevTime;
else if (i == accVector.size() - 1)
dt = curTime - accVector[i - 1].first;
else
dt = accVector[i].first - accVector[i - 1].first;
processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
}
}
processImage(feature.second, feature.first);
prevTime = curTime;
printStatistics(*this, 0);
//std_msgs::Header header;
//header.frame_id = "world";
//header.stamp = ros::Time(feature.first);
//pubOdometry(*this, header);
//pubKeyPoses(*this, header);
//pubCameraPose(*this, header);
//pubPointCloud(*this, header);
//pubKeyframe(*this);
//pubTF(*this, header);
}
//if (! MULTIPLE_THREAD)
// break;
//std::chrono::milliseconds dura(2);
//std::this_thread::sleep_for(dura);
}
}
};
#endif
OdometryVINS::OdometryVINS(const ParametersMap & parameters) :
Odometry(parameters)
#ifdef RTABMAP_VINS
,
vinsEstimator_(0),
initGravity_(false),
previousPose_(Transform::getIdentity())
#endif
{
#ifdef RTABMAP_VINS
// intialize
std::string configFilename;
Parameters::parse(parameters, Parameters::kOdomVINSConfigPath(), configFilename);
if(configFilename.empty())
{
UERROR("VINS config file is empty (%s=%s)!",
Parameters::kOdomVINSConfigPath().c_str(),
Parameters::kOdomVINSConfigPath().c_str());
}
else
{
readParameters(uReplaceChar(configFilename, '~', UDirectory::homeDir()));
}
#endif
}
OdometryVINS::~OdometryVINS()
{
#ifdef RTABMAP_VINS
delete vinsEstimator_;
#endif
}
void OdometryVINS::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
#ifdef RTABMAP_VINS
if(!initGravity_)
{
delete vinsEstimator_;
vinsEstimator_ = 0;
previousPose_.setIdentity();
lastImu_ = IMU();
previousLocalTransform_.setNull();
}
initGravity_ = false;
#endif
}
// return not null transform if odometry is correctly computed
Transform OdometryVINS::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
Transform t;
#ifdef RTABMAP_VINS
UTimer timer;
if(USE_IMU!=0 && !data.imu().empty())
{
double t = data.stamp();
double dx = data.imu().linearAcceleration().val[0];
double dy = data.imu().linearAcceleration().val[1];
double dz = data.imu().linearAcceleration().val[2];
double rx = data.imu().angularVelocity().val[0];
double ry = data.imu().angularVelocity().val[1];
double rz = data.imu().angularVelocity().val[2];
Vector3d acc(dx, dy, dz);
Vector3d gyr(rx, ry, rz);
UDEBUG("IMU update stamp=%f", data.stamp());
if(vinsEstimator_ != 0)
{
vinsEstimator_->inputIMU(t, acc, gyr);
}
else
{
lastImu_ = data.imu();
UWARN("Waiting an image for initialization...");
}
}
if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())
{
if(USE_IMU==1 && lastImu_.localTransform().isNull())
{
UWARN("Waiting IMU for initialization...");
return t;
}
if(vinsEstimator_ == 0)
{
// intialize
vinsEstimator_ = new VinsEstimator(
lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
data.stereoCameraModels()[0],
this->imagesAlreadyRectified());
}
UDEBUG("Image update stamp=%f", data.stamp());
cv::Mat left;
cv::Mat right;
if(data.imageRaw().type() == CV_8UC3)
{
cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY);
}
else if(data.imageRaw().type() == CV_8UC1)
{
left = data.imageRaw().clone();
}
else
{
UFATAL("Not supported color type!");
}
if(data.rightRaw().type() == CV_8UC3)
{
cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
}
else if(data.rightRaw().type() == CV_8UC1)
{
right = data.rightRaw().clone();
}
else
{
UFATAL("Not supported color type!");
}
vinsEstimator_->inputImage(data.stamp(), left, right);
if(vinsEstimator_->solver_flag == Estimator::NON_LINEAR)
{
Quaterniond tmp_Q;
tmp_Q = Quaterniond(vinsEstimator_->Rs[WINDOW_SIZE]);
Transform p(
vinsEstimator_->Ps[WINDOW_SIZE].x(),
vinsEstimator_->Ps[WINDOW_SIZE].y(),
vinsEstimator_->Ps[WINDOW_SIZE].z(),
tmp_Q.x(),
tmp_Q.y(),
tmp_Q.z(),
tmp_Q.w());
if(!p.isNull())
{
if(!lastImu_.localTransform().isNull())
{
p = p * lastImu_.localTransform().inverse();
}
if(this->getPose().rotation().isIdentity())
{
initGravity_ = true;
this->reset(this->getPose()*p.rotation());
}
if(previousPose_.isIdentity())
{
previousPose_ = p;
}
// make it incremental
Transform previousPoseInv = previousPose_.inverse();
t = previousPoseInv*p;
previousPose_ = p;
if(info)
{
info->type = this->getType();
info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
// feature map
Transform fixT = this->getPose()*previousPoseInv;
for (auto &it_per_id : vinsEstimator_->f_manager.feature)
{
int used_num;
used_num = it_per_id.feature_per_frame.size();
if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1)
continue;
int imu_i = it_per_id.start_frame;
Vector3d pts_i = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1].point * it_per_id.estimated_depth;
Vector3d w_pts_i = vinsEstimator_->Rs[imu_i] * (vinsEstimator_->ric[0] * pts_i + vinsEstimator_->tic[0]) + vinsEstimator_->Ps[imu_i];
cv::Point3f p;
p.x = w_pts_i(0);
p.y = w_pts_i(1);
p.z = w_pts_i(2);
p = util3d::transformPoint(p, fixT);
info->localMap.insert(std::make_pair(it_per_id.feature_id, p));
if(this->imagesAlreadyRectified())
{
cv::Point2f pt;
data.stereoCameraModels()[0].left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y);
info->reg.inliersIDs.push_back(info->newCorners.size());
info->newCorners.push_back(pt);
}
}
info->features = info->newCorners.size();
info->localMapSize = info->localMap.size();
}
UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
}
}
else
{
UWARN("VINS not yet initialized... waiting to get enough IMU messages");
}
}
else if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
UERROR("VINS-Fusion doesn't work with RGB-D data, stereo images are required!");
}
else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty())
{
UERROR("VINS-Fusion requires stereo images!");
}
else if(data.imu().empty())
{
UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!");
}
#else
UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach.");
#endif
return t;
}
} // namespace rtabmap
corelib/src/odometry/OdometryVINSFusion.cpp
+581
View File
@@ -0,0 +1,581 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/odometry/OdometryVINSFusion.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UThread.h"
#include "rtabmap/utilite/UDirectory.h"
#include <opencv2/imgproc/types_c.h>
#ifdef RTABMAP_VINS_FUSION
#include <estimator/estimator.h>
#include <estimator/parameters.h>
#include <camodocal/camera_models/PinholeCamera.h>
#include <camodocal/camera_models/PinholeFullCamera.h>
#include <utility/visualization.h>
#endif
namespace rtabmap {
#ifdef RTABMAP_VINS_FUSION
class VinsFusionEstimator: public Estimator
{
public:
VinsFusionEstimator() : Estimator()
{}
bool init(const Transform & imuLocalTransform,
const StereoCameraModel & model,
bool rectified)
{
MULTIPLE_THREAD = 0;
setParameter();
ROW=model.left().imageHeight();
COL=model.left().imageWidth();
//overwrite camera calibration only if received model is radtan, otherwise use config
UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2);
if( (NUM_OF_CAM == 2 && (rectified || (model.left().D_raw().cols >= 4 && model.right().D_raw().cols >= 4))) ||
(NUM_OF_CAM == 1 && (rectified || model.left().D_raw().cols >= 4)))
{
UINFO("Setting up VINS camera calibration config with received pinhole model... rectified=%d distortion coefficients=%d",
rectified?1:0, model.left().D_raw().cols);
featureTracker.m_camera.clear();
double fx = 0.0;
if(!rectified && model.left().D_raw().cols >= 8)
{
if(model.left().D_raw().cols > 8)
{
UWARN("Received %d distortion coefficients, but only the first 8 are supported, ignoring the last coefficents.",
model.left().D_raw().cols);
}
camodocal::PinholeFullCameraPtr camera( new camodocal::PinholeFullCamera );
camodocal::PinholeFullCamera::Parameters params(
model.name(),
model.left().imageWidth(), model.left().imageHeight(),
model.left().D_raw().at<double>(0,0), // k1
model.left().D_raw().at<double>(0,1), // k1
model.left().D_raw().at<double>(0,4), // k3
model.left().D_raw().at<double>(0,5), // k4
model.left().D_raw().at<double>(0,6), // k5
model.left().D_raw().at<double>(0,7), // k6
model.left().D_raw().at<double>(0,2), // p1
model.left().D_raw().at<double>(0,3), // p1
model.left().K_raw().at<double>(0,0), // fx
model.left().K_raw().at<double>(1,1), // fy
model.left().K_raw().at<double>(0,2), // cx
model.left().K_raw().at<double>(1,2)); // cy
camera->setParameters(params);
featureTracker.m_camera.push_back(camera);
fx = params.fx();
if(NUM_OF_CAM == 2)
{
UASSERT(model.left().D_raw().cols == model.right().D_raw().cols);
camodocal::PinholeFullCameraPtr camera2( new camodocal::PinholeFullCamera );
camodocal::PinholeFullCamera::Parameters params2(
model.name(),
model.right().imageWidth(), model.right().imageHeight(),
model.right().D_raw().at<double>(0,0), // k1
model.right().D_raw().at<double>(0,1), // k2
model.right().D_raw().at<double>(0,4), // k3
model.right().D_raw().at<double>(0,5), // k4
model.right().D_raw().at<double>(0,6), // k5
model.right().D_raw().at<double>(0,7), // k6
model.right().D_raw().at<double>(0,2), // p1
model.right().D_raw().at<double>(0,3), // p2
model.right().K_raw().at<double>(0,0), // fx
model.right().K_raw().at<double>(1,1), // fy
model.right().K_raw().at<double>(0,2), // cx
model.right().K_raw().at<double>(1,2)); // cy
camera2->setParameters(params2);
featureTracker.m_camera.push_back(camera2);
}
}
else
{
if(!rectified)
{
if(model.left().D_raw().cols == 6) {
UERROR("Fisheye camera model support not implemented! Provide rectified images instead (see %s).",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
return false;
}
if(model.left().D_raw().cols > 4)
{
UWARN("Received %d distortion coefficients, but only 4 or 8 are supported, ignoring the last coefficents.",
model.left().D_raw().cols);
}
}
camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
camodocal::PinholeCamera::Parameters params(
model.name(),
model.left().imageWidth(), model.left().imageHeight(),
rectified?0:model.left().D_raw().at<double>(0,0), // k1
rectified?0:model.left().D_raw().at<double>(0,1), // k2
rectified?0:model.left().D_raw().at<double>(0,2), // p1
rectified?0:model.left().D_raw().at<double>(0,3), // p2
rectified?model.left().fx():model.left().K_raw().at<double>(0,0),
rectified?model.left().fy():model.left().K_raw().at<double>(1,1),
rectified?model.left().cx():model.left().K_raw().at<double>(0,2),
rectified?model.left().cy():model.left().K_raw().at<double>(1,2));
camera->setParameters(params);
featureTracker.m_camera.push_back(camera);
fx = params.fx();
if(NUM_OF_CAM == 2)
{
UASSERT(model.left().D_raw().cols == model.right().D_raw().cols);
camodocal::PinholeCameraPtr camera2( new camodocal::PinholeCamera );
camodocal::PinholeCamera::Parameters params2(
model.name(),
model.right().imageWidth(), model.right().imageHeight(),
rectified?0:model.right().D_raw().at<double>(0,0), // k1
rectified?0:model.right().D_raw().at<double>(0,1), // k2
rectified?0:model.right().D_raw().at<double>(0,2), // p1
rectified?0:model.right().D_raw().at<double>(0,3), // p2
rectified?model.right().fx():model.right().K_raw().at<double>(0,0),
rectified?model.right().fy():model.right().K_raw().at<double>(1,1),
rectified?model.right().cx():model.right().K_raw().at<double>(0,2),
rectified?model.right().cy():model.right().K_raw().at<double>(1,2));
camera2->setParameters(params2);
featureTracker.m_camera.push_back(camera2);
}
}
double originalParalax = MIN_PARALLAX * FOCAL_LENGTH;
// If you have compiler error about FOCAL_LENGTH being const, make sure to use the following patch for ROS1:
// https://gist.github.com/matlabbe/795ab37067367dca58bbadd8201d986c#file-vins-fusion_pull136-patch
// Use this patch for ROS2: https://gist.github.com/matlabbe/ebbb343cd744da9d6d6d6ded2e1557fd
FOCAL_LENGTH = fx;
MIN_PARALLAX = originalParalax / FOCAL_LENGTH;
ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
}
else
{
UERROR("Received stereo camera model is not compatible with VINS-Fusion.");
if(!rectified && model.left().D_raw().cols != 4) {
UERROR("When raw images are provided (%s=false), we expect 4 distortion coefficients (k1,k2,p1,p2), received %d",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
model.left().D_raw().cols);
}
return false;
}
Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform();
tic[0] = TIC[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z());
ric[0] = RIC[0] = imuCam0.toEigen4d().block<3,3>(0,0);
if(NUM_OF_CAM == 2)
{
Transform cam0cam1;
if(rectified)
{
cam0cam1 = Transform(
1, 0, 0, model.baseline(),
0, 1, 0, 0,
0, 0, 1, 0);
}
else
{
cam0cam1 = model.stereoTransform().inverse();
}
UASSERT(!cam0cam1.isNull());
Transform imuCam1 = imuCam0 * cam0cam1;
tic[1] = TIC[0] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z());
ric[1] = RIC[0] = imuCam1.toEigen4d().block<3,3>(0,0);
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
cout << " new extrinsic cam " << i << endl << ric[i] << endl << tic[i].transpose() << endl;
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
cout << " new intrinsic cam " << i << endl << featureTracker.m_camera[i]->parametersToString() << endl;
}
f_manager.setRic(ric);
return true;
}
// Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled.
void inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
TicToc processTime;
inputImageCnt++;
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
if(_img1.empty()) {
featureFrame = featureTracker.trackImage(t, _img);
}
else {
featureFrame = featureTracker.trackImage(t, _img, _img1);
}
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
processMeasurements();
UDEBUG("VINS process time: %f", processTime.toc());
}
// Copy of original inputIMU() but with publisher commented
void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)
{
mBuf.lock();
accBuf.push(make_pair(t, linearAcceleration));
gyrBuf.push(make_pair(t, angularVelocity));
//printf("input imu with time %f \n", t);
mBuf.unlock();
if (solver_flag == NON_LINEAR)
{
mPropagate.lock();
fastPredictIMU(t, linearAcceleration, angularVelocity);
mPropagate.unlock();
}
}
// Copy of original processMeasurements() but with publishers commented and threading disabled
void processMeasurements()
{
pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
if(!featureBuf.empty())
{
feature = featureBuf.front();
curTime = feature.first + td;
if (USE_IMU && !IMUAvailable(feature.first + td))
{
printf("wait for imu ... \n");
return;
}
mBuf.lock();
if(USE_IMU)
getIMUInterval(prevTime, curTime, accVector, gyrVector);
featureBuf.pop();
mBuf.unlock();
if(USE_IMU)
{
if(!initFirstPoseFlag)
initFirstIMUPose(accVector);
for(size_t i = 0; i < accVector.size(); i++)
{
double dt;
if(i == 0)
dt = accVector[i].first - prevTime;
else if (i == accVector.size() - 1)
dt = curTime - accVector[i - 1].first;
else
dt = accVector[i].first - accVector[i - 1].first;
processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
}
}
mProcess.lock();
processImage(feature.second, feature.first);
prevTime = curTime;
mProcess.unlock();
}
}
};
#endif
OdometryVINSFusion::OdometryVINSFusion(const ParametersMap & parameters) :
Odometry(parameters)
#ifdef RTABMAP_VINS_FUSION
,
vinsEstimator_(0),
initGravity_(false),
previousPose_(Transform::getIdentity())
#endif
{
#ifdef RTABMAP_VINS_FUSION
// intialize
std::string configFilename;
Parameters::parse(parameters, Parameters::kOdomVINSFusionConfigPath(), configFilename);
if(configFilename.empty())
{
UERROR("VINS config file is empty (%s)!",
Parameters::kOdomVINSFusionConfigPath().c_str());
}
else
{
UINFO("Using config file %s", configFilename.c_str());
readParameters(uReplaceChar(configFilename, '~', UDirectory::homeDir()));
}
#endif
}
OdometryVINSFusion::~OdometryVINSFusion()
{
#ifdef RTABMAP_VINS_FUSION
delete vinsEstimator_;
#endif
}
void OdometryVINSFusion::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
#ifdef RTABMAP_VINS_FUSION
if(!initGravity_)
{
delete vinsEstimator_;
vinsEstimator_ = 0;
previousPose_.setIdentity();
lastImu_ = IMU();
previousLocalTransform_.setNull();
}
initGravity_ = false;
#endif
}
// return not null transform if odometry is correctly computed
Transform OdometryVINSFusion::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
Transform t;
#ifdef RTABMAP_VINS_FUSION
UTimer timer;
bool hasImage = !data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection();
if(USE_IMU!=0 && !data.imu().empty())
{
double dx = data.imu().linearAcceleration().val[0];
double dy = data.imu().linearAcceleration().val[1];
double dz = data.imu().linearAcceleration().val[2];
double rx = data.imu().angularVelocity().val[0];
double ry = data.imu().angularVelocity().val[1];
double rz = data.imu().angularVelocity().val[2];
Vector3d acc(dx, dy, dz);
Vector3d gyr(rx, ry, rz);
UDEBUG("IMU update stamp=%f", data.stamp());
if(vinsEstimator_ != 0)
{
vinsEstimator_->inputIMU(data.stamp(), acc, gyr);
}
else
{
lastImu_ = data.imu();
lastImuStamp_ = data.stamp();
if(!hasImage) {
UWARN("Waiting an image for initialization...");
}
}
}
if(hasImage)
{
if(USE_IMU==1 && lastImu_.localTransform().isNull())
{
UWARN("Waiting IMU for initialization...");
return t;
}
if(vinsEstimator_ == 0)
{
// intialize
UINFO("Initializing with image %f", data.stamp());
vinsEstimator_ = new VinsFusionEstimator();
if(!vinsEstimator_->init(
lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
data.stereoCameraModels()[0],
this->imagesAlreadyRectified()))
{
delete vinsEstimator_;
vinsEstimator_ = 0;
return Transform();
}
if(USE_IMU) {
double dx = lastImu_.linearAcceleration().val[0];
double dy = lastImu_.linearAcceleration().val[1];
double dz = lastImu_.linearAcceleration().val[2];
double rx = lastImu_.angularVelocity().val[0];
double ry = lastImu_.angularVelocity().val[1];
double rz = lastImu_.angularVelocity().val[2];
Vector3d acc(dx, dy, dz);
Vector3d gyr(rx, ry, rz);
vinsEstimator_->inputIMU(lastImuStamp_, acc, gyr);
}
}
UDEBUG("Image update stamp=%f", data.stamp());
cv::Mat left;
cv::Mat right;
if(data.imageRaw().type() == CV_8UC3)
{
cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY);
}
else if(data.imageRaw().type() == CV_8UC1)
{
left = data.imageRaw().clone();
}
else
{
UFATAL("Not supported color type!");
}
if(data.rightRaw().type() == CV_8UC3)
{
cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
}
else if(data.rightRaw().type() == CV_8UC1)
{
right = data.rightRaw().clone();
}
else
{
UFATAL("Not supported color type!");
}
vinsEstimator_->inputImage(data.stamp(), left, right);
if(vinsEstimator_->solver_flag == Estimator::NON_LINEAR)
{
Quaterniond tmp_Q;
tmp_Q = Quaterniond(vinsEstimator_->Rs[WINDOW_SIZE]);
Transform p(
vinsEstimator_->Ps[WINDOW_SIZE].x(),
vinsEstimator_->Ps[WINDOW_SIZE].y(),
vinsEstimator_->Ps[WINDOW_SIZE].z(),
tmp_Q.x(),
tmp_Q.y(),
tmp_Q.z(),
tmp_Q.w());
if(!p.isNull())
{
if(!lastImu_.localTransform().isNull())
{
p = p * lastImu_.localTransform().inverse();
}
if(this->getPose().rotation().isIdentity())
{
initGravity_ = true;
this->reset(this->getPose()*p.rotation());
}
if(previousPose_.isIdentity())
{
previousPose_ = p;
}
// make it incremental
Transform previousPoseInv = previousPose_.inverse();
t = previousPoseInv*p;
previousPose_ = p;
if(info)
{
info->type = this->getType();
info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
// feature map: based on code from pubPointCloud() of vins's visualization.cpp
for (auto &it_per_id : vinsEstimator_->f_manager.feature)
{
if(it_per_id.feature_per_frame.size() < 2) {
// feature just added but not tracked, or old feature not tracked anymore
continue;
}
int imu_i = it_per_id.start_frame;
Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth;
Vector3d w_pts_i = vinsEstimator_->Rs[imu_i] * (vinsEstimator_->ric[0] * pts_i + vinsEstimator_->tic[0]) + vinsEstimator_->Ps[imu_i];
cv::Point3f p;
p.x = w_pts_i(0);
p.y = w_pts_i(1);
p.z = w_pts_i(2);
int featureIndex = info->localMap.size();
info->localMap.insert(std::make_pair(featureIndex, p));
FeaturePerFrame & refFrame = it_per_id.feature_per_frame[0]; // First frame it was seen
FeaturePerFrame & newFrame = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1]; // Last frame it was seen (not necessary in last frame)
cv::Point2f refUV(refFrame.uv[0], refFrame.uv[1]);
cv::Point2f newUV(newFrame.uv[0], newFrame.uv[1]);
info->refCorners.push_back(refUV);
info->newCorners.push_back(newUV);
info->reg.matchesIDs.push_back(featureIndex);
if(it_per_id.solve_flag > 0) {
// Feature correctly tracked
info->words.insert(std::make_pair(featureIndex, cv::KeyPoint(newUV, 3.0f)));
info->cornerInliers.push_back(featureIndex);
info->reg.inliersIDs.push_back(featureIndex);
}
++featureIndex;
}
info->features = info->localMap.size();
info->reg.inliers = info->reg.inliersIDs.size();
info->localMapSize = info->localMap.size();
}
UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
}
}
else
{
UWARN("VINS-Fusion not yet initialized... needing more data.");
}
}
else if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
UERROR("VINS-Fusion doesn't work with RGB-D data, stereo images are required!");
}
else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty())
{
UERROR("VINS-Fusion requires stereo images!");
}
else if(data.imu().empty())
{
UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!");
}
#else
UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach.");
#endif
return t;
}
} // namespace rtabmap
corelib/src/optimizer/OptimizerG2O.cpp
+14 -12
View File
@@ -90,14 +90,10 @@ typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSpa
typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
#endif
// We use G2O_SRC_DIR to know we are version after December 24 2020
// We check if g2o/types/sba/sba_utils.h exists to know we use a version after December 24 2020
// where VertexSBAPointXYZ has been renamed to VertexPointXYZ
// (g2o: 0fcccb302787e70ff19f65e70fb103a1295b33a2)
//
// VCPKG commented G2O_SRC_DIR from their port so we cannot use
// G2O_SRC_DIR on windows to deduce it, we then assume it is the
// latest version without VertexSBAPointXYZ
#if defined(G2O_SRC_DIR) or defined(WIN32)
#ifdef RTABMAP_G2O_WITH_SBA_UTILS
namespace g2o {
typedef VertexPointXYZ VertexSBAPointXYZ;
}
@@ -220,7 +216,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_G2O
UDEBUG("Optimizing graph...");
UDEBUG("Optimizing graph... (rootId=%d)", rootId);
#ifndef RTABMAP_VERTIGO
if(this->isRobust())
@@ -352,6 +348,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
{
if(rootId!=0) {
UDEBUG("Removed rootId=%d because there are priors.");
}
rootId = 0;
break;
}
@@ -594,7 +593,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
auto pose = g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
v1->setEstimate(pose); // This will help g2o to converge faster (https://github.com/introlab/rtabmap_ros/issues/1371)
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
@@ -675,6 +676,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
v1->setEstimate(pose); // This will help g2o to converge faster (https://github.com/introlab/rtabmap_ros/issues/1371)
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
@@ -1005,8 +1007,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
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);
UASSERT_MSG(v1 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str());
UASSERT_MSG(v2 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str());
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);
@@ -1173,7 +1175,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
{
UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
UERROR("g2o: Large optimization error detected (%f), aborting optimization!");
return optimizedPoses;
}
@@ -1222,7 +1224,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(optimizer.activeRobustChi2() > 1000000000000.0)
{
UERROR("g2o: Large optimimzation error detected (%f), aborting optimization!");
UERROR("g2o: Large optimization error detected (%f), aborting optimization!");
return optimizedPoses;
}
corelib/src/optimizer/gtsam/GravityFactor.h
+3
View File
@@ -25,6 +25,9 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#if GTSAM_VERSION_NUMERIC >= 40300 && defined(GTSAM_WITH_NOISE_MODEL_FACTOR_N)
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#endif
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
corelib/src/python/PyDetector.cpp
+242 -230
View File
@@ -1,230 +1,242 @@
/**
* Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
*/
#include "PyDetector.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <pybind11/embed.h>
#define NPY_NO_DEPRECATED_API NPY_API_VERSION
#include <numpy/arrayobject.h>
namespace rtabmap
{
PyDetector::PyDetector(const ParametersMap & parameters) :
pModule_(0),
pFunc_(0),
path_(Parameters::defaultPyDetectorPath()),
cuda_(Parameters::defaultPyDetectorCuda())
{
this->parseParameters(parameters);
UDEBUG("path = %s", path_.c_str());
if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
{
UERROR("Cannot initialize Python detector, the path is not valid: \"%s\"=\"%s\"",
Parameters::kPyDetectorPath().c_str(), path_.c_str());
return;
}
pybind11::gil_scoped_acquire acquire;
std::string matcherPythonDir = UDirectory::getDir(path_);
if(!matcherPythonDir.empty())
{
PyRun_SimpleString("import sys");
PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
}
_import_array();
std::string scriptName = uSplit(UFile::getName(path_), '.').front();
PyObject * pName = PyUnicode_FromString(scriptName.c_str());
UDEBUG("PyImport_Import() beg");
pModule_ = PyImport_Import(pName);
UDEBUG("PyImport_Import() end");
Py_DECREF(pName);
if(!pModule_)
{
UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
UERROR("%s", getPythonTraceback().c_str());
}
}
PyDetector::~PyDetector()
{
pybind11::gil_scoped_acquire acquire;
if(pFunc_)
{
Py_DECREF(pFunc_);
}
if(pModule_)
{
Py_DECREF(pModule_);
}
}
void PyDetector::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_);
Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_);
path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
}
std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
UDEBUG("");
descriptors_ = cv::Mat();
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
std::vector<cv::KeyPoint> keypoints;
cv::Mat imgRoi(image, roi);
UTimer timer;
if(!pModule_)
{
UERROR("Python detector module not loaded!");
return keypoints;
}
pybind11::gil_scoped_acquire acquire;
if(!pFunc_)
{
PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
if(pFunc)
{
if(PyCallable_Check(pFunc))
{
PyObject * result = PyObject_CallFunction(pFunc, "i", cuda_?1:0);
if(result == NULL)
{
UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
UERROR("%s", getPythonTraceback().c_str());
return keypoints;
}
Py_DECREF(result);
pFunc_ = PyObject_GetAttrString(pModule_, "detect");
if(pFunc_ && PyCallable_Check(pFunc_))
{
// we are ready!
}
else
{
UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str());
UERROR("%s", getPythonTraceback().c_str());
if(pFunc_)
{
Py_DECREF(pFunc_);
pFunc_ = 0;
}
return keypoints;
}
}
else
{
UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
UERROR("%s", getPythonTraceback().c_str());
return keypoints;
}
Py_DECREF(pFunc);
}
else
{
UERROR("Cannot find method \"init(...)\"");
UERROR("%s", getPythonTraceback().c_str());
return keypoints;
}
UDEBUG("init time = %fs", timer.ticks());
}
if(pFunc_)
{
npy_intp dims[2] = {imgRoi.rows, imgRoi.cols};
PyObject* pImageBuffer = PyArray_SimpleNewFromData(2, dims, NPY_UBYTE, (void*)imgRoi.data);
UASSERT(pImageBuffer);
UDEBUG("Preparing data time = %fs", timer.ticks());
PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageBuffer, NULL);
if(pReturn == NULL)
{
UERROR("Failed to call match() function!");
UERROR("%s", getPythonTraceback().c_str());
}
else
{
UDEBUG("Python detector time = %fs", timer.ticks());
if (PyTuple_Check(pReturn) && PyTuple_GET_SIZE(pReturn) == 2)
{
PyObject *kptsPtr = PyTuple_GET_ITEM(pReturn, 0);
PyObject *descPtr = PyTuple_GET_ITEM(pReturn, 1);
if(PyArray_Check(kptsPtr) && PyArray_Check(descPtr))
{
PyArrayObject *arrayPtr = reinterpret_cast<PyArrayObject*>(kptsPtr);
int nKpts = PyArray_SHAPE(arrayPtr)[0];
int kptSize = PyArray_SHAPE(arrayPtr)[1];
int type = PyArray_TYPE(arrayPtr);
UDEBUG("Kpts array %dx%d (type=%d)", nKpts, kptSize, type);
UASSERT(kptSize == 3);
UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
float* c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
keypoints.reserve(nKpts);
for (int i = 0; i < nKpts*kptSize; i+=kptSize)
{
cv::KeyPoint kpt(c_out[i], c_out[i+1], 8, -1, c_out[i+2]);
keypoints.push_back(kpt);
}
arrayPtr = reinterpret_cast<PyArrayObject*>(descPtr);
int nDesc = PyArray_SHAPE(arrayPtr)[0];
UASSERT(nDesc = nKpts);
int dim = PyArray_SHAPE(arrayPtr)[1];
type = PyArray_TYPE(arrayPtr);
UDEBUG("Desc array %dx%d (type=%d)", nDesc, dim, type);
UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
for (int i = 0; i < nDesc*dim; i+=dim)
{
cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
descriptors_.push_back(descriptor);
}
}
}
else
{
UWARN("Expected tuple (Kpts 3 x N, Descriptors dim x N), returning empty features.");
}
Py_DECREF(pReturn);
}
Py_DECREF(pImageBuffer);
}
return keypoints;
}
cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT((int)keypoints.size() == descriptors_.rows);
return descriptors_;
}
}
/**
* Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
*/
#include "PyDetector.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <pybind11/embed.h>
#define NPY_NO_DEPRECATED_API NPY_API_VERSION
#include <numpy/arrayobject.h>
namespace rtabmap
{
PyDetector::PyDetector(const ParametersMap & parameters) :
pModule_(0),
pFunc_(0),
path_(Parameters::defaultPyDetectorPath()),
cuda_(Parameters::defaultPyDetectorCuda())
{
this->parseParameters(parameters);
UDEBUG("path = %s", path_.c_str());
if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
{
UERROR("Cannot initialize Python detector, the path is not valid: \"%s\"=\"%s\"",
Parameters::kPyDetectorPath().c_str(), path_.c_str());
return;
}
pybind11::gil_scoped_acquire acquire;
std::string matcherPythonDir = UDirectory::getDir(path_);
if(!matcherPythonDir.empty())
{
PyRun_SimpleString("import sys");
PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
}
_import_array();
std::string scriptName = uSplit(UFile::getName(path_), '.').front();
PyObject * pName = PyUnicode_FromString(scriptName.c_str());
UDEBUG("PyImport_Import() beg");
pModule_ = PyImport_Import(pName);
UDEBUG("PyImport_Import() end");
Py_DECREF(pName);
if(!pModule_)
{
UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
UERROR("%s", getPythonTraceback().c_str());
}
}
PyDetector::~PyDetector()
{
pybind11::gil_scoped_acquire acquire;
if(pFunc_)
{
Py_DECREF(pFunc_);
}
if(pModule_)
{
Py_DECREF(pModule_);
}
}
void PyDetector::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_);
Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_);
path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
}
std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
UDEBUG("");
descriptors_ = cv::Mat();
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
std::vector<cv::KeyPoint> keypoints;
cv::Mat imgRoi(image, roi);
UTimer timer;
if(!pModule_)
{
UERROR("Python detector module not loaded!");
return keypoints;
}
pybind11::gil_scoped_acquire acquire;
if(!pFunc_)
{
PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
if(pFunc)
{
if(PyCallable_Check(pFunc))
{
PyObject * result = PyObject_CallFunction(pFunc, "i", cuda_?1:0);
if(result == NULL)
{
UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
UERROR("%s", getPythonTraceback().c_str());
return keypoints;
}
Py_DECREF(result);
pFunc_ = PyObject_GetAttrString(pModule_, "detect");
if(pFunc_ && PyCallable_Check(pFunc_))
{
// we are ready!
}
else
{
UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str());
UERROR("%s", getPythonTraceback().c_str());
if(pFunc_)
{
Py_DECREF(pFunc_);
pFunc_ = 0;
}
return keypoints;
}
}
else
{
UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
UERROR("%s", getPythonTraceback().c_str());
return keypoints;
}
Py_DECREF(pFunc);
}
else
{
UERROR("Cannot find method \"init(...)\"");
UERROR("%s", getPythonTraceback().c_str());
return keypoints;
}
UDEBUG("init time = %fs", timer.ticks());
}
if(pFunc_)
{
npy_intp dims[2] = {imgRoi.rows, imgRoi.cols};
PyObject * pImageBuffer = PyArray_SimpleNewFromData(2, dims, NPY_UBYTE, (void*)imgRoi.data);
UASSERT(pImageBuffer);
UDEBUG("Preparing data time = %fs", timer.ticks());
PyObject * pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageBuffer, NULL);
if(pReturn == NULL)
{
UERROR("Failed to call match() function!");
UERROR("%s", getPythonTraceback().c_str());
}
else
{
UDEBUG("Python detector time = %fs", timer.ticks());
if (PyTuple_Check(pReturn) && PyTuple_GET_SIZE(pReturn) == 2)
{
PyObject * kptsPtr = PyTuple_GET_ITEM(pReturn, 0);
PyObject * descPtr = PyTuple_GET_ITEM(pReturn, 1);
if(PyArray_Check(kptsPtr) && PyArray_Check(descPtr))
{
PyArrayObject *arrayPtr = reinterpret_cast<PyArrayObject*>(kptsPtr);
int nKpts = PyArray_SHAPE(arrayPtr)[0];
int kptSize = PyArray_SHAPE(arrayPtr)[1];
int type = PyArray_TYPE(arrayPtr);
UDEBUG("Kpts array %dx%d (type=%d)", nKpts, kptSize, type);
UASSERT(kptSize == 3);
UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
float* c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
std::vector<bool> keep_kpt(nKpts);
keypoints.reserve(nKpts);
for (int i = 0, kpt_idx = 0; i < nKpts*kptSize; i+=kptSize, kpt_idx++)
{
// x,y in full image coordinates. Mask is in full image coordinates too.
int full_x = (int)(c_out[i] + roi.x);
int full_y = (int)(c_out[i+1] + roi.y);
keep_kpt[kpt_idx] = mask.empty() || (full_x >= 0 && full_x < mask.cols && full_y >= 0 && full_y < mask.rows && mask.at<unsigned char>(full_y, full_x) != 0);
if(keep_kpt[kpt_idx]) {
cv::KeyPoint kpt(c_out[i], c_out[i+1], 8, -1, c_out[i+2]);
keypoints.push_back(kpt);
}
}
arrayPtr = reinterpret_cast<PyArrayObject*>(descPtr);
int nDesc = PyArray_SHAPE(arrayPtr)[0];
UASSERT(nDesc = nKpts);
int dim = PyArray_SHAPE(arrayPtr)[1];
type = PyArray_TYPE(arrayPtr);
UDEBUG("Desc array %dx%d (type=%d)", nDesc, dim, type);
UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
for (int i = 0, kpt_idx = 0; i < nDesc*dim; i+=dim, kpt_idx++)
{
if(keep_kpt[kpt_idx]) {
cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
descriptors_.push_back(descriptor);
}
}
}
}
else
{
UWARN("Expected tuple (Kpts 3 x N, Descriptors dim x N), returning empty features.");
}
Py_DECREF(pReturn);
}
Py_DECREF(pImageBuffer);
}
// Apply limitKeypoints to enforce maxFeatures and SSC
this->limitKeypoints(keypoints, descriptors_, this->getMaxFeatures(), cv::Size(roi.width, roi.height), this->getSSC());
return keypoints;
}
cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT((int)keypoints.size() == descriptors_.rows);
return descriptors_;
}
}
corelib/src/python/rtabmap_superpoint_rpautrat.py
+71
View File
@@ -0,0 +1,71 @@
#! /usr/bin/env python3
#
# Drop this file in the root folder of SuperPoint git: https://github.com/rpautrat/SuperPoint
# To use with rtabmap:
# --Vis/FeatureType 15 --Kp/DetectorStrategy 15 --PyDetector/Path "~/SuperPoint/rtabmap_superpoint_rpautrat.py"
#
import numpy as np
import os
import torch
from superpoint_pytorch import SuperPoint
superpoint = []
device = 'cpu'
def init(cuda):
global superpoint, device
superpoint = SuperPoint().eval()
# set up device, gpu or cpu depending on the availability and the user's choice
device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu'
# Load weights directly to target device
# Get the directory where this script is located
script_dir = os.path.dirname(os.path.abspath(__file__))
weights_path = os.path.join(script_dir, 'weights', 'superpoint_v6_from_tf.pth')
# Load model weights with proper error handling
try:
state_dict = torch.load(weights_path, map_location=device, weights_only=True)
superpoint.load_state_dict(state_dict)
except Exception as e:
print(f"Error loading weights: {e}")
raise
# Move the model to the target device
superpoint.to(device)
# Ensure model is in eval mode for inference
superpoint.eval()
def detect(imageBuffer):
global superpoint, device
image = np.asarray(imageBuffer)
image = (image.astype('float32') / 255.)
try:
image_with_dims = image[None, None] # Add batch and channel dims
image_tensor = torch.from_numpy(image_with_dims).float()
image_tensor = image_tensor.to(device)
except Exception as e:
print(f"Error creating tensor: {e}")
raise
# Result: (1, 1, H, W) - PyTorch tensor on correct device (CPU or GPU).
with torch.no_grad():
pred = superpoint({'image': image_tensor})
# Extract keypoints and descriptors
keypoints = pred['keypoints'][0].cpu().numpy() # Shape: (N, 2)
keypoints_response = pred['keypoint_scores'][0].cpu().numpy()
keypoints_with_response = np.column_stack([keypoints, keypoints_response]).astype(np.float32)
# Result: (N, 3) with [x, y, response]
descriptors = pred['descriptors'][0].cpu().numpy()
# Result: (N, descriptor_dim)
desc = np.float32(descriptors).copy()
pts = np.float32(keypoints_with_response).copy()
return pts, desc
corelib/src/resources/DatabaseSchema.sql.in
+3 -1
View File
@@ -131,7 +131,9 @@ CREATE TABLE Admin (
opt_map BLOB, -- compressed CV_8SC1 occupancy grid
opt_map_x_min FLOAT,
opt_map_y_min FLOAT,
opt_map_resolution FLOAT,
opt_map_resolution FLOAT,
dictionary_index BLOB, -- serialized dictionary index
time_enter DATE
);
corelib/src/resources/backward_compatibility/DatabaseSchema_0_22_0.sql
+183
View File
@@ -0,0 +1,183 @@
-- *******************************************************************
-- DatabaseSchema: Script for creating the database
-- Usage:
-- $ sqlite3 LTM.db < DatabaseSchema.sql
--
-- *******************************************************************
-- *******************************************************************
-- CLEAN
-- *******************************************************************
/*DROP TABLE Node;*/
-- *******************************************************************
-- CREATE
-- *******************************************************************
CREATE TABLE Node (
id INTEGER NOT NULL,
map_id INTEGER NOT NULL,
weight INTEGER,
stamp FLOAT,
pose BLOB, -- 3x4 float
ground_truth_pose BLOB, -- 3x4 float
velocity BLOB, -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
label TEXT,
gps BLOB, -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
env_sensors BLOB, -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Data (
id INTEGER NOT NULL,
image BLOB, -- compressed image (Grayscale or RGB)
depth BLOB, -- compressed image (Depth or Right image)
depth_confidence BLOB, -- compressed data (low=0 high=100)
calibration BLOB, -- fx, fy, cx, cy, [baseline,] width, height, local_transform
scan BLOB, -- compressed data (Laser scan)
scan_info BLOB, -- scan_max_pts, scan_max_range, scan_format, local_transform
ground_cells BLOB, -- compressed data (occupancy grid)
obstacle_cells BLOB, -- compressed data (occupancy grid)
empty_cells BLOB, -- compressed data (occupancy grid)
cell_size FLOAT,
view_point_x FLOAT,
view_point_y FLOAT,
view_point_z FLOAT,
user_data BLOB, -- compressed data (User data)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- kNeighbor=0, kGlobalClosure=1, kLocalSpaceClosure=2, kLocalTimeClosure=3, kUserClosure=4, kVirtualClosure=5, kNeighborMerged=6, kPosePrior=7, kLandmark=8
information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
transform BLOB, -- 3x4 float
user_data BLOB, -- compressed data (User data)
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
);
--
CREATE TABLE Word (
id INTEGER NOT NULL,
descriptor_size INTEGER NOT NULL,
descriptor BLOB NOT NULL,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Feature (
node_id INTEGER NOT NULL,
word_id INTEGER NOT NULL,
pos_x FLOAT NOT NULL,
pos_y FLOAT NOT NULL,
size INTEGER NOT NULL,
dir FLOAT NOT NULL,
response FLOAT NOT NULL,
octave INTEGER NOT NULL,
depth_x FLOAT,
depth_y FLOAT,
depth_z FLOAT,
descriptor_size INTEGER,
descriptor BLOB,
FOREIGN KEY (node_id) REFERENCES Node(id)
);
CREATE TABLE GlobalDescriptor (
node_id INTEGER NOT NULL,
type INTEGER NOT NULL,
info BLOB,
data BLOB NOT NULL,
FOREIGN KEY (node_id) REFERENCES Node(id)
);
--
CREATE TABLE Info (
STM_size INTEGER,
last_sign_added INTEGER,
process_mem_used INTEGER,
database_mem_used INTEGER,
dictionary_size INTEGER,
parameters TEXT,
time_enter DATE
);
CREATE TABLE Statistics (
id INTEGER NOT NULL,
stamp FLOAT,
data BLOB, -- compressed string
wm_state BLOB, -- compressed data
FOREIGN KEY (id) REFERENCES Node(id)
);
CREATE TABLE Admin (
version TEXT,
preview_image BLOB, -- compressed image
opt_cloud BLOB, -- compressed data
opt_ids BLOB, -- Node ids used to generate the optimized cloud/mesh
opt_poses BLOB, -- compressed N*3x4 float
opt_last_localization BLOB, -- 3x4 float
opt_polygons_size INTEGER, -- e.g., 3
opt_polygons BLOB, -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
opt_tex_coords BLOB, -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
opt_tex_materials BLOB, -- compressed image
opt_map BLOB, -- compressed CV_8SC1 occupancy grid
opt_map_x_min FLOAT,
opt_map_y_min FLOAT,
opt_map_resolution FLOAT,
time_enter DATE
);
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature
WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
BEGIN
SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
END;
-- Creating a trigger for time_enter
CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
BEGIN
UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
BEGIN
UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
BEGIN
UPDATE Word SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
BEGIN
UPDATE Info SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
-- *******************************************************************
-- INDEXES
-- *******************************************************************
CREATE UNIQUE INDEX IDX_Node_id on Node (id);
CREATE INDEX IDX_Feature_node_id on Feature (node_id);
CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
CREATE INDEX IDX_Link_from_id on Link (from_id);
CREATE UNIQUE INDEX IDX_node_label on Node (label);
CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
-- *******************************************************************
-- VERSION
-- *******************************************************************
INSERT INTO Admin(version) VALUES('0.22.0');
corelib/src/rtflann/flann.hpp
+31 -2
View File
@@ -103,7 +103,6 @@ public:
{
flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
loaded_ = false;
if (index_type == FLANN_INDEX_SAVED) {
nnIndex_ = load_saved_index(features, get_param<std::string>(params,"filename"), distance);
loaded_ = true;
@@ -180,10 +179,20 @@ public:
if (fout == NULL) {
throw FLANNException("Cannot open file");
}
nnIndex_->saveIndex(fout);
save(fout);
fclose(fout);
}
/**
* Save index to file stream.
* Caller has to open file stream with "wb" and close it afterwards.
* @param filename
*/
void save(FILE * stream)
{
nnIndex_->saveIndex(stream);
}
/**
* \returns number of features in this index.
*/
@@ -377,6 +386,26 @@ public:
return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
}
void load_saved_index(FILE* fin)
{
if(loaded_) {
throw FLANNException("Index already loaded!");
}
if(nnIndex_->sizeAtBuild() != 0) {
throw FLANNException("Index must not be already built to load data.");
}
if (fin == NULL) {
throw FLANNException("File pointer must be valid!");
}
IndexHeader header = load_header(fin);
if (header.h.data_type != flann_datatype_value<ElementType>::value) {
throw FLANNException("Datatype of saved index is different than of the one to be loaded.");
}
rewind(fin);
nnIndex_->loadIndex(fin);
loaded_ = true;
}
private:
IndexType* load_saved_index(const Matrix<ElementType>& dataset, const std::string& filename, Distance distance)
{
corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp
+234
View File
@@ -0,0 +1,234 @@
/**
* SuperPoint implementation based on the PyTorch version by Rémi Pautrat, Paul-Edouard Sarlin
* Adapted for RTAB-Map integration
*/
#include "SuperpointRpautrat.h"
#include <rtabmap/core/Features2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <pybind11/embed.h>
#include <torch/torch.h>
#include <torch/script.h>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <sstream>
#include "superpoint_to_torchscript_py.h"
namespace rtabmap
{
// Run the python script to export the SuperPoint model file with the desired parameters
static std::string exportSuperPointTorchScript(
const std::string & superpointWeightsPath,
const std::string & superpointModelPath,
const std::string & outputDir,
const int & width,
const int & height,
const float & threshold,
const int & nms_radius,
const bool & cuda)
{
// Validate output directory is explicitly set and exists
if(outputDir.empty())
{
UERROR("Output directory is not set.");
return std::string("");
}
if(!UDirectory::exists(outputDir))
{
UERROR("Output directory does not exist: %s", outputDir.c_str());
return std::string("");
}
// Resolve paths (no dependency on source tree)
const std::string weightsPath = superpointWeightsPath;
const std::string modelPath = superpointModelPath;
const std::string output = std::string(outputDir + "/superpoint_v6_from_tf.pt");
// Sanity checks
if(!UFile::exists(weightsPath)) {
UERROR("Weights not found: %s", weightsPath.c_str());
return "";
}
if(!UFile::exists(modelPath)) {
UERROR("Model not found: %s", modelPath.c_str());
return "";
}
// Execute the script inside the embedded Python interpreter
try
{
pybind11::gil_scoped_acquire acquire;
pybind11::dict scope;
scope["__builtins__"] = pybind11::module_::import("builtins");
// set sys.path to the location of the model definition so it can be imported
std::string model_dir = UDirectory::getDir(modelPath);
auto sys = pybind11::module_::import("sys");
pybind11::list sys_path = sys.attr("path");
sys_path.attr("insert")(0, model_dir);
try {
// execute the script to generate the model
pybind11::exec(uHex2Str(SUPERPOINT_TO_TORCHSCRIPT_PY), scope, scope);
pybind11::function generate_model = scope["generate_model"].cast<pybind11::function>();
pybind11::object result = generate_model(weightsPath, output, cuda, nms_radius, threshold, width, height);
sys_path.attr("remove")(model_dir);
}
catch(...) {
// Ensure sys.path cleanup on any exception
sys_path.attr("remove")(model_dir);
throw;
}
}
// pybind11 throws std::exception for RuntimeError
catch (const std::exception &e)
{
UERROR("Python export failed: %s", e.what());
return "";
}
return output;
}
SPDetectorRpautrat::SPDetectorRpautrat(std::string superpointWeightsPath, std::string superpointModelPath, std::string outputDir, float threshold, bool nms, int minDistance, bool cuda, int maxFeatures, bool ssc) :
device_(torch::kCPU),
superpointWeightsPath_(superpointWeightsPath),
superpointModelPath_(superpointModelPath),
outputDir_(outputDir),
threshold_(threshold),
nms_(nms),
minDistance_(minDistance),
maxFeatures_(maxFeatures),
ssc_(ssc),
detected_(false)
{
if(cuda && !torch::cuda::is_available())
{
UWARN("Cuda option is enabled but torch doesn't have cuda support on this platform, using CPU instead.");
}
cuda_ = cuda && torch::cuda::is_available();
if(!UFile::exists(superpointWeightsPath_)) {
UERROR("Superpoint weights not found: %s", superpointWeightsPath_.c_str());
}
// Update device based on cuda availability
device_ = torch::Device(cuda_ ? torch::kCUDA : torch::kCPU);
}
SPDetectorRpautrat::~SPDetectorRpautrat()
{
}
cv::Mat SPDetectorRpautrat::compute(const std::vector<cv::KeyPoint> &keypoints)
{
if(!detected_)
{
UERROR("SPDetector has been reset before extracting the descriptors! detect() should be called before compute().");
return cv::Mat();
}
if(keypoints.empty())
{
return cv::Mat();
}
// These should have the same size
UASSERT(static_cast<size_t>(desc_.rows) == keypoints.size());
return desc_;
}
std::vector<cv::KeyPoint> SPDetectorRpautrat::detect(const cv::Mat &img, const cv::Mat & mask)
{
// On first frame, run a trace of the model with the desired parameters and load the model file
if(!detected_)
{
// effectively disable nms if it is not enabled by setting radius to 0
int nms_radius = nms_ ? minDistance_ : 0;
std::string modelPath = exportSuperPointTorchScript(
superpointWeightsPath_,
superpointModelPath_,
outputDir_,
img.cols,
img.rows,
threshold_,
nms_radius,
cuda_
);
UDEBUG("Initializing SuperPoint Rpautrat detector with model: %s", modelPath.c_str());
UDEBUG("modelPath=%s thr=%f nms=%d minDistance=%d cuda=%d", modelPath.c_str(), threshold_, nms_?1:0, minDistance_, cuda_?1:0);
if(modelPath.empty())
{
UERROR("Model's path is empty! The model was not exported correctly.");
return std::vector<cv::KeyPoint>();
}
if(!UFile::exists(modelPath))
{
UERROR("Model's path \"%s\" doesn't exist!", modelPath.c_str());
return std::vector<cv::KeyPoint>();
}
// Load TorchScript model
model_ = torch::jit::load(modelPath);
model_.eval(); // put in evaluation mode
model_.to(device_);
}
// format the input tensor for the model
torch::NoGradGuard no_grad_guard;
auto x = torch::from_blob(img.data, {1, 1, img.rows, img.cols}, torch::kByte);
x = x.to(torch::kFloat) / 255;
x = x.set_requires_grad(false).to(device_);
auto outputs = model_.forward({x}).toTuple();
auto kpts_tensor = outputs->elements()[0].toTensor(); // [N, 2] keypoint coordinates
auto scores_tensor = outputs->elements()[1].toTensor(); // [N] keypoint scores
torch::Tensor desc_tensor = outputs->elements()[2].toTensor(); // [N, 256] descriptors
// Convert to CPU for processing
auto keypoints_cpu = kpts_tensor.to(torch::kCPU);
auto scores_cpu = scores_tensor.to(torch::kCPU);
std::vector<cv::KeyPoint> filtered_keypoints;
std::vector<int64_t> keep_indices_vec;
// Apply mask filtering
for(int i = 0; i < keypoints_cpu.size(0); i++) {
float score = scores_cpu[i].item<float>();
float x = keypoints_cpu[i][0].item<float>(); // x coordinate
float y = keypoints_cpu[i][1].item<float>(); // y coordinate
// Check mask if provided
if(mask.empty() || mask.at<unsigned char>((int)y, (int)x) != 0) {
keep_indices_vec.push_back(i);
filtered_keypoints.emplace_back(cv::KeyPoint(x, y, 8, -1, score));
}
}
// Filter descriptors based on mask
auto keep_indices = torch::from_blob(keep_indices_vec.data(), {(long int)keep_indices_vec.size()}, torch::kLong);
keep_indices = keep_indices.to(desc_tensor.device());
auto filtered_descriptors = desc_tensor.index_select(0, keep_indices);
// Convert descriptors to cv::Mat
auto filtered_descriptors_cpu = filtered_descriptors.to(torch::kCPU);
cv::Mat descriptors_mat(filtered_descriptors_cpu.size(0), filtered_descriptors_cpu.size(1), CV_32FC1, filtered_descriptors_cpu.data_ptr<float>());
cv::Mat descriptors_clone = descriptors_mat.clone(); // Clone to own the memory
// Apply limitKeypoints to enforce maxFeatures and SSC
Feature2D::limitKeypoints(filtered_keypoints, descriptors_clone, maxFeatures_, cv::Size(img.cols, img.rows), ssc_);
desc_ = descriptors_clone;
detected_ = true;
return filtered_keypoints;
}
} // namespace rtabmap
corelib/src/superpoint_rpautrat/SuperpointRpautrat.h
+59
View File
@@ -0,0 +1,59 @@
/**
* SuperPoint implementation based on the PyTorch version by Rémi Pautrat, Paul-Edouard Sarlin
* Adapted for RTAB-Map integration
*/
#ifndef SUPERPOINT_RPAUTRAT_H
#define SUPERPOINT_RPAUTRAT_H
#include <torch/torch.h>
#include <opencv2/opencv.hpp>
#include <vector>
#include <memory>
namespace rtabmap
{
class SPDetectorRpautrat {
public:
SPDetectorRpautrat(
std::string superpointWeightsPath,
std::string superpointModelPath,
std::string outputDir,
float threshold = 0.005f,
bool nms = true,
int nmsRadius = 4,
bool cuda = false,
int maxFeatures = 1000,
bool ssc = false
);
virtual ~SPDetectorRpautrat();
std::vector<cv::KeyPoint> detect(const cv::Mat &img, const cv::Mat & mask = cv::Mat());
cv::Mat compute(const std::vector<cv::KeyPoint> &keypoints);
// Setters for post-processing parameters that don't require model reinitialization
void setMaxFeatures(int maxFeatures) { maxFeatures_ = maxFeatures; }
void setSSC(bool ssc) { ssc_ = ssc; }
private:
torch::jit::script::Module model_;
torch::Device device_;
cv::Mat desc_;
std::string superpointWeightsPath_;
std::string superpointModelPath_;
std::string outputDir_;
float threshold_;
bool nms_;
int minDistance_;
bool cuda_;
int maxFeatures_;
bool ssc_;
bool detected_;
};
}
#endif // SUPERPOINT_RPAUTRAT_H
corelib/src/superpoint_rpautrat/superpoint_to_torchscript.py
+107
View File
@@ -0,0 +1,107 @@
#!/usr/bin/env python3
"""
Convert PyTorch weights to TorchScript format for C++ usage.
"""
import argparse
import os
import torch
import torch.nn as nn
from superpoint_pytorch import SuperPoint
def wrap_model(model: nn.Module):
"""
Simple wrapper to fix SuperPoint input format for TorchScript.
Easier to call from C++ code since the input isn't a dictionary.
"""
class Wrapper(nn.Module):
def __init__(self, net: nn.Module):
super().__init__()
self.net = net
def forward(self, x: torch.Tensor):
# SuperPoint expects {"image": tensor} but TorchScript doesn't like dict indexing
out = self.net.forward({"image": x})
# Return the format expected by C++ code: keypoints, scores, descriptors
# For single batch item, take the first (and only) element
keypoints = out["keypoints"][0] if out["keypoints"] else torch.empty(0, 2)
scores = out["keypoint_scores"][0] if out["keypoint_scores"] else torch.empty(0)
descriptors = out["descriptors"][0] if out["descriptors"] else torch.empty(0, 256)
return (keypoints, scores, descriptors)
return Wrapper(model)
def generate_model(
weights_path: str,
output_path: str,
cuda: bool,
nms_radius: int,
threshold: float,
width: int,
height: int,
):
# Check if weights are already TorchScript
try:
scripted = torch.jit.load(weights_path, map_location="cpu")
scripted.eval()
torch.jit.save(scripted, output_path)
print(f"Converted TorchScript file: {output_path}")
return
except:
pass
device = "cuda" if cuda else "cpu"
# Load SuperPoint model and weights
model = SuperPoint(
nms_radius=nms_radius,
detection_threshold=threshold,
).eval().to(device)
# Load weights without forcing CPU location to allow CUDA usage
weights = torch.load(weights_path, map_location=None)
if isinstance(weights, dict) and "state_dict" in weights:
weights = weights["state_dict"]
model.load_state_dict(weights, strict=False)
wrapped = wrap_model(model)
dummy = torch.randn(1, 1, height, width, device=device) # Dummy input, grayscale, using cuda.
# Convert to TorchScript using trace (SuperPoint has dynamic behavior that scripting can't handle)
print("Using torch.jit.trace (SuperPoint has dynamic behavior)...")
scripted = torch.jit.trace(wrapped, (dummy,), strict=False)
print("Successfully traced SuperPoint model")
# Save output
os.makedirs(os.path.dirname(output_path), exist_ok=True)
torch.jit.save(scripted, output_path)
print(f"Converted SuperPoint weights to TorchScript: {output_path}")
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Convert SuperPoint weights to TorchScript")
parser.add_argument("--weights", required=True, help="Path to weights file")
parser.add_argument("--output", required=True, help="Output TorchScript file")
parser.add_argument("--cuda", action="store_true", help="Use CUDA")
parser.add_argument("--width", type=int, default=1920, help="Width of the input image")
parser.add_argument("--height", type=int, default=288, help="Height of the input image")
parser.add_argument("--nms_radius", type=int, default=4, help="NMS radius")
parser.add_argument("--threshold", type=float, default=0.005, help="Confidence threshold")
args = parser.parse_args()
print(f"Generating model from weights: {args.weights} to output: {args.output}")
generate_model(
weights_path=args.weights,
output_path=args.output,
cuda=args.cuda,
nms_radius=args.nms_radius,
threshold=args.threshold,
width=args.width,
height=args.height,
)
corelib/src/util3d_filtering.cpp
+4 -2
View File
@@ -1048,7 +1048,8 @@ pcl::IndicesPtr cropBoxImpl(
const Transform & transform,
bool negative)
{
UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
UASSERT_MSG(min[0] < max[0] && min[1] < max[1] && min[2] <= max[2], // z can be equal in 2D case
uFormat("x=%f->%f y=%f->%f z=%f->%f", min[0], max[0], min[1], max[1], min[2], max[2]).c_str());
pcl::IndicesPtr output(new std::vector<int>);
pcl::CropBox<PointT> filter;
@@ -1070,7 +1071,8 @@ pcl::IndicesPtr cropBoxImpl(
pcl::IndicesPtr cropBox(const pcl::PCLPointCloud2::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
UASSERT_MSG(min[0] < max[0] && min[1] < max[1] && min[2] <= max[2], // z can be equal in 2D case
uFormat("x=%f->%f y=%f->%f z=%f->%f", min[0], max[0], min[1], max[1], min[2], max[2]).c_str());
pcl::IndicesPtr output(new std::vector<int>);
pcl::CropBox<pcl::PCLPointCloud2> filter;