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

rtabmap: refining neighbor links uses computeIcpTransform() instead of computeTransform() (to ignore Visual registration, which adds more errors most of the time). When database is in RAM, we can close under a different name than the one used when the database was loaded.

Browse Source 
Tango: Added Append Mode option (only start a new map after being localized, default true) and the old map is still shown for convenience until localization. Increased minInliers to 25. Increased ICP correspondenceRatio to 0.5. Decreased time to open a database. Saving doesn't close the database anymore and overwrite only if we save with the same name. Removed ICP refining from standard optimization.
This commit is contained in:
matlabbe
2016-11-21 17:09:04 -05:00
parent 4c559a3191
commit 14908e3a11
19 changed files with 380 additions and 296 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/DBDriver.h
View File

@@ -102,7 +102,7 @@ public:
// Mutex-protected methods of abstract versions below
bool openConnection(const std::string & url, bool overwritten = false);
void closeConnection(bool save = true);
void closeConnection(bool save = true, const std::string & outputUrl = "");
bool isConnected() const;
long getMemoryUsed() const; // In bytes
std::string getDatabaseVersion() const;
@@ -146,7 +146,7 @@ protected:
private:
virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
virtual void disconnectDatabaseQuery(bool save = true) = 0;
virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
virtual bool isConnectedQuery() const = 0;
virtual long getMemoryUsedQuery() const = 0; // In bytes
virtual bool getDatabaseVersionQuery(std::string & version) const = 0;

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -80,7 +80,7 @@ public:
bool dbOverwritten = false,
const ParametersMap & parameters = ParametersMap(),
bool postInitClosingEvents = false);
void close(bool databaseSaved = true, bool postInitClosingEvents = false);
void close(bool databaseSaved = true, bool postInitClosingEvents = false, const std::string & ouputDatabasePath = "");
std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids);
int incrementMapId(std::map<int, int> * reducedIds = 0);

9
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -67,7 +67,14 @@ public:
void init(const ParametersMap & parameters, const std::string & databasePath = "");
void init(const std::string & configFile = "", const std::string & databasePath = "");
void close(bool databaseSaved = true);
/**
* Close rtabmap. This will delete rtabmap object if set.
* @param databaseSaved true=database saved, false=database discarded.
* @param databasePath output database file name, ignored if
* Db/Sqlite3InMemory=false (opened database is
* then overwritten).
*/
void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
const std::string & getWorkingDir() const {return _wDir;}
bool isRGBDMode() const { return _rgbdSlamMode; }

2
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -61,7 +61,7 @@ public:
enum Cmd {
kCmdInit, // params: [string] database path + ParametersMap
kCmdResetMemory,
kCmdClose, // params: [bool] database saved (default true)
kCmdClose, // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin

10
corelib/include/rtabmap/core/RtabmapThread.h
View File

@@ -82,8 +82,14 @@ public:
void setDataBufferSize(unsigned int bufferSize);
void createIntermediateNodes(bool enabled);
// this will delete rtabmap object if set
void close(bool databaseSaved);
/**
* Close rtabmap. This will delete rtabmap object if set.
* @param databaseSaved true=database saved, false=database discarded.
* @param databasePath output database file name, ignored if
* Db/Sqlite3InMemory=false (opened database is
* then overwritten).
*/
void close(bool databaseSaved, const std::string & databasePath = "");
protected:
virtual void handleEvent(UEvent * anEvent);

4
corelib/src/DBDriver.cpp
View File

@@ -61,7 +61,7 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
{
}
void DBDriver::closeConnection(bool save)
void DBDriver::closeConnection(bool save, const std::string & outputUrl)
{
UDEBUG("isRunning=%d", this->isRunning());
this->join(true);
@@ -78,7 +78,7 @@ void DBDriver::closeConnection(bool save)
_trashesMutex.unlock();
}
_dbSafeAccessMutex.lock();
this->disconnectDatabaseQuery(save);
this->disconnectDatabaseQuery(save, outputUrl);
_dbSafeAccessMutex.unlock();
UDEBUG("");
}

18
corelib/src/DBDriverSqlite3.cpp
View File

@@ -390,7 +390,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
return true;
}
void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & outputUrl)
{
UDEBUG("");
if(_ppDb)
@@ -411,11 +411,23 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
{
UTimer timer;
timer.start();
UINFO("Saving database to %s ...", this->getUrl().c_str());
rc = loadOrSaveDb(_ppDb, this->getUrl(), 1); // Save memory to file
std::string outputFile = this->getUrl();
if(!outputUrl.empty())
{
outputFile = outputUrl;
}
UINFO("Saving database to %s ...", outputFile.c_str());
rc = loadOrSaveDb(_ppDb, outputFile, 1); // Save memory to file
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Saving DB time = %fs", timer.ticks());
}
else if(save && !outputUrl.empty() && outputUrl.compare(this->getUrl()) != 0)
{
UWARN("Output database path (%s) is different than the opened database "
"path (%s). Exporting to a different path is only available "
"when database is in memory (%s=true). Opened database path is overwritten.",
outputUrl.c_str(), this->getUrl().c_str(), Parameters::kDbSqlite3InMemory().c_str());
}
// Then close (delete) the database connection
UINFO("Disconnecting database %s...", this->getUrl().c_str());

2
corelib/src/DBDriverSqlite3.h
View File

@@ -49,7 +49,7 @@ public:
private:
virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
virtual void disconnectDatabaseQuery(bool save = true);
virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
virtual bool isConnectedQuery() const;
virtual long getMemoryUsedQuery() const; // In bytes
virtual bool getDatabaseVersionQuery(std::string & version) const;

4
corelib/src/Memory.cpp
View File

@@ -304,7 +304,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
return success;
}
void Memory::close(bool databaseSaved, bool postInitClosingEvents)
void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath)
{
UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
@@ -342,7 +342,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents)
_dbDriver->emptyTrashes();
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
_dbDriver->closeConnection();
_dbDriver->closeConnection(true, ouputDatabasePath);
delete _dbDriver;
_dbDriver = 0;
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));

