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

merged master->branch

Browse Source
This commit is contained in:
matlabbe
2026-04-05 14:20:35 -07:00
parent ffb3be7ba3 8fd701aabe
commit 0405949ff4
103 changed files with 7621 additions and 3191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CMakeLists.txt
+1 -24
View File
@@ -791,37 +791,14 @@ IF(CUVSLAM_FOUND)
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)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${GTSAM_INCLUDE_DIR}
)
ELSE()
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${GTSAM_INCLUDE_DIRS}
)
ENDIF()
SET(SRC_FILES
${SRC_FILES}
optimizer/gtsam/GravityFactor.cpp
)
IF(WIN32)
# GTSAM should be built in STATIC on Windows to avoid "error C2338: THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS" when building GTSAM
add_definitions("-DGTSAM_IMPORT_STATIC")
ENDIF(WIN32)
SET(LIBRARIES
${LIBRARIES}
gtsam # Windows: Place static libs at the end
gtsam
)
IF(WIN32)
#explicitly add metis target on windows (after gtsam target)
SET(LIBRARIES
${LIBRARIES}
metis
)
ENDIF(WIN32)
ENDIF(GTSAM_FOUND)
IF(WITH_MADGWICK)
corelib/src/CameraModel.cpp
+16
View File
@@ -353,6 +353,22 @@ bool CameraModel::load(const std::string & filePath)
data[0], data[1], data[2], data[3],
data[4], data[5], data[6], data[7],
data[8], data[9], data[10], data[11]);
Transform detCheck = localTransform_.clone();
localTransform_.normalizeRotation(); /// Normalize by default
float det = detCheck.toEigen3f().linear().determinant();
if(fabs(det - 1.0f) > 0.0001)
{
std::stringstream streamBefore, streamAfter;
streamBefore << detCheck << std::endl;
streamAfter << localTransform_ << std::endl;
UWARN("The camera model's local_transform from \"%s\" doesn't "
"have a normalized rotation matrix (dertminant=%f). We will normalize "
"it for convenience.\nWas:\n%sNow\n%s",
filePath.c_str(),
det,
streamBefore.str().c_str(),
streamAfter.str().c_str());
}
}
else
{
corelib/src/DBDriver.cpp
+11 -3
View File
@@ -45,7 +45,7 @@ DBDriver * DBDriver::create(const ParametersMap & parameters)
DBDriver::DBDriver(const ParametersMap & parameters) :
_emptyTrashesTime(0),
_timestampUpdate(true)
_timestampUpdate(false)
{
this->parseParameters(parameters);
}
@@ -73,7 +73,15 @@ void DBDriver::closeConnection(bool save, const std::string & outputUrl)
else
{
_trashesMutex.lock();
for(auto & iter: _trashSignatures)
{
delete iter.second;
}
_trashSignatures.clear();
for(auto & iter: _trashVisualWords)
{
delete iter.second;
}
_trashVisualWords.clear();
_trashesMutex.unlock();
}
@@ -83,12 +91,12 @@ void DBDriver::closeConnection(bool save, const std::string & outputUrl)
UDEBUG("");
}
bool DBDriver::openConnection(const std::string & url, bool overwritten)
bool DBDriver::openConnection(const std::string & url, bool overwritten, bool readOnly)
{
UDEBUG("");
_url = url;
_dbSafeAccessMutex.lock();
if(this->connectDatabaseQuery(url, overwritten))
if(this->connectDatabaseQuery(url, overwritten, readOnly))
{
_dbSafeAccessMutex.unlock();
return true;
corelib/src/DBDriverSqlite3.cpp
+15 -16
View File
@@ -320,7 +320,7 @@ bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const
return false;
}
bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten, bool readOnly)
{
this->disconnectDatabaseQuery();
// Open a database connection
@@ -332,7 +332,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
if(!url.empty())
{
dbFileExist = UFile::exists(url.c_str());
if(dbFileExist && overwritten)
if(dbFileExist && overwritten && !readOnly)
{
UINFO("Deleting database %s...", url.c_str());
UASSERT(UFile::erase(url.c_str()) == 0);
@@ -354,12 +354,12 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
{
ULOGGER_INFO("Using empty database in the memory.");
}
rc = sqlite3_open_v2(":memory:", &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
rc = sqlite3_open_v2(":memory:", &_ppDb, readOnly ? SQLITE_OPEN_READONLY : SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
}
else
{
ULOGGER_INFO("Using database \"%s\" from the hard drive.", url.c_str());
rc = sqlite3_open_v2(url.c_str(), &_ppDb, SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
rc = sqlite3_open_v2(url.c_str(), &_ppDb, readOnly ? SQLITE_OPEN_READONLY : SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE, 0);
}
if(rc != SQLITE_OK)
{
@@ -4350,7 +4350,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
{
UDEBUG("nodes = %d", nodes.size());
UDEBUG("nodes = %d, updateTimestamp = %s", nodes.size(), updateTimestamp?"true":"false");
if(_ppDb && nodes.size())
{
UTimer timer;
@@ -4389,7 +4389,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
{
s = *i;
int index = 1;
if(s)
if(s && (s->isModified() || updateTimestamp))
{
rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4413,7 +4413,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (node id = %d map=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), s->id(), s->mapId()).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4426,14 +4426,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks());
// Update links part1
if(uStrNumCmp(_version, "0.18.3") >= 0)
{
query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark);
}
else
{
query = uFormat("DELETE FROM Link WHERE from_id=?;");
}
query = uFormat("DELETE FROM Link WHERE from_id=?;");
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());
for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
@@ -4468,6 +4461,12 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
{
stepLink(ppStmt, i->second);
}
// Save landmarks
const std::map<int, Link> & landmarks = (*j)->getLandmarks();
for(std::map<int, Link>::const_iterator i=landmarks.begin(); i!=landmarks.end(); ++i)
{
stepLink(ppStmt, i->second);
}
}
}
// Finalize (delete) the statement
@@ -5758,7 +5757,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
void DBDriverSqlite3::saveFlannIndexQuery(const std::vector<unsigned char> & data) const
{
UDEBUG("");
UDEBUG("data size = %ld bytes", data.size());
if(_ppDb && uStrNumCmp(_version, "0.23.0") >= 0)
{
UTimer timer;
corelib/src/DBReader.cpp
+14 -7
View File
@@ -56,6 +56,7 @@ DBReader::DBReader(const std::string & databasePath,
int startMapId,
int stopMapId,
bool priorsIgnored,
bool imuIgnored,
const std::vector<Transform> & cameraLocalTransformOverrides) :
Camera(frameRate),
_paths(uSplit(databasePath, ';')),
@@ -69,6 +70,7 @@ DBReader::DBReader(const std::string & databasePath,
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_priorsIgnored(priorsIgnored),
_imuIgnored(imuIgnored),
_startMapId(startMapId),
_stopMapId(stopMapId),
_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
@@ -96,6 +98,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
int startMapId,
int stopMapId,
bool priorsIgnored,
bool imuIgnored,
const std::vector<Transform> & cameraLocalTransformOverrides) :
Camera(frameRate),
_paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
_landmarksIgnored(landmarksIgnored),
_featuresIgnored(featuresIgnored),
_priorsIgnored(priorsIgnored),
_imuIgnored(imuIgnored),
_startMapId(startMapId),
_stopMapId(stopMapId),
_cameraLocalTransformOverrides(cameraLocalTransformOverrides),
@@ -463,14 +467,17 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
}
Transform gravityTransform;
std::multimap<int, Link> gravityLinks;
_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
if( gravityLinks.size() &&
!gravityLinks.begin()->second.transform().isNull() &&
gravityLinks.begin()->second.infMatrix().cols == 6 &&
gravityLinks.begin()->second.infMatrix().rows == 6)
if(!_imuIgnored)
{
gravityTransform = gravityLinks.begin()->second.transform();
std::multimap<int, Link> gravityLinks;
_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
if( gravityLinks.size() &&
!gravityLinks.begin()->second.transform().isNull() &&
gravityLinks.begin()->second.infMatrix().cols == 6 &&
gravityLinks.begin()->second.infMatrix().rows == 6)
{
gravityTransform = gravityLinks.begin()->second.transform();
}
}
Landmarks landmarks;
corelib/src/Features2d.cpp
+58 -43
View File
@@ -303,7 +303,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
cv::Mat descriptorsTmp;
if(ssc)
{
ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());
// Sorting keypoints by deacreasing order of strength
std::vector<float> responseVector;
@@ -419,7 +419,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
inliers.resize(keypoints.size(), false);
if(ssc)
{
ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());
// Sorting keypoints by deacreasing order of strength
std::vector<float> responseVector;
@@ -466,7 +466,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
minimumHessian = iter->first;
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size()-removed, minimumHessian);
ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
}
else
@@ -1251,7 +1251,8 @@ SIFT::SIFT(const ParametersMap & parameters) :
preciseUpscale_(Parameters::defaultSIFTPreciseUpscale()),
rootSIFT_(Parameters::defaultSIFTRootSIFT()),
gpu_(Parameters::defaultSIFTGpu()),
guaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
gaussianThreshold_(Parameters::defaultSIFTGaussianThreshold()),
maxGaussianThreshold_(Parameters::defaultSIFTMaxGaussianThreshold()),
upscale_(Parameters::defaultSIFTUpscale()),
cudaSiftData_(0),
cudaSiftMemory_(0),
@@ -1284,23 +1285,25 @@ void SIFT::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kSIFTPreciseUpscale(), preciseUpscale_);
Parameters::parse(parameters, Parameters::kSIFTRootSIFT(), rootSIFT_);
Parameters::parse(parameters, Parameters::kSIFTGpu(), gpu_);
Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), guaussianThreshold_);
Parameters::parse(parameters, Parameters::kSIFTGaussianThreshold(), gaussianThreshold_);
Parameters::parse(parameters, Parameters::kSIFTMaxGaussianThreshold(), maxGaussianThreshold_);
Parameters::parse(parameters, Parameters::kSIFTUpscale(), upscale_);
if(gpu_)
{
#ifdef RTABMAP_CUDASIFT
// Check if there is a cuda device
if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
UDEBUG("Init SiftData");
if(cudaSiftData_ == 0) {
if(cudaSiftData_==0)
{
if(InitCuda(0, ULogger::level() == ULogger::kDebug)) {
UDEBUG("Init SiftData");
cudaSiftData_ = new SiftData();
InitSiftData(*cudaSiftData_, 8192, true, true);
}
}
else{
UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
gpu_ = false;
else{
UWARN("No cuda device(s) detected, CudaSift is not available! Using SIFT CPU version instead.");
gpu_ = false;
}
}
#else
UWARN("RTAB-Map is not built with CudaSift so %s cannot be used!", Parameters::kSIFTGpu().c_str());
@@ -1363,7 +1366,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
numOctaves = 7; // hard-coded limit in CudaSift
}
float initBlur = sigma_; /* Amount of initial Gaussian blurring in standard deviations */
float thresh = guaussianThreshold_; /* Threshold on difference of Gaussians for feature pruning */
float thresh = gaussianThreshold_; /* Threshold on difference of Gaussians for feature pruning */
float edgeLimit = edgeThreshold_;
float minScale = 0.0f; /* Minimum acceptable scale to remove fine-scale features */
UDEBUG("numOctaves=%d initBlur=%f thresh=%f edgeLimit=%f minScale=%f upScale=%s w=%d h=%d", numOctaves, initBlur, thresh, edgeLimit, minScale, upscale_?"true":"false", w, h);
@@ -1388,15 +1391,9 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
cudaSiftDescriptors_ = cv::Mat();
if(cudaSiftData_->numPts)
{
int maxKeypoints = this->getMaxFeatures();
if(maxKeypoints == 0 || maxKeypoints > cudaSiftData_->numPts)
{
maxKeypoints = cudaSiftData_->numPts;
}
// Re-using same implementation of limitKeypoints() directly here to avoid doubling memory copies
// Sort words by hessian
std::multimap<float, int> hessianMap; // <hessian,id>
keypoints.resize(cudaSiftData_->numPts);
cudaSiftDescriptors_ = cv::Mat(cudaSiftData_->numPts, 128, CV_32FC1);
size_t k=0;
for(int i=0; i<cudaSiftData_->numPts; ++i)
{
// Ignore keypoints with invalid descriptors
@@ -1413,29 +1410,40 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
continue;
}
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, int>(cudaSiftData_->h_data[i].sharpness, i));
}
if(i>0 &&
cudaSiftData_->h_data[i].subsampling == cudaSiftData_->h_data[i-1].subsampling &&
fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].xpos) +
fabs(cudaSiftData_->h_data[i].xpos-cudaSiftData_->h_data[i-1].ypos) < 0.1f)
{
// Same feature, skip doubles
continue;
}
if((int)hessianMap.size() < maxKeypoints)
{
maxKeypoints = hessianMap.size();
}
float response = abs(cudaSiftData_->h_data[i].sharpness);
if(maxGaussianThreshold_>gaussianThreshold_ && response > maxGaussianThreshold_)
{
continue;
}
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
keypoints.resize(maxKeypoints);
cudaSiftDescriptors_ = cv::Mat(maxKeypoints, 128, CV_32FC1);
for(unsigned int k=0; k<keypoints.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
int i = iter->second;
float *desc = cudaSiftData_->h_data[i].data;
cv::Mat(1, 128, CV_32FC1, desc).copyTo(cudaSiftDescriptors_.row(k));
keypoints[k].pt.x = cudaSiftData_->h_data[i].xpos;
keypoints[k].pt.y = cudaSiftData_->h_data[i].ypos;
keypoints[k].size = 2.0f*cudaSiftData_->h_data[i].scale; // x2 because the scale is more like a radius than a diameter, see CudaSift's ExtractSiftDescriptors function to see how they convert scale to patch size
keypoints[k].angle = cudaSiftData_->h_data[i].orientation;
keypoints[k].response = cudaSiftData_->h_data[i].sharpness;
keypoints[k].response = response;
keypoints[k].octave = log2(cudaSiftData_->h_data[i].subsampling)-(upscale_?1:0);
++k;
}
if(k < keypoints.size())
{
UDEBUG("keypoints extracted = %d, valid=%d", keypoints.size(), k);
keypoints.resize(k);
cudaSiftDescriptors_.resize(k);
}
if(this->getMaxFeatures() != 0 && this->getMaxFeatures() < (int)keypoints.size())
{
// Call limitKeypoints() now to filter the descriptors.
this->limitKeypoints(keypoints, cudaSiftDescriptors_, this->getMaxFeatures(), cv::Size(w,h), this->getSSC());
}
}
}
@@ -1457,12 +1465,13 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
cv::Mat descriptors;
#ifdef RTABMAP_CUDASIFT
if(gpu_)
{
if((int)keypoints.size() == cudaSiftDescriptors_.rows)
{
return cudaSiftDescriptors_.clone();
descriptors = cudaSiftDescriptors_.clone();
}
else
{
@@ -1470,19 +1479,25 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
return cv::Mat();
}
}
else
{
#endif
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
#ifdef RTABMAP_NONFREE
sift_->compute(image, keypoints, descriptors);
sift_->compute(image, keypoints, descriptors);
#else
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
#endif
#else // >=4.4, >=3.4.11
sift_->compute(image, keypoints, descriptors);
sift_->compute(image, keypoints, descriptors);
#endif
#ifdef RTABMAP_CUDASIFT
}
#endif
if( rootSIFT_ && !descriptors.empty())
{
UDEBUG("Performing RootSIFT...");
corelib/src/FlannIndex.cpp
+1 -1
View File
@@ -96,7 +96,7 @@ std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) cons
#else
UTimer timer;
const int headerSizeBytes = sizeof(int)*FLANN_INDEX_HEADER_SIZE;
std::vector<unsigned char> indexData(1024*1024*100 + headerSizeBytes); // Max 100 MB
std::vector<unsigned char> indexData(1024*1024*1024 + headerSizeBytes); // Max 1 GB
FILE* indexDataPtr = fmemopen(indexData.data()+headerSizeBytes, indexData.size() - headerSizeBytes, "wb");
long bytes_written = 0;
if (indexDataPtr) {
corelib/src/Graph.cpp
+12 -23
View File
@@ -2020,19 +2020,21 @@ std::list<std::pair<int, Transform> > computePath(
bool lookInDatabase,
bool updateNewCosts,
float linearVelocity, // m/sec
float angularVelocity) // rad/sec
float angularVelocity, // rad/sec
bool ignoreDirectLinks)
{
UASSERT(memory!=0);
UASSERT(fromId>=0);
UASSERT(toId!=0);
std::list<std::pair<int, Transform> > path;
UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f ignoreDirectLinks=%d",
fromId,
toId,
lookInDatabase?1:0,
updateNewCosts?1:0,
linearVelocity,
angularVelocity);
angularVelocity,
ignoreDirectLinks?1:0);
std::multimap<int, Link> allLinks;
if(lookInDatabase)
@@ -2110,7 +2112,9 @@ std::list<std::pair<int, Transform> > computePath(
}
for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
if(iter->second.from() != iter->second.to() &&
(!ignoreDirectLinks ||
(!(iter->second.from()==fromId && iter->second.to()==toId) && !(iter->second.to()==fromId && iter->second.from()==toId))))
{
Transform nextPose = currentNode->pose()*iter->second.transform();
float cost = 0.0f;
@@ -2396,26 +2400,15 @@ std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const st
float computePathLength(
const std::vector<std::pair<int, Transform> > & path,
unsigned int fromIndex,
unsigned int toIndex)
const std::vector<std::pair<int, Transform> > & path)
{
float length = 0.0f;
if(path.size() > 1)
{
UASSERT(fromIndex < path.size() && toIndex < path.size() && fromIndex <= toIndex);
if(fromIndex >= toIndex)
for(unsigned int i=0; i<path.size()-1; ++i)
{
toIndex = (unsigned int)path.size()-1;
length+=path[i].second.getDistance(path[i+1].second);
}
float x=0, y=0, z=0;
for(unsigned int i=fromIndex; i<toIndex-1; ++i)
{
x += fabs(path[i].second.x() - path[i+1].second.x());
y += fabs(path[i].second.y() - path[i+1].second.y());
z += fabs(path[i].second.z() - path[i+1].second.z());
}
length = sqrt(x*x + y*y + z*z);
}
return length;
}
@@ -2426,19 +2419,15 @@ float computePathLength(
float length = 0.0f;
if(path.size() > 1)
{
float x=0, y=0, z=0;
std::map<int, Transform>::const_iterator iter=path.begin();
Transform previousPose = iter->second;
++iter;
for(; iter!=path.end(); ++iter)
{
const Transform & currentPose = iter->second;
x += fabs(previousPose.x() - currentPose.x());
y += fabs(previousPose.y() - currentPose.y());
z += fabs(previousPose.z() - currentPose.z());
length+=previousPose.getDistance(currentPose);
previousPose = currentPose;
}
length = sqrt(x*x + y*y + z*z);
}
return length;
}
corelib/src/IMUThread.cpp
+14 -3
View File
@@ -135,8 +135,20 @@ void IMUThread::mainLoop()
std::stringstream stream(line);
std::string s;
std::getline(stream, s, ',');
std::string nanoseconds = s.substr(s.size() - 9, 9);
std::string seconds = s.substr(0, s.size() - 9);
double stamp = 0.0;
if(s.find('.') != std::string::npos)
{
// Normal [epoch] timestamp
stamp = uStr2Double(s);
}
else
{
// Assume EuRoC format
std::string nanoseconds = s.substr(s.size() - 9, 9);
std::string seconds = s.substr(0, s.size() - 9);
stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
}
cv::Vec3d gyr;
for (int j = 0; j < 3; ++j) {
@@ -150,7 +162,6 @@ void IMUThread::mainLoop()
acc[j] = uStr2Double(s);
}
double stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
if(previousStamp_>0 && stamp > previousStamp_)
{
captureDelay_ = stamp - previousStamp_;
corelib/src/Memory.cpp
+308 -125
View File
@@ -83,6 +83,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rgbCompressionFormat(Parameters::defaultMemImageCompressionFormat()),
_depthCompressionFormat(Parameters::defaultMemDepthCompressionFormat()),
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
_localizationReadOnly(Parameters::defaultMemLocalizationReadOnly()),
_localizationDataSaved(Parameters::defaultMemLocalizationDataSaved()),
_flannIndexSaved(Parameters::defaultKpFlannIndexSaved()),
_reduceGraph(Parameters::defaultMemReduceGraph()),
@@ -185,10 +186,6 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
_dbDriver = 0; // HACK for the clear() below to think that there is no db
}
}
else if(!_memoryChanged && _linksChanged)
{
_dbDriver->setTimestampUpdateEnabled(false); // update links only
}
this->clear();
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
@@ -212,10 +209,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
bool success = true;
if(_dbDriver)
{
_dbDriver->setTimestampUpdateEnabled(true); // make sure that timestamp update is enabled (may be disabled above)
success = false;
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\"..."));
if(_dbDriver->openConnection(dbUrl, dbOverwritten))
if(_dbDriver->openConnection(dbUrl, dbOverwritten, isReadOnly()))
{
success = true;
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\", done!"));
@@ -245,6 +242,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
if(loadAllNodesInWM)
{
UDEBUG("Loading all nodes to WM...");
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
std::set<int> ids;
_dbDriver->getAllNodeIds(ids, true);
@@ -252,6 +250,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
}
else
{
UDEBUG("Loading last nodes to WM...");
// load previous session working memory
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
_dbDriver->loadLastNodes(dbSignatures, !_loadVisualLocalFeaturesOnInit);
@@ -438,7 +437,8 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
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)
bool corruptedDictionary = false;
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end() && !corruptedDictionary; ++i)
{
Signature * s = this->_getSignature(i->first);
UASSERT(s != 0);
@@ -451,12 +451,114 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
{
if(iter->first > 0)
{
_vwd->addWordRef(iter->first, i->first);
if(!_vwd->addWordRef(iter->first, s->id()))
{
corruptedDictionary = true;
break;
}
}
}
s->setEnabled(!corruptedDictionary);
if(corruptedDictionary)
{
//revert all changes from that signature till it broke above
for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
{
if(iter->first > 0)
{
_vwd->removeAllWordRef(iter->first, s->id());
}
}
}
s->setEnabled(true);
}
}
if(corruptedDictionary)
{
if(!_vwd->isIncremental())
{
UERROR("The dictionary is empty or missing some words from nodes in WM, "
"we cannot repair it because it is a fixed dictionary. Make sure you "
"are using the right fixed dictionary that was used to generate the map.");
}
else
{
std::string msg = uFormat(
"The dictionary is empty or missing some words from nodes in WM, "
"we will try to repair it. This can be caused by rtabmap closing before it has time "
"to save the dictionary. Re-creating the dictionary from %ld nodes...",
signatures.size());
UWARN("%s", msg.c_str());
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg));
//remove all words ref
const std::map<int, VisualWord *> & addedWords = _vwd->getVisualWords();
int nodesRepaired = 0;
size_t oldSize = addedWords.size();
std::string assertMsg =
"If we assert here, the problem is maybe deeper. Try "
"to use rtabmap-recovery tool instead to fix the database.";
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
Signature * s = this->_getSignature(i->first);
UASSERT_MSG(s != 0, assertMsg.c_str());
if(s->isEnabled())
{
// Words already in dictionary and references added
continue;
}
const std::multimap<int, int> * words = &s->getWords();
if(words->size())
{
cv::Mat descriptors = s->getWordsDescriptors();
std::multimap<int, int> loadedWords;
if(descriptors.empty())
{
// We may have started rtabmap without loading features, check in the database
std::multimap<int, int> w;
std::vector<cv::KeyPoint> k;
std::vector<cv::Point3f> p;
_dbDriver->getLocalFeatures(s->id(), loadedWords, k, p, descriptors);
UASSERT_MSG(loadedWords.size() == words->size(), assertMsg.c_str()); // Just doublecheck
words = &loadedWords; // The index will be set
UASSERT_MSG(!descriptors.empty(), assertMsg.c_str());
}
bool repaired = false;
for(std::multimap<int, int>::const_iterator iter = words->begin(); iter!=words->end(); ++iter)
{
if(iter->first > 0)
{
if(addedWords.find(iter->first) == addedWords.end())
{
UASSERT_MSG(iter->second >= 0 && iter->second < descriptors.rows,
uFormat("iter->second=%d descriptors.rows=%d (signature=%d word=%d). %s",
iter->second, descriptors.rows, s->id(), iter->first, assertMsg.c_str()).c_str());
_vwd->addWord(new VisualWord(iter->first, descriptors.row(iter->second).clone()));
repaired = true;
}
UASSERT_MSG(_vwd->addWordRef(iter->first, s->id()), assertMsg.c_str());
}
}
nodesRepaired += (repaired?1:0);
s->setEnabled(true);
}
}
msg = uFormat(
"Regenerated the dictionary with %ld missing words (%ld -> %ld) from %d nodes.",
addedWords.size() - oldSize,
oldSize,
addedWords.size(),
nodesRepaired);
UWARN("%s", msg.c_str());
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg));
_memoryChanged = true; // This will force rtabmap to save back the dictionary even if we don't process any new data
_vwd->update();
}
}
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Adding word references, done! (%d)", _vwd->getTotalActiveReferences())));
if(_vwd->getUnusedWordsSize() && _vwd->isIncremental())
@@ -531,16 +633,23 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
UDEBUG("_memoryChanged=%d _linksChanged=%d databaseNameChanged=%d", _memoryChanged?1:0, _linksChanged?1:0, databaseNameChanged?1:0);
if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged))
if(!databaseSaved || (!_memoryChanged && !_linksChanged && !databaseNameChanged) || this->isReadOnly())
{
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
UINFO("No changes added to database.");
if(_dbDriver)
{
saveFlannIndex(postInitClosingEvents);
if(!this->isReadOnly()) {
saveFlannIndex(postInitClosingEvents);
}
else if(_memoryChanged || _linksChanged || databaseNameChanged)
{
UWARN("Memory has been modified (nodes=%s links=%s name=%s) but the database is read-only, changes are not saved to database.",
_memoryChanged?"true":"false", _linksChanged?"true":"false", databaseNameChanged?"true":"false");
}
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
_dbDriver->closeConnection(false, ouputDatabasePath);
_dbDriver->closeConnection(false);
delete _dbDriver;
_dbDriver = 0;
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
@@ -556,12 +665,6 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
if(!_memoryChanged && _dbDriver)
{
saveFlannIndex(postInitClosingEvents);
if(_linksChanged) {
// don't update the time stamps!
UDEBUG("");
_dbDriver->setTimestampUpdateEnabled(false);
}
}
this->clear();
if(_dbDriver)
@@ -663,6 +766,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMarkerVarianceOrientationIgnored(), _markerOrientationIgnored);
Parameters::parse(params, Parameters::kMemLocalizationDataSaved(), _localizationDataSaved);
Parameters::parse(params, Parameters::kKpFlannIndexSaved(), _flannIndexSaved);
Parameters::parse(params, Parameters::kMemLocalizationReadOnly(), _localizationReadOnly);
if(_markerAngVariance>=9999)
{
@@ -1184,114 +1288,180 @@ void Memory::addSignatureToWmFromLTM(Signature * signature)
}
}
void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
bool Memory::canBeReduced(const Link & link, float maxDistance, int direction)
{
UDEBUG("Inserting node %d from STM in WM...", id);
UASSERT(_stMem.find(id) != _stMem.end());
return link.to() != link.from() &&
link.type() != Link::kNeighbor &&
link.type() != Link::kNeighborMerged &&
link.userDataCompressed().empty() &&
link.type() != Link::kUndef &&
link.type() != Link::kVirtualClosure &&
(maxDistance == 0.0f || link.transform().getNorm() < maxDistance) &&
(direction == 0 || (direction==-1 && link.to() < link.from()) || (direction==1 && link.to() > link.from()));
}
int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direction)
{
UDEBUG("Reducing %d (max distance=%f, keep linked in db=%s, direction=%d)",
id, maxDistance, keepLinkedInDb?"true":"false", direction);
Signature * s = this->_getSignature(id);
UASSERT(s!=0);
if(_reduceGraph)
if(s==0)
{
bool merge = false;
const std::multimap<int, Link> & links = s->getLinks();
std::map<int, Link> neighbors;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(!merge)
{
merge = iter->second.to() < s->id() && // should be a parent->child link
iter->second.to() != iter->second.from() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.userDataCompressed().empty() &&
iter->second.type() != Link::kUndef &&
iter->second.type() != Link::kVirtualClosure;
if(merge)
{
UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
if(reducedTo)
{
*reducedTo = iter->second.to();
}
}
UWARN("Node %d is not in WM/STM, cannot reduce it.", id);
return 0;
}
}
if(iter->second.type() == Link::kNeighbor)
if(!s->getLabel().empty())
{
// We currently not remove nodes with labels
return 0;
}
std::multimap<int, Link> links = s->getLinks();
std::map<int, Link> neighbors;
int reducedTo = 0;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(canBeReduced(iter->second, maxDistance, direction))
{
float distance = iter->second.transform().getNorm();
reducedTo = iter->second.to();
UDEBUG("Reduce %d to %d (distance=%f)",
s->id(), iter->second.to(), distance);
}
if(iter->second.type() == Link::kNeighbor)
{
neighbors.insert(*iter);
}
}
if(reducedTo>0)
{
if(maxDistance > 0.0f)
{
// Only reduce if all neighbor merged links are also below maxDistance
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
neighbors.insert(*iter);
if( iter->second.type() == Link::kNeighborMerged &&
iter->second.transform().getNorm() > maxDistance)
{
return 0;
}
}
}
if(merge)
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(s->getLabel().empty())
Signature * sTo = this->_getSignature(iter->first);
if(sTo->id()!=s->id()) // Not Prior/Gravity links...
{
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
sTo->removeLink(s->id());
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kUndef)
{
Signature * sTo = this->_getSignature(iter->first);
if(sTo->id()!=s->id()) // Not Prior/Gravity links...
if(iter->second.type() == Link::kNeighborMerged)
{
UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
sTo->removeLink(s->id());
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kUndef)
s->removeLink(sTo->id());
if(maxDistance == 0.0f)
{
// link to all neighbors
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
// online graph reduction, always skip these links
continue;
}
}
// link to all neighbors
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
{
if(!sTo->hasLink(jter->second.to()))
{
Link l = iter->second.inverse().merge(
jter->second,
iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
UDEBUG("Merging link %d->%d (type=%d) to with %d->%d (type %d). Adding %d->%d (type %d) to %d and %d",
iter->second.to(), iter->second.from(), iter->second.type(),
jter->second.from(), jter->second.to(), jter->second.type(),
l.from(), l.to(), l.type(), sTo->id(), l.to());
sTo->addLink(l);
Signature * sB = this->_getSignature(l.to());
UASSERT(sB!=0);
UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d type=%d", sB->id(), l.to(), l.type()).c_str());
sB->addLink(l.inverse());
}
}
// link to all landmarks
for(std::map<int, Link>::const_iterator jter=s->getLandmarks().begin(); jter!=s->getLandmarks().end(); ++jter)
{
if(!uContains(sTo->getLandmarks(), jter->first))
{
UDEBUG("Move landmark observation %d from %d to %d",
jter->first, s->id(), sTo->id());
Link l = iter->second.inverse().merge(
jter->second,
jter->second.type());
sTo->addLandmark(l);
// Update landmark index
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(jter->first);
if(nter!=_landmarksIndex.end())
{
if(!sTo->hasLink(jter->second.to()))
{
UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)",
iter->second.from(), iter->second.to(), iter->second.type(),
jter->second.from(), jter->second.to(), jter->second.type());
Link l = iter->second.inverse().merge(
jter->second,
iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
sTo->addLink(l);
Signature * sB = this->_getSignature(l.to());
UASSERT(sB!=0);
UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d", sB->id(), l.to()).c_str());
sB->addLink(l.inverse());
}
nter->second.insert(sTo->id());
}
else
{
std::set<int> tmp;
tmp.insert(sTo->id());
_landmarksIndex.insert(std::make_pair(jter->first, tmp));
}
}
}
}
}
}
//remove neighbor links
std::multimap<int, Link> linksCopy = links;
for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
this->moveToTrash(s, keepLinkedInDb);
s = 0;
_linksChanged = true;
_memoryChanged = true;
}
return reducedTo;
}
void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
{
UDEBUG("Inserting node %d from STM in WM...", id);
UASSERT(_stMem.find(id) != _stMem.end());
int reducedId = 0;
if(_reduceGraph)
{
Signature * s = this->_getSignature(id);
UASSERT(s!=0);
std::multimap<int, Link> links = s->getLinks();
// Setting true to make sure we save all visual
// words that could be referenced in a previously
// transferred node in LTM (#979)
reducedId = reduceNode(s->id(), 0, true);
if(reducedToOut) {
*reducedToOut = reducedId;
}
if(reducedId>0)
{
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor)
{
if(iter->second.type() == Link::kNeighborMerged)
if(_lastGlobalLoopClosureId == s->id())
{
// 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(_lastGlobalLoopClosureId == s->id())
{
_lastGlobalLoopClosureId = iter->first;
}
_lastGlobalLoopClosureId = iter->first;
}
}
// Setting true to make sure we save all visual
// words that could be referenced in a previously
// transferred node in LTM (#979)
this->moveToTrash(s, true);
s = 0;
}
}
}
if(s != 0)
if(reducedId == 0)
{
_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
_stMem.erase(*_stMem.begin());
}
// else already removed from STM/WM in moveToTrash()
// else already removed from STM/WM in reduceNode()
}
const Signature * Memory::getSignature(int id) const
@@ -1865,6 +2035,7 @@ void Memory::clear()
uInsert(parameters, parameters_);
parameters.erase(Parameters::kRtabmapWorkingDirectory()); // don't save working directory as it is machine dependent
UDEBUG("");
_dbDriver->setTimestampUpdateEnabled(true); // Only re-stamp if we updated the memory
_dbDriver->addInfoAfterRun(memSize,
_lastSignature?_lastSignature->id():0,
UProcessInfo::getMemoryUsage(),
@@ -1938,6 +2109,7 @@ void Memory::clear()
_dbDriver->join(true);
cleanUnusedWords();
_dbDriver->emptyTrashes();
_dbDriver->setTimestampUpdateEnabled(false);
}
_vwd->clear(_dbDriver!=NULL);
UDEBUG("");
@@ -2504,9 +2676,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
// If not saved to database
if(!keepLinkedToGraph)
{
UASSERT_MSG(this->isInSTM(s->id()),
UASSERT_MSG(this->isInSTM(s->id()) || this->isInWM(s->id()),
uFormat("Deleting location (%d) outside the "
"STM is not implemented!", s->id()).c_str());
"WM/STM is not implemented! STM size=%ld WM size=%ld",
s->id(), this->getStMem().size(), this->getWorkingMem().size()).c_str());
const std::multimap<int, Link> & links = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
@@ -2517,7 +2690,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
UASSERT_MSG(sTo!=0,
uFormat("A neighbor (%d) of the deleted location %d is "
"not found in WM/STM! Are you deleting a location "
"outside the STM?", iter->first, s->id()).c_str());
"outside the WM/STM?", iter->first, s->id()).c_str());
if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
{
@@ -2527,7 +2700,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
}
// child
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0)
if(iter->second.type() == Link::kGlobalClosure && s->getWeight()>0)
{
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
}
@@ -3626,7 +3799,7 @@ void Memory::updateLink(const Link & link, bool updateInDatabase)
if(oldType!=Link::kVirtualClosure || link.type()!=Link::kVirtualClosure)
{
_linksChanged = true;
_linksChanged = _incrementalMemory || (fromS->isSaved() && toS->isSaved());
}
}
else
@@ -5062,16 +5235,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
UASSERT(!decimatedData.cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
}
depthMask = util3d::filterFloor(depthMask, decimatedData.cameraModels(), _maskFloorThreshold);
UDEBUG("Masking floor done.");
}
@@ -5121,6 +5285,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
else
{
int oldMaxFeatures = _feature2D->getMaxFeatures();
bool oldSSC = _feature2D->getSSC();
UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
ParametersMap tmpMaxFeatureParameter;
if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
@@ -5128,6 +5293,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// The total extracted features should match the number of features used for transformation estimation
UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpSSC(), uNumber2Str(_visSSC)));
_feature2D->parseParameters(tmpMaxFeatureParameter);
}
@@ -5138,6 +5304,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(tmpMaxFeatureParameter.size())
{
tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
tmpMaxFeatureParameter.at(Parameters::kKpSSC()) = uBool2Str(oldSSC);
_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
}
t = timer.ticks();
@@ -5338,8 +5505,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
if((int)keypoints.size() > maxFeatures)
{
if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc);
if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()), ssc);
else
_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
}
@@ -5572,13 +5739,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
}
if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
if(decimatedData.cameraModels().size()>=1 || decimatedData.stereoCameraModels().size()>=1 ||
data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
Feature2D::limitKeypoints(
keypoints,
inliers,
_feature2D->getMaxFeatures(),
decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()):
decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()):
data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):
cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()),
_feature2D->getSSC());
}
else
@@ -5836,7 +6007,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
cameraModels.size() == 1 &&
words.size() &&
(words3D.size() == 0 || (words.size() == words3D.size() && words3DValid!=(int)words3D.size())) &&
_registrationPipeline->isImageRequired() &&
_signatures.size() &&
_signatures.rbegin()->second->mapId() == _idMapCount) // same map
{
@@ -5880,11 +6050,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// The following is used only to re-estimate the correspondences, the returned transform is ignored
Transform tmpt;
RegistrationVis reg(parameters_);
ParametersMap tmpParams = parameters_;
// Pure 2D-2D without guess would generate variance=1
uInsert(tmpParams, ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1"));
RegistrationVis reg(tmpParams);
if(_registrationPipeline->isScanRequired())
{
// If icp is used, remove it to just do visual registration
RegistrationVis vis(parameters_);
RegistrationVis vis(tmpParams);
tmpt = vis.computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
}
else
@@ -5906,11 +6079,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
}
float reprojError = Parameters::defaultVisPnPReprojError();
int varianceMedianRatio = Parameters::defaultVisPnPVarianceMedianRatio();
Parameters::parse(parameters_, Parameters::kVisPnPReprojError(), reprojError);
Parameters::parse(parameters_, Parameters::kVisPnPVarianceMedianRatio(), varianceMedianRatio);
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
currentWords,
previousWords,
cameraModels[0],
cameraTransform);
cameraTransform,
reprojError,
0.99f,
varianceMedianRatio);
UDEBUG("inliers=%d", (int)inliers.size());
@@ -6587,7 +6767,10 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
{
if(keys.at(i)>0)
{
_vwd->addWordRef(keys.at(i), (*j)->id());
if(_vwd->addWordRef(keys.at(i), (*j)->id()))
{
UERROR("Could not add word ref %d to node %d!?", keys.at(i), (*j)->id());
}
}
}
(*j)->setEnabled(true);
corelib/src/OdometryThread.cpp
+2 -2
View File
@@ -188,7 +188,7 @@ void OdometryThread::addData(const SensorEvent & event)
"(%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)."
"Current camera/lidar delay is %fs.",
"Current camera/lidar delay with system time is %fs.",
event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
notify = false;
}
@@ -197,7 +197,7 @@ void OdometryThread::addData(const SensorEvent & event)
"(%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). "
"Current camera/lidar delay is %fs.",
"Current camera/lidar delay with system time is %fs.",
event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
notify = false;
}
corelib/src/Optimizer.cpp
+5 -1
View File
@@ -217,6 +217,10 @@ LinkIdKey(int id, Link::Type type) :
{
return true;
}
else if(k.type_ == Link::kNeighborMerged && type_ != Link::kNeighbor && type_ != Link::kNeighborMerged)
{
return false;
}
else
{
// normal link, sort by smallest to largest id
@@ -256,7 +260,7 @@ void Optimizer::getConnectedGraph(
}
}
while(nextPoses.size())
while(!nextPoses.empty())
{
// Fill up all nodes before landmarks
// For nodes, fill up all neightbor nodes before loop closure ones
corelib/src/RegistrationIcp.cpp
+1
View File
@@ -529,6 +529,7 @@ Transform RegistrationIcp::computeTransformationImpl(
double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
UDEBUG("structural complexity: from=%f to=%f", fromComplexity, toComplexity);
if(complexity < _pointToPlaneMinComplexity)
{
tooLowComplexityForPlaneToPlane = true;
corelib/src/RegistrationVis.cpp
+23 -23
View File
@@ -84,6 +84,9 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_flowEps(Parameters::defaultVisCorFlowEps()),
_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
_flowGpu(Parameters::defaultVisCorFlowGpu()),
_flowUseMinEigenVals(Parameters::defaultVisCorFlowUseMinEigenVals()),
_flowMinEigThreshold(Parameters::defaultVisCorFlowMinEigThreshold()),
_flowErrorThreshold(Parameters::defaultVisCorFlowErrorThreshold()),
_nndr(Parameters::defaultVisCorNNDR()),
_nnType(Parameters::defaultVisCorNNType()),
_gmsWithRotation(Parameters::defaultGMSWithRotation()),
@@ -145,6 +148,9 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
Parameters::parse(parameters, Parameters::kVisCorFlowGpu(), _flowGpu);
Parameters::parse(parameters, Parameters::kVisCorFlowUseMinEigenVals(), _flowUseMinEigenVals);
Parameters::parse(parameters, Parameters::kVisCorFlowMinEigThreshold(), _flowMinEigThreshold);
Parameters::parse(parameters, Parameters::kVisCorFlowErrorThreshold(), _flowErrorThreshold);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
Parameters::parse(parameters, Parameters::kGMSWithRotation(), _gmsWithRotation);
@@ -321,6 +327,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
UDEBUG("%s=%f", Parameters::kVisPnPVarianceMedianRatio().c_str(), _PnPVarMedianRatio);
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -440,16 +447,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
UASSERT(!fromSignature.sensorData().cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
}
depthMask = util3d::filterFloor(depthMask, fromSignature.sensorData().cameraModels(), _maskFloorThreshold);
UDEBUG("Masking floor done.");
}
@@ -655,6 +653,7 @@ Transform RegistrationVis::computeTransformationImpl(
// Find features in the new left image
UDEBUG("guessSet = %d", guessSet?1:0);
std::vector<unsigned char> status;
std::vector<float> err;
#ifdef HAVE_OPENCV_CUDAOPTFLOW
if (_flowGpu)
{
@@ -684,7 +683,6 @@ Transform RegistrationVis::computeTransformationImpl(
else
#endif
{
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
imageFrom,
@@ -696,7 +694,8 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Size(_flowWinSize, _flowWinSize),
guessSet ? 0 : _flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, _flowIterations, _flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0), 1e-4);
(_flowUseMinEigenVals ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0) | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0),
_flowMinEigThreshold);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
}
@@ -705,11 +704,14 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
int ki = 0;
UASSERT((status.empty() || cornersTo.size() == status.size()) &&
(err.empty() || cornersTo.size() == err.size()));
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)) &&
(_flowUseMinEigenVals || err.empty() || err[i] < _flowErrorThreshold))
{
if(orignalWordsFromIdsCpy.size())
{
@@ -806,16 +808,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
UASSERT(!toSignature.sensorData().cameraModels().empty());
UDEBUG("Masking floor (threshold=%f)", _maskFloorThreshold);
if(_maskFloorThreshold<0.0f)
{
cv::Mat depthBelow;
util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold*-1.0f, &depthBelow);
depthMask = depthBelow;
}
else
{
depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
}
depthMask = util3d::filterFloor(depthMask, toSignature.sensorData().cameraModels(), _maskFloorThreshold);
UDEBUG("Masking floor done.");
}
@@ -1632,6 +1625,7 @@ Transform RegistrationVis::computeTransformationImpl(
cameraTransform,
_PnPReprojError,
0.99f,
_PnPVarMedianRatio,
words3A, // for scale estimation
&variance,
&matchesV);
@@ -2182,6 +2176,8 @@ Transform RegistrationVis::computeTransformationImpl(
// We take the second eigen value
info.inliersDistribution = pca_analysis.eigenvalues.at<float>(0, 1);
UDEBUG("Visual distribution: %f (eigen values = %f %f)", info.inliersDistribution, pca_analysis.eigenvalues.at<float>(0, 0), pca_analysis.eigenvalues.at<float>(0, 1));
if(info.inliersDistribution < _minInliersDistributionThr)
{
msg = uFormat("The distribution (%f) of inliers is under %s threshold (%f)",
@@ -2204,6 +2200,10 @@ Transform RegistrationVis::computeTransformationImpl(
info.matches = matchesCount;
info.rejectedMsg = msg;
info.covariance = covariance;
if(!covariance.empty())
{
info.variance = covariance.at<double>(0,0);
}
UDEBUG("inliers=%d/%d", info.inliers, info.matches);
UDEBUG("transform=%s", transform.prettyPrint().c_str());
corelib/src/Rtabmap.cpp
+140 -49
View File
@@ -385,19 +385,19 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
!_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first,
false, _optimizedPoses, cov, &_constraints);
}
if(!_optimizedPoses.empty())
if(_optimizedPoses.lower_bound(1) != _optimizedPoses.end())
{
if(_restartAtOrigin)
{
UWARN("last localization pose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str());
lastPose = _optimizedPoses.begin()->second;
UWARN("last localization pose is ignored (%s=true), assuming we start at the first node of the map.", Parameters::kRGBDStartAtOrigin().c_str());
lastPose = _optimizedPoses.lower_bound(1)->second;
}
_lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
_optimizedPoses.size(),
_optimizedPoses.begin()->first,
_optimizedPoses.begin()->second.prettyPrint().c_str(),
_optimizedPoses.lower_bound(1)->first,
_optimizedPoses.lower_bound(1)->second.prettyPrint().c_str(),
_lastLocalizationPose.prettyPrint().c_str());
if(_constraints.empty())
@@ -411,7 +411,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
UTimer time;
std::map<int, float> likelihood;
likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
for(std::map<int, Transform>::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter)
{
if(_memory->getSignature(iter->first))
{
@@ -507,6 +507,11 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
}
if(_memory)
{
if(_memory->isReadOnly() && databaseSaved)
{
UWARN("Database is read-only, latest optimized poses, latest localization pose and latest state of the memory are not saved.");
databaseSaved = false;
}
if(databaseSaved)
{
if(_memory->isGraphReduced() && _memory->isIncremental())
@@ -723,29 +728,24 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
isMemIncremental != _memory->isIncremental())
{
// Mode has changed from Mapping to Localization, cleanup the local graph
if(_memory->isGraphReduced() && _memory->isIncremental())
if(_memory->isIncremental())
{
// Force reducing graph, then remove filtered nodes from the optimized poses
std::map<int, int> reducedIds;
_memory->incrementMapId(&reducedIds);
for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
if(_memory->isGraphReduced())
{
_optimizedPoses.erase(iter->first);
// Force reducing graph, then remove filtered nodes from the optimized poses
std::map<int, int> reducedIds;
_memory->incrementMapId(&reducedIds);
for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
{
_optimizedPoses.erase(iter->first);
}
}
_odomCachePoses.clear();
_odomCacheConstraints.clear();
}
// In both cases, we save the latest optimized graph and latest localization pose
_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
// Mode changed from Localization to Mapping, clear local graph
if(!_memory->isIncremental()) {
_optimizedPoses.clear();
_lastLocalizationPose.setNull();
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
_localizationCovariance = cv::Mat();
_lastLocalizationNodeId = 0;
}
}
_memory->parseParameters(parameters);
@@ -760,12 +760,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
{
this->createGlobalScanMap();
}
if(_memory->isIncremental())
{
_odomCachePoses.clear();
_odomCacheConstraints.clear();
}
}
if(!_epipolarGeometry)
@@ -1789,7 +1783,7 @@ bool Rtabmap::process(
}
}
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(!_memory->isIncremental() && signature->getWeight() >= 0)
if(signature->getWeight() >= 0)
{
UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize);
if(!_odomCachePoses.empty())
@@ -2613,6 +2607,7 @@ bool Rtabmap::process(
int loopClosureVisualInliers = 0; // for statistics
float loopClosureVisualInliersRatio = 0.0f;
int loopClosureVisualMatches = 0;
float loopClosureVisualVariance = 0.0f;
float loopClosureLinearVariance = 0.0f;
float loopClosureAngularVariance = 0.0f;
float loopClosureVisualInliersMeanDist = 0;
@@ -2666,7 +2661,9 @@ bool Rtabmap::process(
std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
std::map<int, Transform> nearestPoses;
std::map<int, Transform> optimizedPosesWithOdomCache;
std::multimap<int, int> links;
std::map<int, Transform> * refPoses = &_optimizedPoses;
if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
{
// get bidirectional links
@@ -2678,6 +2675,25 @@ bool Rtabmap::process(
links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
}
}
if(_odomCachePoses.size() > 1)
{
// Add odometry cache if it contains a loop closure
// That could happen when we just switched from localization mode to
// mapping mode while being localized on the previous session.
optimizedPosesWithOdomCache = _optimizedPoses;
optimizedPosesWithOdomCache.insert(_odomCachePoses.begin(), _odomCachePoses.end());
refPoses = &optimizedPosesWithOdomCache;
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter)
{
if(uContains(optimizedPosesWithOdomCache, iter->second.from()) &&
uContains(optimizedPosesWithOdomCache, iter->second.to()) &&
iter->second.from() != iter->second.to())
{
links.insert(std::make_pair(iter->second.from(), iter->second.to()));
links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
}
}
}
}
for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
{
@@ -2685,7 +2701,7 @@ bool Rtabmap::process(
{
if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
{
std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first);
std::list<std::pair<int, Transform> > path = graph::computePath(*refPoses, links, signature->id(), iter->first);
UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
{
@@ -2795,6 +2811,7 @@ bool Rtabmap::process(
loopClosureVisualInliers = info.inliers;
loopClosureVisualInliersRatio = info.inliersRatio;
loopClosureVisualMatches = info.matches;
loopClosureVisualVariance = info.variance;
cv::Mat information = getInformation(info.covariance);
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
@@ -3062,6 +3079,7 @@ bool Rtabmap::process(
loopClosureVisualInliers = info.inliers;
loopClosureVisualInliersRatio = info.inliersRatio;
loopClosureVisualMatches = info.matches;
loopClosureVisualVariance = info.variance;
rejectedLoopClosure = transform.isNull();
if(rejectedLoopClosure)
{
@@ -4049,6 +4067,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
statistics_.addStatistic(Statistics::kLoopVisual_variance(), loopClosureVisualVariance);
statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
@@ -4309,6 +4328,20 @@ bool Rtabmap::process(
// If there is a too small displacement, remove the node
signaturesRemoved.push_back(signature->id());
_memory->deleteLocation(signature->id());
// Update odom cache (if we just switched from mapping mode to localization mode)
_odomCachePoses.erase(signature->id());
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
{
if(iter->second.from() == signature->id() || iter->second.to() == signature->id())
{
_odomCacheConstraints.erase(iter++);
}
else
{
++iter;
}
}
}
else
{
@@ -5636,7 +5669,8 @@ int Rtabmap::detectMoreLoopClosures(
bool intraSession,
bool interSession,
const ProgressState * processState,
float clusterRadiusMin)
float clusterRadiusMin,
int toFromMapId)
{
UDEBUG("");
UASSERT(iterations>0);
@@ -5663,17 +5697,23 @@ int Rtabmap::detectMoreLoopClosures(
std::map<int, Transform> posesToCheckLoopClosures;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
this->getGraph(poses, links, true, true, &signatures);
this->getGraph(poses, links, true, true);
std::map<int, int> mapIds;
UDEBUG("remove all invalid or intermediate nodes, fill mapIds");
for(std::map<int, Transform>::iterator iter=poses.upper_bound(0); iter!=poses.end();++iter)
{
if(signatures.at(iter->first).getWeight() >= 0)
Transform odom, gt;
int mapId, weight;
std::string l;
double s;
std::vector<float> v;
GPS gps;
EnvSensors srs;
if(_memory->getNodeInfo(iter->first, odom, mapId, weight, l, s, gt, v, gps, srs, true) && weight >= 0)
{
posesToCheckLoopClosures.insert(*iter);
mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId()));
mapIds.insert(std::make_pair(iter->first, mapId));
}
}
@@ -5687,7 +5727,56 @@ int Rtabmap::detectMoreLoopClosures(
clusterRadiusMax,
clusterAngle);
UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
UINFO("Looking for more loop closures: clustering poses... found %ld clusters.", clusters.size());
if(toFromMapId >=0)
{
size_t clustersBefore = clusters.size();
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
{
int mapId = uValue(mapIds, iter->first, 0);
if(mapId != toFromMapId)
{
iter = clusters.erase(iter);
}
else {
++iter;
}
}
UINFO("Looking for more loop closures: filtered %ld/%ld clusters for map session %d.", clustersBefore-clusters.size(), clustersBefore, toFromMapId);
if(clusters.empty())
{
UERROR("No clusters belong to mapId %d, aborting.", toFromMapId);
break;
}
}
if(_memory->getMaxStMemSize() > 1)
{
size_t clustersBefore = clusters.size();
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
{
if(abs(iter->first - iter->second) < _memory->getMaxStMemSize())
{
iter = clusters.erase(iter);
}
else
{
// compute path to know how far we are in terms of graph length
std::map<int, int> ids = _memory->getNeighborsId(iter->first, _memory->getMaxStMemSize(), -1, true, true, true);
if(ids.find(iter->second) != ids.end())
{
iter = clusters.erase(iter);
}
else
{
++iter;
}
}
}
UINFO("Looking for more loop closures: filtered %ld/%ld clusters for too close nodes (below %s=%d).",
clustersBefore-clusters.size(), clustersBefore, Parameters::kMemSTMSize().c_str(), _memory->getMaxStMemSize());
}
int i=0;
std::set<int> addedLinks;
@@ -5739,8 +5828,10 @@ int Rtabmap::detectMoreLoopClosures(
{
checkedLoopClosures.insert(std::make_pair(from, to));
UASSERT(signatures.find(from) != signatures.end());
UASSERT(signatures.find(to) != signatures.end());
Signature fromS = getSignatureCopy(from, false, true, false, false, true, false);
Signature toS = getSignatureCopy(to, false, true, false, false, true, false);
UASSERT(fromS.getWeight()>=0);
UASSERT(toS.getWeight()>=0);
Transform guess;
if(_proximityBySpace && uContains(poses, from) && uContains(poses, to))
@@ -5750,7 +5841,7 @@ int Rtabmap::detectMoreLoopClosures(
RegistrationInfo info;
// use signatures instead of IDs because some signatures may not be in WM
Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
Transform t = _memory->computeTransform(fromS, toS, guess, &info);
if(!t.isNull())
{
@@ -5759,11 +5850,11 @@ int Rtabmap::detectMoreLoopClosures(
//optimize the graph to see if the new constraint is globally valid
int fromId = from;
int mapId = signatures.at(from).mapId();
int mapId = fromS.mapId();
// use first node of the map containing from
for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
for(std::map<int, Transform>::iterator ster=posesToCheckLoopClosures.begin(); ster!=posesToCheckLoopClosures.end(); ++ster)
{
if(ster->second.mapId() == mapId)
if(uValue(mapIds, ster->first, 0) == mapId)
{
fromId = ster->first;
break;
@@ -5778,22 +5869,22 @@ int Rtabmap::detectMoreLoopClosures(
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
std::map<int, Transform> optimizedPoses;
std::multimap<int, Link> links;
std::multimap<int, Link> linksOut;
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, linksOut);
UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT(graph::findLink(links, from, to) != links.end());
optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)linksOut.size()).c_str());
UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)linksOut.size()).c_str());
UASSERT(graph::findLink(linksOut, from, to) != linksOut.end());
optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, linksOut);
std::string msg;
if(optimizedPoses.size())
{
graph::computeMaxGraphErrors(
optimizedPoses,
links,
linksOut,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
corelib/src/Stereo.cpp
+22 -6
View File
@@ -120,6 +120,9 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
Stereo(parameters),
epsilon_(Parameters::defaultStereoEps()),
useMinEigenVals_(Parameters::defaultStereoUseMinEigenVals()),
minEigThreshold_(Parameters::defaultStereoMinEigThreshold()),
errorThreshold_(Parameters::defaultStereoErrorThreshold()),
gpu_(Parameters::defaultStereoGpu())
{
this->parseParameters(parameters);
@@ -129,6 +132,9 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
{
Stereo::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
Parameters::parse(parameters, Parameters::kStereoUseMinEigenVals(), useMinEigenVals_);
Parameters::parse(parameters, Parameters::kStereoMinEigThreshold(), minEigThreshold_);
Parameters::parse(parameters, Parameters::kStereoErrorThreshold(), errorThreshold_);
Parameters::parse(parameters, Parameters::kStereoGpu(), gpu_);
#ifndef HAVE_OPENCV_CUDAOPTFLOW
if(gpu_)
@@ -185,11 +191,18 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
err,
this->winSize(),
this->maxLevel(),
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), this->epsilon()),
this->usingMinEigenVals() ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0, this->minEigThreshold());
UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
}
updateStatus(leftCorners, rightCorners, status);
if(this->usingMinEigenVals())
{
updateStatus(leftCorners, rightCorners, status);
}
else
{
updateStatus(leftCorners, rightCorners, status, err);
}
return rightCorners;
}
@@ -241,14 +254,17 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
void StereoOpticalFlow::updateStatus(
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
std::vector<unsigned char> & status) const
std::vector<unsigned char> & status,
std::vector<float> err) const
{
UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
UASSERT(
leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size() &&
(err.empty() || err.size() == leftCorners.size()));
int countFlowRejected = 0;
int countDisparityRejected = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i]!=0)
if(status[i]!=0 && (err.empty() || err[i] < this->errorThreshold()))
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity <= this->minDisparity() || disparity > this->maxDisparity())
corelib/src/Transform.cpp
+25
View File
@@ -52,6 +52,26 @@ Transform::Transform(
r11, r12, r13, o14,
r21, r22, r23, o24,
r31, r32, r33, o34);
if( r11>0.0f || r12>0.0f || r13>0.0f ||
r21>0.0f || r22>0.0f || r23>0.0f ||
r31>0.0f || r32>0.0f || r33>0.0f)
{
Eigen::Matrix3f m;
m << r11, r12, r13,
r21, r22, r23,
r31, r32, r33;
float d = m.determinant();
if(fabs(d-1.0f) > 0.0001)
{
UWARN("Created transform doesn't have normalized rotation. Any transformation with this transform can cause unexpected results!"
" Determinant([%f %f %f;%f %f %f;%f %f %f])=%f",
r11, r12, r13,
r21, r22, r23,
r31, r32, r33,
d);
}
}
}
Transform::Transform(const cv::Mat & transformationMatrix)
@@ -509,6 +529,11 @@ Transform Transform::fromString(const std::string & string)
numbers[4], numbers[5], numbers[6], numbers[7],
numbers[8], numbers[9], numbers[10], numbers[11]);
}
// Always normalize
if(!t.isNull())
{
t.normalizeRotation();
}
return t;
}
corelib/src/VWDictionary.cpp
+28 -9
View File
@@ -571,19 +571,36 @@ void VWDictionary::update()
else if(_strategy >= kNNBruteForce &&
_notIndexedWords.size() &&
_removedIndexedWords.size() == 0 &&
_visualWords.size() &&
_dataTree.rows)
_visualWords.size())
{
const int IMGIDX_SHIFT = 18;
const int IMGIDX_ONE = (1 << IMGIDX_SHIFT); // a limit defined in https://github.com/opencv/opencv/blob/4.x/modules/features2d/src/matchers.cpp
if(_dataTree.rows >= IMGIDX_ONE)
{
UWARN("%s=%d is not a FLANN strategy and the number of words in the vocabulary (%d) is over %d (IMGIDX_ONE), so opencv may "
"assert on an IMGIDX_ONE check when adding new words. Use a FLANN strategy instead (%s<%d).",
Parameters::kKpNNStrategy().c_str(), _strategy, _dataTree.rows, IMGIDX_ONE, Parameters::kKpNNStrategy().c_str(), kNNBruteForce);
}
//just add not indexed words
int i = _dataTree.rows;
_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
if(!_dataTree.empty()) {
_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
}
for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
{
VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
UASSERT(w);
UASSERT(w->getDescriptor().cols == _dataTree.cols);
UASSERT(w->getDescriptor().type() == _dataTree.type());
_dataTree.push_back(w->getDescriptor());
if(_dataTree.empty())
{
_dataTree = w->getDescriptor().clone();
}
else
{
UASSERT(w->getDescriptor().cols == _dataTree.cols);
UASSERT(w->getDescriptor().type() == _dataTree.type());
_dataTree.push_back(w->getDescriptor());
}
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, w->id()));
std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), i));
UASSERT(inserted.second);
@@ -865,7 +882,7 @@ int VWDictionary::getNextId()
return ++_lastWordId;
}
void VWDictionary::addWordRef(int wordId, int signatureId)
bool VWDictionary::addWordRef(int wordId, int signatureId)
{
VisualWord * vw = 0;
vw = uValue(_visualWords, wordId, vw);
@@ -875,10 +892,12 @@ void VWDictionary::addWordRef(int wordId, int signatureId)
_totalActiveReferences += 1;
_unusedWords.erase(vw->id());
return true;
}
else
{
UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
UWARN("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
return false;
}
}
@@ -1001,7 +1020,7 @@ std::list<int> VWDictionary::addNewWords(
if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
{
//Find nearest neighbors
UDEBUG("newPts.total()=%d ", descriptors.rows);
UDEBUG("newPts.total()=%d _strategy=%d", descriptors.rows, _strategy);
if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
{
corelib/src/camera/CameraImages.cpp
+50 -31
View File
@@ -63,6 +63,7 @@ CameraImages::CameraImages() :
_syncImageRateWithStamps(true),
_odometryFormat(0),
_groundTruthFormat(0),
_groundTruthLocalTransform(Transform::getIdentity()),
_maxPoseTimeDiff(0.02),
_captureDelay(0.0)
{}
@@ -93,6 +94,7 @@ CameraImages::CameraImages(const std::string & path,
_syncImageRateWithStamps(true),
_odometryFormat(0),
_groundTruthFormat(0),
_groundTruthLocalTransform(Transform::getIdentity()),
_maxPoseTimeDiff(0.02),
_captureDelay(0.0)
{
@@ -478,27 +480,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
if(success && _odometryPath.size() && odometry_.empty())
{
success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
if(!success)
{
UERROR("Failed to read odometry poses.");
}
if(success)
{
for(size_t i=0; i<odometry_.size(); ++i)
{
// linear cov = 0.0001
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
if(i!=0)
{
// angular cov = 0.000001
covariance.at<double>(3,3) *= 0.01;
covariance.at<double>(4,4) *= 0.01;
covariance.at<double>(5,5) *= 0.01;
}
covariances_.push_back(covariance);
}
}
}
if(success && _groundTruthPath.size())
{
success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
}
if(!odometry_.empty())
{
for(size_t i=0; i<odometry_.size(); ++i)
if(!success)
{
// linear cov = 0.0001
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
if(i!=0)
UERROR("Failed to read ground truth poses.");
}
else if(!_groundTruthLocalTransform.isIdentity())
{
Transform gtInv = _groundTruthLocalTransform.inverse();
for(auto pose: groundTruth_)
{
// angular cov = 0.000001
covariance.at<double>(3,3) *= 0.01;
covariance.at<double>(4,4) *= 0.01;
covariance.at<double>(5,5) *= 0.01;
pose = pose*gtInv; // pose of base_link, assuming ground truth frame and base frame are rigidly fixed
}
covariances_.push_back(covariance);
}
}
}
@@ -607,7 +625,7 @@ bool CameraImages::readPoses(
}
if(validPoses != (int)inOutStamps.size())
{
UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
UWARN("%d/%ld valid poses of %ld stamps", validPoses, outputPoses.size(), inOutStamps.size());
}
}
else
@@ -756,32 +774,19 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
if(_stamps.size())
{
stamp = _stamps.front();
_stamps.pop_front();
if(_stamps.size())
{
_captureDelay = _stamps.front() - stamp;
}
UERROR("stamps cannot be used when startAt < 0");
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
{
covariance = covariances_.front();
covariances_.pop_front();
}
UERROR("odometry cannot be used when startAt < 0");
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
UERROR("groundTruth cannot be used when startAt < 0");
}
if(_models.size() && !model.isValidForProjection())
{
model = _models.front();
_models.pop_front();
UERROR("models cannot be used when startAt < 0");
}
}
else
@@ -792,6 +797,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
{
imageFilePath = _path + imageFileName;
scanFilePath = _scanPath + scanFileName;
size_t stampsSize = _stamps.size();
if(_stamps.size())
{
stamp = _stamps.front();
@@ -803,6 +809,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
}
if(odometry_.size())
{
UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(),
uFormat("Stamps=%ld odometry=%ld", _stamps.size(), odometry_.size()).c_str());
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
@@ -813,11 +821,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
}
if(groundTruth_.size())
{
UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(),
uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str());
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
if(_models.size() && !model.isValidForProjection())
{
UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(),
uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
model = _models.front();
_models.pop_front();
}
@@ -834,6 +846,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
imageFilePath = _path + imageFileName;
scanFilePath = _scanPath + scanFileName;
size_t stampsSize = _stamps.size();
if(_stamps.size())
{
stamp = _stamps.front();
@@ -845,6 +858,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
}
if(odometry_.size())
{
UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(),
uFormat("Stamps=%ld odometry=%ld", stampsSize, odometry_.size()).c_str());
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
@@ -855,11 +870,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
}
if(groundTruth_.size())
{
UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(),
uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str());
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
if(_models.size() && !model.isValidForProjection())
{
UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(),
uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
model = _models.front();
_models.pop_front();
}
corelib/src/camera/CameraRealSense2.cpp
+98 -56
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UDirectory.h>
#include <opencv2/imgproc/types_c.h>
#ifdef RTABMAP_REALSENSE2
@@ -78,7 +79,8 @@ CameraRealSense2::CameraRealSense2(
cameraDepthFps_(30),
globalTimeSync_(true),
dualMode_(false),
closing_(false)
closing_(false),
playback_(false)
#endif
{
UDEBUG("");
@@ -130,6 +132,7 @@ void CameraRealSense2::close()
}
closing_ = false;
playback_ = false;
}
void CameraRealSense2::imu_callback(rs2::frame frame)
@@ -492,64 +495,76 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
clockSyncWarningShown_ = false;
imuGlobalSyncWarningShown_ = false;
rs2::device_list list = ctx_.query_devices();
if (0 == list.size())
if(uStrContains(deviceId_, ".bag"))
{
UERROR("No RealSense2 devices were found!");
return false;
// playback (bag recorded by realsense-viewer)
dev_.resize(1);
dev_[0] = ctx_.load_device(uReplaceChar(deviceId_, '~', UDirectory::homeDir()));
playback_ = true;
UINFO("Device ID is a bag (\"%s\"), using playback mode", deviceId_.c_str());
}
bool found=false;
try
else
{
for (rs2::device dev : list)
rs2::device_list list = ctx_.query_devices();
if (0 == list.size())
{
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
UERROR("No RealSense2 devices were found!");
return false;
}
uint16_t pid;
std::stringstream ss;
ss << std::hex << pid_str;
ss >> pid;
UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
if(dualMode_ && pid == 0x0B37)
bool found=false;
try
{
for (rs2::device dev : list)
{
// Dual setup: device[0] = D400, device[1] = T265
// T265
dev_.resize(2);
dev_[1] = dev;
}
else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
{
if(dev_.empty())
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
auto name = dev.get_info(RS2_CAMERA_INFO_NAME);
uint16_t pid;
std::stringstream ss;
ss << std::hex << pid_str;
ss >> pid;
UINFO("Device \"%s\" with serial number %s was found with product ID=%d.", name, sn, (int)pid);
if(dualMode_ && pid == 0x0B37)
{
dev_.resize(1);
// Dual setup: device[0] = D400, device[1] = T265
// T265
dev_.resize(2);
dev_[1] = dev;
}
else if (!found && (deviceId_.empty() || deviceId_ == sn || uStrContains(name, uToUpperCase(deviceId_))))
{
if(dev_.empty())
{
dev_.resize(1);
}
dev_[0] = dev;
found=true;
}
dev_[0] = dev;
found=true;
}
}
}
catch(const rs2::error & error)
{
UWARN("%s. Is the camera already used with another app?", error.what());
catch(const rs2::error & error)
{
UWARN("%s. Is the camera already used with another app?", error.what());
}
if (!found)
{
if(dualMode_ && dev_.size()==2)
{
UERROR("Dual setup is enabled, but a D400 camera is not detected!");
dev_.clear();
}
else
{
UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
}
return false;
}
}
if (!found)
{
if(dualMode_ && dev_.size()==2)
{
UERROR("Dual setup is enabled, but a D400 camera is not detected!");
dev_.clear();
}
else
{
UERROR("The requested device \"%s\" is NOT found!", deviceId_.c_str());
}
return false;
}
else if(dualMode_ && dev_.size()!=2)
if(dualMode_ && dev_.size()!=2)
{
UERROR("Dual setup is enabled, but a T265 camera is not detected!");
dev_.clear();
@@ -634,7 +649,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
sensors[1] = elem;
if(sensors[1].supports(rs2_option::RS2_OPTION_EMITTER_ENABLED))
{
sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
if(!sensors[1].is_option_read_only(rs2_option::RS2_OPTION_EMITTER_ENABLED))
{
sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
}
else if(!emitterEnabled_)
{
UWARN("rs2_option::RS2_OPTION_EMITTER_ENABLED option is read-only, cannot disable IR emitter.");
}
}
}
else if ("Coded-Light Depth Sensor" == module_name)
@@ -732,7 +754,8 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
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 &&
((strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 ||
(playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0)) &&
video_profile.width() == cameraDepthWidth_ &&
video_profile.height() == cameraDepthHeight_ &&
video_profile.fps() == cameraDepthFps_))
@@ -741,7 +764,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
// rgb or ir left
if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) ||
(ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
(ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
{
if(!profilesPerSensor[i].empty())
{
@@ -880,8 +903,17 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
if (!added)
{
UERROR("Given stream configuration is not supported by the device! "
"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
if(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 ||
(playback_ && strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "Stereo Module")==0))
{
UERROR("Given stream configuration is not supported by the device! "
"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraDepthWidth_, cameraDepthHeight_, cameraDepthFps_);
}
else
{
UERROR("Given stream configuration is not supported by the device! "
"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
}
UERROR("Available configurations:");
for (auto& profile : profiles)
{
@@ -1087,9 +1119,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
}
if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
{
float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
float value = sensors[i].get_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED);
UINFO("Set RS2_OPTION_GLOBAL_TIME_ENABLED=1 (was %f) for sensor %d", value, (int)i);
if(!sensors[i].is_option_read_only(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
{
sensors[i].set_option(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED, 1);
}
else if(value != 1)
{
UWARN("rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED option is read-only, cannot enable it.");
}
}
sensors[i].open(profilesPerSensor[i]);
if(sensors[i].is<rs2::depth_sensor>())
@@ -1538,7 +1577,10 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
}
catch(const std::exception& ex)
{
UERROR("An error has occurred during frame callback: %s", ex.what());
if(!playback_)
{
UERROR("An error has occurred during frame callback: %s", ex.what());
}
}
#else
UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
corelib/src/global_map/OccupancyGrid.cpp
-75
View File
@@ -92,7 +92,6 @@ void OccupancyGrid::clear()
{
map_ = cv::Mat();
mapInfo_ = cv::Mat();
cellCount_.clear();
GlobalMap::clear();
}
@@ -433,11 +432,6 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
{
addAssembledNode(kter->first, kter->second);
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
if(cter == cellCount_.end() && kter->first > 0)
{
cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
}
if(iter!=emptyLocalMaps.end())
{
for(int i=0; i<iter->second.cols; ++i)
@@ -459,30 +453,12 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId > 0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.first += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = ptf[0];
info[2] = ptf[1];
cter->second.first+=1;
}
value = 0; // free space
@@ -533,23 +509,6 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId>0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.first += 1;
}
}
}
if(kter->first > 0)
{
@@ -557,7 +516,6 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
info[1] = float(i) * cellSize_ + xMin;
info[2] = float(j) * cellSize_ + yMin;
info[3] = logOddsClampingMin_;
cter->second.first+=1;
}
value = -2; // free space (footprint)
}
@@ -585,30 +543,12 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId>0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.second += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = ptf[0];
info[2] = ptf[1];
cter->second.second+=1;
}
// update odds
@@ -651,20 +591,6 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
mapInfo_ = mapInfo;
minValues_[0] = xMin;
minValues_[1] = yMin;
// clean cellCount_
for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
{
UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
if(iter->second.first == 0 && iter->second.second == 0)
{
cellCount_.erase(iter++);
}
else
{
++iter;
}
}
}
}
@@ -677,7 +603,6 @@ unsigned long OccupancyGrid::getMemoryUsed() const
memoryUsage += map_.total() * map_.elemSize();
memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::map<int, std::pair<int, int> >::iterator)) + sizeof(std::map<int, std::pair<int, int> >);
return memoryUsage;
}
corelib/src/odometry/OdometryMono.cpp
+1
View File
@@ -775,6 +775,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
cameraTransform,
fundMatrixReprojError_,
fundMatrixConfidence_,
4,
refWords3Guess); // for scale estimation
if(cameraTransform.getNorm() < minTranslation_*5)
corelib/src/odometry/OdometryORBSLAM3.cpp
+8 -1
View File
@@ -430,7 +430,14 @@ Transform OdometryORBSLAM3::computeTransform(
(data.stereoCameraModels().size() == 1 &&
data.stereoCameraModels()[0].isValidForProjection())))
{
UERROR("Invalid camera model!");
if(data.cameraModels().size() > 1 || data.stereoCameraModels().size() > 1)
{
UERROR("Multi-camera not supported with ORB_SLAM integration!");
}
else
{
UERROR("Invalid camera model!");
}
return t;
}
corelib/src/optimizer/OptimizerG2O.cpp
+127 -100
View File
@@ -61,7 +61,6 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/edge_se3_xyzprior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
#include "g2o/edge_se3_gravity.h"
#include "g2o/edge_sbacam_gravity.h"
#include "g2o/edge_xy_prior.h" // Include after types_slam2d.h to be ignored on newest g2o versions
#include "g2o/edge_xyz_prior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
#ifdef G2O_HAVE_CSPARSE
@@ -77,6 +76,19 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
#include "g2o/types/types_sba.h"
#include "g2o/types/types_six_dof_expmap.h"
#include "g2o/solvers/linear_solver_eigen.h"
#include "g2o/edge_se3_expmap.h"
#endif
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
namespace rtabmap {
#ifdef RTABMAP_ORB_SLAM
typedef g2o::VertexSE3Expmap VertexCam;
#else
typedef g2o::VertexCam VertexCam;
#endif
}
#include "g2o/edge_sbacam_gravity.h"
#include "g2o/edge_sbacam_prior.h"
#endif
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
@@ -1414,81 +1426,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
return optimizedPoses;
}
#ifdef RTABMAP_ORB_SLAM
/**
* \brief 3D edge between two SBAcam
*/
class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeSE3Expmap(): BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
bool read(std::istream& is)
{
return false;
}
bool write(std::ostream& os) const
{
return false;
}
void computeError()
{
const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
_error[0]=delta.translation().x();
_error[1]=delta.translation().y();
_error[2]=delta.translation().z();
_error[3]=delta.rotation().x();
_error[4]=delta.rotation().y();
_error[5]=delta.rotation().z();
}
virtual void setMeasurement(const g2o::SE3Quat& meas){
_measurement=meas;
_inverseMeasurement=meas.inverse();
}
virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
if (from_.count(from) > 0)
to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
else
from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
}
virtual bool setMeasurementData(const double* d){
Eigen::Map<const g2o::Vector7d> v(d);
_measurement.fromVector(v);
_inverseMeasurement = _measurement.inverse();
return true;
}
virtual bool getMeasurementData(double* d) const{
Eigen::Map<g2o::Vector7d> v(d);
v = _measurement.toVector();
return true;
}
virtual int measurementDimension() const {return 7;}
virtual bool setMeasurementFromState() {
const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
_measurement = (v1->estimate().inverse()*v2->estimate());
_inverseMeasurement = _measurement.inverse();
return true;
}
protected:
g2o::SE3Quat _inverseMeasurement;
};
#endif
std::map<int, Transform> OptimizerG2O::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
@@ -1559,7 +1496,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#endif // RTABMAP_ORB_SLAM
#ifndef RTABMAP_ORB_SLAM
if(optimizer_ == 1)
// ISSUE: It seems the fatal error
// "[SetJac] infinite jac" happens relatively
// easily with GaussNewton on SBA problem,
// ignore optimizer_ and always use Levenberg for SBA.
// TODO: Note that g2o/RobustKernelDelta parameter could be
// potentially tuned to avoid that error with GaussNewton.
if(0)//optimizer_ == 1)
{
#ifdef RTABMAP_G2O_CPP11
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
@@ -1579,8 +1522,23 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#endif
}
// detect if there are gravity constraints
bool hasGravityConstraints = false;
if(!isSlam2d() && gravitySigma() > 0)
{
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if( iter->second.from() == iter->second.to() &&
iter->second.type() == Link::kGravity)
{
hasGravityConstraints = true;
break;
}
}
}
UDEBUG("fill poses to g2o...");
UDEBUG("fill %ld poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", poses.size(), rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
@@ -1596,11 +1554,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// Add node's pose
UASSERT(!camPose.isNull());
#ifdef RTABMAP_ORB_SLAM
g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
#else
g2o::VertexCam * vCam = new g2o::VertexCam();
#endif
rtabmap::VertexCam * vCam = new rtabmap::VertexCam();
Eigen::Affine3d a = camPose.toEigen3d();
#ifdef RTABMAP_ORB_SLAM
@@ -1619,7 +1574,65 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
vCam->setId(iter->first*MULTICAM_OFFSET + i);
// negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
bool fixNode = (rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId);
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
if(this->isSlam2d())
{
if(fixNode)
{
UDEBUG("Set node %d fixed", iter->first);
vCam->setFixed(true);
}
else if(i==0) // Only set prior on the first camera
{
// add a singleton constraint that locks the position of the robot on the plane
EdgeSBACamPrior* planeConstraint = new EdgeSBACamPrior();
Eigen::Matrix<double, 6, 6> pinfo = Eigen::Matrix<double, 6, 6>::Zero();
pinfo(2, 2) = 1e9;
planeConstraint->setInformation(pinfo);
g2o::SE3Quat fixedZ = g2o::SE3Quat();
fixedZ.setTranslation(Eigen::Vector3d(0,0,iter->second.z()));
planeConstraint->setMeasurement(fixedZ);
Eigen::Affine3d a = iterModel->second[i].localTransform().inverse().toEigen3d();
planeConstraint->setCameraInvLocalTransform(g2o::SE3Quat(a.linear(), a.translation()));
planeConstraint->vertices()[0] = vCam;
optimizer.addEdge(planeConstraint);
}
}
else if(fixNode)
{
if(rootId < 0 || !hasGravityConstraints)
{
UDEBUG("Set node %d fixed", iter->first);
vCam->setFixed(true);
}
else if(hasGravityConstraints && i==0) // Only set prior on the first camera in case of multi-cam
{
// Setup root prior (fixed x,y,z,yaw)
EdgeSBACamPrior * e = new EdgeSBACamPrior();
e->vertices()[0] = vCam;
Eigen::Affine3d a = iter->second.toEigen3d();
e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
a = iterModel->second[i].localTransform().inverse().toEigen3d();
e->setCameraInvLocalTransform(g2o::SE3Quat(a.linear(), a.translation()));
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
// pitch and roll not fixed
information(3,3) = information(4,4) = 1;
e->setInformation(information);
if (!optimizer.addEdge(e))
{
delete e;
UERROR("Map: Failed adding fixed constraint of node %d, set as fixed instead", iter->first);
vCam->setFixed(true);
}
else
{
UDEBUG("Set node %d fixed with prior (have gravity constraints)", iter->first);
}
}
}
/*UDEBUG("camPose %d (camid=%d) (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first,
@@ -1632,8 +1645,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
iterModel->second[i].Tx(),
iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_,
camPose.prettyPrint().c_str());*/
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
}
}
}
@@ -1652,7 +1663,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(id1 == id2)
{
#ifndef RTABMAP_ORB_SLAM
g2o::HyperGraph::Edge * edge = 0;
if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
@@ -1666,7 +1676,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma());
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
rtabmap::VertexCam* v1 = (rtabmap::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity());
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
// Gravity constraint added only to first camera of a pose
@@ -1683,7 +1693,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
return optimizedPoses;
}
#endif
}
else if(id1>0 && id2>0) // not supporting landmarks
{
@@ -1839,14 +1848,14 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
g2o::OptimizableGraph::Edge * e;
double baseline = 0.0;
rtabmap::VertexCam* vcam = dynamic_cast<rtabmap::VertexCam*>(optimizer.vertex(camId));
#ifdef RTABMAP_ORB_SLAM
g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(poseId);
UASSERT(iterModel != models.end() && camIndex<iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
UASSERT(iterModel != models.end() && camIndex<(int)iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
baseline = iterModel->second[camIndex].Tx()<0.0?-iterModel->second[camIndex].Tx()/iterModel->second[camIndex].fx():baseline_;
#else
g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
baseline = vcam->estimate().baseline;
#endif
double variance = pixelVariance_;
@@ -1944,7 +1953,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(uIsNan(chi2))
{
UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current %s=%d) or solver (current %s=%d).",
Parameters::kg2oOptimizer().c_str(), optimizer_, Parameters::kg2oSolver().c_str(), solver_);
return optimizedPoses;
}
UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
@@ -1978,15 +1988,18 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
#endif
cv::Point3f pt3d;
int id=-1;
if((*iter)->vertex(0)->id() > negVertexOffset)
{
pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
id = negVertexOffset - (*iter)->vertex(0)->id();
}
else
{
pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
id = (*iter)->vertex(0)->id() - stepVertexId;
}
UASSERT_MSG(points3DMap.find(id) != points3DMap.end(), uFormat("word id=%d points3DMap=%ld vertex id=%d (negVertexOffset=%d stepVertexId=%d)",
id, points3DMap.size(), (*iter)->vertex(0)->id(), negVertexOffset, stepVertexId).c_str());
cv::Point3f pt3d = points3DMap.at(id);
((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
if(outliers)
@@ -2043,12 +2056,26 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
return optimizedPoses;
}
// FIXME: is there a way that we can add the 2D constraint directly in SBA?
if(this->isSlam2d())
{
// get transform between old and new pose
t = iter->second.inverse() * t;
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
// The optimized poses should be already fixed to original height,
// but it may have varied a little (not exaclty the same number).
// Here we just put back the original z value.
if(fabs(t.z() - iter->second.z()) < 0.001)
{
t.z() = iter->second.z();
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
}
else
{
UWARN("Planar constraints didn't work!? original pose (%d), pose %s -> %s. Falling back to old approach.",
iter->first,
iter->second.prettyPrint().c_str(),
t.prettyPrint().c_str());
// get transform between old and new pose
t = iter->second.inverse() * t;
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
}
}
else
{
corelib/src/optimizer/OptimizerTORO.cpp
+1 -1
View File
@@ -543,7 +543,7 @@ bool OptimizerTORO::loadGraph(
}
else
{
UFATAL("Referred poses from the link not exist!");
UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo);
}
}
else if(strList.size())
corelib/src/optimizer/ceres/pose_graph_2d/angle_manifold.h
+2 -2
View File
@@ -77,7 +77,7 @@ class AngleManifold {
#else
class AngleManfold {
class AngleManifold {
public:
template <typename T>
@@ -90,7 +90,7 @@ class AngleManfold {
}
static ceres::LocalParameterization* Create() {
return (new ceres::AutoDiffLocalParameterization<AngleManfold, 1, 1>);
return (new ceres::AutoDiffLocalParameterization<AngleManifold, 1, 1>);
}
};
corelib/src/optimizer/g2o/edge_sbacam_gravity.h
+30 -19
View File
@@ -32,18 +32,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef RTAB_G2O_EDGE_SBACAM_GRAVITY_H_
#define RTAB_G2O_EDGE_SBACAM_GRAVITY_H_
#ifdef RTABMAP_ORB_SLAM
#include "g2o/types/types_six_dof_expmap.h"
#else
#include "g2o/types/sba/types_sba.h"
#endif
#include "g2o/core/base_unary_edge.h"
namespace rtabmap {
/**
* \brief EdgeSBACamGravity
* \brief g2o edge with gravity constraint
*/
class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, g2o::VertexCam> {
class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, VertexCam> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSBACamGravity(){
information().setIdentity();
cameraInvLocalTransform_.setIdentity();
}
virtual bool read(std::istream& is) {return false;} // not implemented
virtual bool write(std::ostream& os) const {return false;} // not implemented
@@ -55,30 +60,36 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6,
// return the error estimate as a 3-vector
void computeError(){
const g2o::VertexCam* v1 = static_cast<const g2o::VertexCam*>(_vertices[0]);
const VertexCam* v = static_cast<const VertexCam*>(_vertices[0]);
Eigen::Vector3d direction = _measurement.head<3>();
Eigen::Vector3d measurement = _measurement.tail<3>();
Eigen::Vector3d direction = _measurement.head<3>();
Eigen::Vector3d measurement = _measurement.tail<3>();
Eigen::Vector3d ea;
g2o::SE3Quat estimate;
#ifdef RTABMAP_ORB_SLAM
estimate = v->estimate().inverse();
#else
estimate = v->estimate();
#endif
// Transform pose from camera frame to world frame
Eigen::Matrix3d t = v1->estimate().rotation().toRotationMatrix() * cameraInvLocalTransform_;
ea[0] = atan2(t (2, 1), t (2, 2));
ea[1] = asin(-t (2, 0));
ea[2] = atan2(t (1, 0), t (0, 0));
// Transform pose from camera frame to world frame
Eigen::Matrix3d t = estimate.rotation().toRotationMatrix() * cameraInvLocalTransform_;
Eigen::Vector3d ea;
ea[0] = atan2(t (2, 1), t (2, 2));
ea[1] = asin(-t (2, 0));
ea[2] = atan2(t (1, 0), t (0, 0));
Eigen::Matrix3d rot =
(Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX())).toRotationMatrix();
Eigen::Matrix3d rot =
(Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX())).toRotationMatrix();
Eigen::Vector3d estimate = rot * -direction;
_error = estimate - measurement;
Eigen::Vector3d newEstimate = rot * -direction;
_error = newEstimate - measurement;
/*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(),
measurement[0], measurement[1], measurement[2],
estimate[0], estimate[1], estimate[2],
_error[0], _error[1], _error[2]);*/
/*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(),
measurement[0], measurement[1], measurement[2],
estimate[0], estimate[1], estimate[2],
_error[0], _error[1], _error[2]);*/
}
// 6 values:
corelib/src/optimizer/g2o/edge_sbacam_prior.h
+135
View File
@@ -0,0 +1,135 @@
/*
Copyright (c) 2010-2019, 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.
*/
/**
* Adapted from EdgeSE3Prior
*/
#ifndef RTAB_G2O_EDGE_SBACAM_PRIOR_H_
#define RTAB_G2O_EDGE_SBACAM_PRIOR_H_
#ifdef RTABMAP_ORB_SLAM
#include "g2o/types/types_six_dof_expmap.h"
#else
#include "g2o/types/sba/types_sba.h"
#endif
#include "g2o/core/base_unary_edge.h"
namespace rtabmap {
/**
* \brief EdgeSBACamPrior
* \brief g2o edge with gravity constraint
*/
class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, VertexCam> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSBACamPrior() {
setMeasurement(g2o::SE3Quat());
information().setIdentity();
}
void setCameraInvLocalTransform(const g2o::SE3Quat & t)
{
_cameraInvLocalTransform = t;
}
// return the error estimate as a 3-vector
void computeError() {
const VertexCam* v = static_cast<const VertexCam*>(_vertices[0]);
g2o::SE3Quat estimate;
#ifdef RTABMAP_ORB_SLAM
estimate = v->estimate().inverse();
#else
estimate = v->estimate();
#endif
g2o::SE3Quat delta = _inverseMeasurement * estimate * _cameraInvLocalTransform;
_error[0]=delta.translation().x();
_error[1]=delta.translation().y();
_error[2]=delta.translation().z();
_error[3]=delta.rotation().x();
_error[4]=delta.rotation().y();
_error[5]=delta.rotation().z();
}
// jacobian
virtual void linearizeOplus() {
_jacobianOplusXi = Eigen::Matrix<double, 6, 6>::Identity();
}
virtual void setMeasurement(const g2o::SE3Quat& m){
_measurement = m;
_inverseMeasurement = m.inverse();
}
virtual bool setMeasurementData(const double* d) override {
Eigen::Map<const Eigen::Matrix<double, 7, 1, Eigen::ColMajor> > v(d);
// SE3Quat expects [x, y, z, qx, qy, qz, qw]
_measurement.fromVector(v);
_inverseMeasurement = _measurement.inverse();
return true;
}
virtual bool getMeasurementData(double* d) const override {
Eigen::Map<Eigen::Matrix<double, 7, 1, Eigen::ColMajor> > v(d);
// Returns [x, y, z, qx, qy, qz, qw]
v = _measurement.toVector();
return true;
}
virtual int measurementDimension() const {return 7;}
virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& /*from*/,
g2o::OptimizableGraph::Vertex* /*to*/) {
return 1.;
}
virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to) {
VertexCam *v = static_cast<VertexCam*>(_vertices[0]);
assert(v && "Vertex for the Prior edge is not set");
#ifdef RTABMAP_ORB_SLAM
g2o::SE3Quat newEstimate = _cameraInvLocalTransform * _inverseMeasurement;
#else
g2o::SE3Quat newEstimate = measurement()*_cameraInvLocalTransform.inverse();
#endif
if (_information.block<3,3>(0,0).array().abs().sum() == 0){ // do not set translation, as that part of the information is all zero
newEstimate.setTranslation(v->estimate().translation());
}
if (_information.block<3,3>(3,3).array().abs().sum() == 0){ // do not set rotation, as that part of the information is all zero
newEstimate.setRotation(v->estimate().rotation());
}
v->setEstimate(newEstimate);
}
virtual bool read(std::istream& is) override { return true; }
virtual bool write(std::ostream& os) const override { return true; }
protected:
g2o::SE3Quat _inverseMeasurement;
g2o::SE3Quat _cameraInvLocalTransform;
};
}
#endif
corelib/src/optimizer/g2o/edge_se3_expmap.h
+74
View File
@@ -0,0 +1,74 @@
#include "g2o/types/types_six_dof_expmap.h"
/**
* \brief 3D edge between two SBAcam
*/
class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeSE3Expmap(): BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
bool read(std::istream& is)
{
return false;
}
bool write(std::ostream& os) const
{
return false;
}
void computeError()
{
const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
_error[0]=delta.translation().x();
_error[1]=delta.translation().y();
_error[2]=delta.translation().z();
_error[3]=delta.rotation().x();
_error[4]=delta.rotation().y();
_error[5]=delta.rotation().z();
}
virtual void setMeasurement(const g2o::SE3Quat& meas){
_measurement=meas;
_inverseMeasurement=meas.inverse();
}
virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
if (from_.count(from) > 0)
to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
else
from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
}
virtual bool setMeasurementData(const double* d){
Eigen::Map<const g2o::Vector7d> v(d);
_measurement.fromVector(v);
_inverseMeasurement = _measurement.inverse();
return true;
}
virtual bool getMeasurementData(double* d) const{
Eigen::Map<g2o::Vector7d> v(d);
v = _measurement.toVector();
return true;
}
virtual int measurementDimension() const {return 7;}
virtual bool setMeasurementFromState() {
const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
_measurement = (v1->estimate().inverse()*v2->estimate());
_inverseMeasurement = _measurement.inverse();
return true;
}
protected:
g2o::SE3Quat _inverseMeasurement;
};
corelib/src/python/PyMatcher.cpp
+20 -4
View File
@@ -228,16 +228,32 @@ std::vector<cv::DMatch> PyMatcher::match(
int len2 = PyArray_SHAPE(np_ret)[1];
int type = PyArray_TYPE(np_ret);
UDEBUG("Matches array %dx%d (type=%d)", len1, len2, type);
UASSERT_MSG(type == NPY_LONG || type == NPY_INT, uFormat("Returned matches should type INT=5 or LONG=7, received type=%d", type).c_str());
if(type == NPY_LONG)
UASSERT_MSG(type == NPY_INT32 || type == NPY_UINT32 || type == NPY_INT64 || type == NPY_UINT64, uFormat("Returned matches should type INT32=%d UINT32=%d, INT64=%d or UINT64=%d, received type=%d", NPY_INT, NPY_UINT32, NPY_INT64, NPY_UINT64, type).c_str());
if(type == NPY_UINT64)
{
long* c_out = reinterpret_cast<long*>(PyArray_DATA(np_ret));
long long* c_out = reinterpret_cast<long long*>(PyArray_DATA(np_ret));
for (int i = 0; i < len1*len2; i+=2)
{
matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
}
}
else // INT
if(type == NPY_INT64)
{
unsigned long long* c_out = reinterpret_cast<unsigned long long*>(PyArray_DATA(np_ret));
for (int i = 0; i < len1*len2; i+=2)
{
matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
}
}
else if(type == NPY_UINT32)
{
unsigned int* c_out = reinterpret_cast<unsigned int*>(PyArray_DATA(np_ret));
for (int i = 0; i < len1*len2; i+=2)
{
matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
}
}
else // NPY_INT
{
int* c_out = reinterpret_cast<int*>(PyArray_DATA(np_ret));
for (int i = 0; i < len1*len2; i+=2)
corelib/src/python/PythonInterface.cpp
+7
View File
@@ -9,6 +9,7 @@
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UThread.h>
#include <pybind11/embed.h>
#include <filesystem>
namespace rtabmap {
@@ -16,6 +17,12 @@ PythonInterface::PythonInterface()
{
UINFO("Initialize python interpreter");
guard_ = new pybind11::scoped_interpreter();
// Tell Python to look in this directory for DLLs
std::string exe_dir = std::filesystem::current_path().string();
pybind11::module_ os = pybind11::module_::import("os");
os.attr("add_dll_directory")(exe_dir);
pybind11::module::import("threading");
release_ = new pybind11::gil_scoped_release();
}
corelib/src/python/rtabmap_superglue.py
+2 -1
View File
@@ -38,7 +38,6 @@ def init(descriptorDim, matchThreshold, iterations, cuda, model):
global superglue
superglue = SuperGlue(config.get('superglue', {})).eval().to(device)
def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo, imageWidth, imageHeight):
#print("SuperGlue python match()")
global device
@@ -77,6 +76,8 @@ def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo
matchesArray = np.stack((matchesFrom, matchesTo), axis=1);
# rtabmap expects format:
# matches: array Nx2 (type=9 or uint64)
return matchesArray
corelib/src/python/rtabmap_superpoint.py
+11 -2
View File
@@ -8,6 +8,7 @@
import random
import numpy as np
import torch
import os
#import sys
#import os
@@ -21,15 +22,19 @@ torch.set_grad_enabled(False)
device = 'cpu'
superpoint = []
script_dir = os.path.dirname(os.path.abspath(__file__))
def init(cuda):
#print("SuperPoint python init()")
global device
device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu'
weights_abs_path = os.path.join(script_dir, "superpoint_v1.pth")
# This class runs the SuperPoint network and processes its outputs.
global superpoint
superpoint = SuperPointFrontend(weights_path="superpoint_v1.pth",
superpoint = SuperPointFrontend(weights_path=weights_abs_path,
nms_dist=4,
conf_thresh=0.015,
nn_thresh=1,
@@ -47,10 +52,14 @@ def detect(imageBuffer):
# use copy to make sure memory is correctly re-ordered
pts = np.float32(np.transpose(pts)).copy()
desc = np.float32(np.transpose(desc)).copy()
# rtabmap expects format:
# pts: array Nx3 (type=11 or float)
# descriptors: array NxDIM 35x256 (type=11 or float)
return pts, desc
if __name__ == '__main__':
#test
init(True)
init(False)
detect(np.random.rand(640,480)*255)
corelib/src/python/rtabmap_trace_superpoint.py
+13
View File
@@ -0,0 +1,13 @@
import os
import sys
from pathlib import Path
import torch
import torchvision
from demo_superpoint import SuperPointNet
model = SuperPointNet()
model.load_state_dict(torch.load("superpoint_v1.pth"))
model.eval()
example = torch.rand(1, 1, 640, 480)
traced_script_module = torch.jit.trace(model, example, check_trace=False)
traced_script_module.save("superpoint_v1.pt")
corelib/src/util2d.cpp
+1
View File
@@ -2325,6 +2325,7 @@ std::vector<int> SSC(
UASSERT(keypoints.empty() || indx.empty() || keypoints.size() == indx.size());
bool useIndx = !indx.empty();
maxKeypoints = maxKeypoints - round(maxKeypoints * tolerance); // Just the make sure the solution will always be <= input maxKeypoints
// several temp expression variables to simplify solution equation
int exp1 = rows + cols + 2*maxKeypoints;
corelib/src/util3d_features.cpp
+3 -2
View File
@@ -213,6 +213,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
Transform & cameraTransform,
float ransacReprojThreshold,
float ransacConfidence,
int varianceMedianRatio,
const std::map<int, cv::Point3f> & refGuess3D,
double * varianceOut,
std::vector<int> * matchesOut)
@@ -345,7 +346,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> varianceMedianRatio];
float var = 2.1981 * median_error_sqr;
//UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);
@@ -369,7 +370,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> varianceMedianRatio];
variance = 2.1981 * median_error_sqr;
}
}