6
corelib/src/Rtabmap.cpp
View File

@@ -320,7 +320,7 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
this->init(param, databasePath);
}
void Rtabmap::close(bool databaseSaved)
void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
{
UINFO("databaseSaved=%d", databaseSaved?1:0);
_highestHypothesis = std::make_pair(0,0.0f);
@@ -354,7 +354,7 @@ void Rtabmap::close(bool databaseSaved)
}
if(_memory)
{
_memory->close(databaseSaved, true);
_memory->close(databaseSaved, true, ouputDatabasePath);
delete _memory;
_memory = 0;
}
@@ -1024,7 +1024,7 @@ bool Rtabmap::process(
{
UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
RegistrationInfo info;
Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
if(!t.isNull())
{
UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",

7
corelib/src/RtabmapThread.cpp
View File

@@ -117,12 +117,12 @@ void RtabmapThread::createIntermediateNodes(bool enabled)
_createIntermediateNodes = enabled;
}
void RtabmapThread::close(bool databaseSaved)
void RtabmapThread::close(bool databaseSaved, const std::string & ouputDatabasePath)
{
this->join(true);
if(_rtabmap)
{
_rtabmap->close(databaseSaved);
_rtabmap->close(databaseSaved, ouputDatabasePath);
delete _rtabmap;
_rtabmap = 0;
}
@@ -241,7 +241,7 @@ void RtabmapThread::mainLoop()
UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
this->clearBufferedData();
}
_rtabmap->close(uStr2Bool(parameters.at("saved")));
_rtabmap->close(uStr2Bool(parameters.at("saved")), parameters.at("outputPath"));
break;
case kStateDumpingMemory:
_rtabmap->dumpData();
@@ -409,6 +409,7 @@ void RtabmapThread::handleEvent(UEvent* event)
UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
ParametersMap param;
param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
param.insert(ParametersPair("outputPath", rtabmapEvent->value2().toStr()));
pushNewState(kStateClose, param);
}
else if(cmd == RtabmapEventCmd::kCmdResetMemory)

26
corelib/src/util2d.cpp
View File

@@ -1742,15 +1742,15 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
UDEBUG("Begin: depth float=%d %dx%d sigmaS=%f sigmaR=%f earlDivision=%d",
depth.type()==CV_32FC1?1:0, depth.cols, depth.rows, sigmaS, sigmaR, earlyDivision?1:0);
cv::Mat output = depth.clone();
cv::Mat output = cv::Mat::zeros(depth.size(), CV_32FC1);
float base_max = -std::numeric_limits<float>::max ();
float base_min = std::numeric_limits<float>::max ();
bool found_finite = false;
for (int x = 0; x < output.cols; ++x)
for (int y = 0; y < output.rows; ++y)
for (int x = 0; x < depth.cols; ++x)
for (int y = 0; y < depth.rows; ++y)
{
float z = depth.type()==CV_32FC1?output.at<float>(y, x):float(output.at<unsigned short>(y, x))/1000.0f;
float z = depth.type()==CV_32FC1?depth.at<float>(y, x):float(depth.at<unsigned short>(y, x))/1000.0f;
if (z > 0.0f && uIsFinite(z))
{
if (base_max < z)
@@ -1783,7 +1783,7 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigmaS + 0.5f) + padding_xy;
for (int y = 0; y < depth.rows; ++y)
{
float v = depth.type()==CV_32FC1?output.at<float>(y,x):float(output.at<unsigned short>(y,x))/1000.0f;
float v = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
if((v > 0 && uIsFinite(v)))
{
float z = v - base_min;
@@ -1832,7 +1832,7 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
for (int x = 0; x < depth.cols; ++x)
for (int y = 0; y < depth.rows; ++y)
{
float z = depth.type()==CV_32FC1?output.at<float>(y,x):float(output.at<unsigned short>(y,x))/1000.0f;
float z = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
if(z > 0 && uIsFinite(z))
{
z -= base_min;
@@ -1844,19 +1844,11 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
{
v = 0.0f;
}
if(depth.type()==CV_32FC1)
if(depth.type()==CV_16UC1 && v>65.5350f)
{
output.at<float>(y,x) = v;
}
else
{
v*=1000.0f;
if(v>65535.0f)
{
v = 65535.0f;
}
output.at<unsigned short>(y,x) = v;
v = 65.5350f;
}
output.at<float>(y,x) = v;
}
}