mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-01 17:10:26 +08:00
Tango #57: Global optimization / Post-processing on pause
This commit is contained in:
matlabbe
@@ -546,6 +546,31 @@ void DBDriver::getNodeData(
|
||||
}
|
||||
}
|
||||
|
||||
bool DBDriver::getCalibration(
|
||||
int signatureId,
|
||||
std::vector<CameraModel> & models,
|
||||
StereoCameraModel & stereoModel) const
|
||||
{
|
||||
bool found = false;
|
||||
// look in the trash
|
||||
_trashesMutex.lock();
|
||||
if(uContains(_trashSignatures, signatureId))
|
||||
{
|
||||
models = _trashSignatures.at(signatureId)->sensorData().cameraModels();
|
||||
stereoModel = _trashSignatures.at(signatureId)->sensorData().stereoCameraModel();
|
||||
found = true;
|
||||
}
|
||||
_trashesMutex.unlock();
|
||||
|
||||
if(!found)
|
||||
{
|
||||
_dbSafeAccessMutex.lock();
|
||||
found = this->getCalibrationQuery(signatureId, models, stereoModel);
|
||||
_dbSafeAccessMutex.unlock();
|
||||
}
|
||||
return found;
|
||||
}
|
||||
|
||||
bool DBDriver::getNodeInfo(
|
||||
int signatureId,
|
||||
Transform & pose,
|
||||
|
||||
@@ -1009,6 +1009,158 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
|
||||
}
|
||||
}
|
||||
|
||||
bool DBDriverSqlite3::getCalibrationQuery(
|
||||
int signatureId,
|
||||
std::vector<CameraModel> & models,
|
||||
StereoCameraModel & stereoModel) const
|
||||
{
|
||||
bool found = false;
|
||||
if(_ppDb && signatureId)
|
||||
{
|
||||
int rc = SQLITE_OK;
|
||||
sqlite3_stmt * ppStmt = 0;
|
||||
std::stringstream query;
|
||||
|
||||
if(uStrNumCmp(_version, "0.10.0") >= 0)
|
||||
{
|
||||
query << "SELECT calibration "
|
||||
<< "FROM Data "
|
||||
<< "WHERE id = " << signatureId
|
||||
<<";";
|
||||
}
|
||||
else if(uStrNumCmp(_version, "0.7.0") >= 0)
|
||||
{
|
||||
query << "SELECT local_transform, fx, fy, cx, cy "
|
||||
<< "FROM Depth "
|
||||
<< "WHERE id = " << signatureId
|
||||
<<";";
|
||||
}
|
||||
else
|
||||
{
|
||||
query << "SELECT local_transform, constant "
|
||||
<< "FROM Depth "
|
||||
<< "WHERE id = " << signatureId
|
||||
<<";";
|
||||
}
|
||||
|
||||
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
|
||||
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
|
||||
|
||||
const void * data = 0;
|
||||
int dataSize = 0;
|
||||
Transform localTransform;
|
||||
StereoCameraModel stereoModel;
|
||||
std::vector<CameraModel> models;
|
||||
|
||||
// Process the result if one
|
||||
rc = sqlite3_step(ppStmt);
|
||||
if(rc == SQLITE_ROW)
|
||||
{
|
||||
found = true;
|
||||
int index = 0;
|
||||
|
||||
// calibration
|
||||
if(uStrNumCmp(_version, "0.10.0") >= 0)
|
||||
{
|
||||
data = sqlite3_column_blob(ppStmt, index);
|
||||
dataSize = sqlite3_column_bytes(ppStmt, index++);
|
||||
// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
|
||||
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
|
||||
if(dataSize > 0 && data)
|
||||
{
|
||||
float * dataFloat = (float*)data;
|
||||
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
|
||||
{
|
||||
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
|
||||
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
|
||||
int max = cameraCount*(4+localTransform.size());
|
||||
for(int i=0; i<max; i+=4+localTransform.size())
|
||||
{
|
||||
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
|
||||
models.push_back(CameraModel(
|
||||
(double)dataFloat[i],
|
||||
(double)dataFloat[i+1],
|
||||
(double)dataFloat[i+2],
|
||||
(double)dataFloat[i+3],
|
||||
localTransform));
|
||||
}
|
||||
}
|
||||
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
|
||||
{
|
||||
UDEBUG("Loading calibration of a stereo camera");
|
||||
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
|
||||
stereoModel = StereoCameraModel(
|
||||
dataFloat[0], // fx
|
||||
dataFloat[1], // fy
|
||||
dataFloat[2], // cx
|
||||
dataFloat[3], // cy
|
||||
dataFloat[4], // baseline
|
||||
localTransform);
|
||||
}
|
||||
else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
|
||||
{
|
||||
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
|
||||
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
|
||||
int max = cameraCount*(6+localTransform.size());
|
||||
for(int i=0; i<max; i+=6+localTransform.size())
|
||||
{
|
||||
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
|
||||
models.push_back(CameraModel(
|
||||
(double)dataFloat[i],
|
||||
(double)dataFloat[i+1],
|
||||
(double)dataFloat[i+2],
|
||||
(double)dataFloat[i+3],
|
||||
localTransform));
|
||||
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
|
||||
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
|
||||
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
|
||||
localTransform.prettyPrint().c_str());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else if(uStrNumCmp(_version, "0.7.0") >= 0)
|
||||
{
|
||||
double fx = sqlite3_column_double(ppStmt, index++);
|
||||
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
|
||||
double cx = sqlite3_column_double(ppStmt, index++);
|
||||
double cy = sqlite3_column_double(ppStmt, index++);
|
||||
if(fyOrBaseline < 1.0)
|
||||
{
|
||||
//it is a baseline
|
||||
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
|
||||
}
|
||||
else
|
||||
{
|
||||
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float depthConstant = sqlite3_column_double(ppStmt, index++);
|
||||
float fx = 1.0f/depthConstant;
|
||||
float fy = 1.0f/depthConstant;
|
||||
float cx = 0.0f;
|
||||
float cy = 0.0f;
|
||||
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
|
||||
}
|
||||
|
||||
rc = sqlite3_step(ppStmt); // next result...
|
||||
}
|
||||
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
|
||||
|
||||
// Finalize (delete) the statement
|
||||
rc = sqlite3_finalize(ppStmt);
|
||||
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
|
||||
}
|
||||
return found;
|
||||
}
|
||||
|
||||
bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
|
||||
Transform & pose,
|
||||
int & mapId,
|
||||
|
||||
@@ -83,6 +83,7 @@ private:
|
||||
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
|
||||
|
||||
virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
|
||||
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
|
||||
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
|
||||
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
|
||||
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
|
||||
|
||||
@@ -2066,113 +2066,7 @@ Transform Memory::computeTransform(
|
||||
|
||||
if(fromS && toS)
|
||||
{
|
||||
// make sure we have all data needed
|
||||
// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
|
||||
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS->sensorData().imageCompressed().empty()) ||
|
||||
(_registrationPipeline->isScanRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().laserScanCompressed().empty()) ||
|
||||
(_registrationPipeline->isUserDataRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().userDataCompressed().empty()))
|
||||
{
|
||||
getNodeData(fromS->id());
|
||||
}
|
||||
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS->sensorData().imageCompressed().empty()) ||
|
||||
(_registrationPipeline->isScanRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().laserScanCompressed().empty()) ||
|
||||
(_registrationPipeline->isUserDataRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().userDataCompressed().empty()))
|
||||
{
|
||||
getNodeData(toS->id());
|
||||
}
|
||||
// uncompress only what we need
|
||||
cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
|
||||
fromS->sensorData().uncompressData(
|
||||
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
|
||||
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
|
||||
_registrationPipeline->isScanRequired()?&laserBuf:0,
|
||||
_registrationPipeline->isUserDataRequired()?&userBuf:0);
|
||||
toS->sensorData().uncompressData(
|
||||
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
|
||||
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
|
||||
_registrationPipeline->isScanRequired()?&laserBuf:0,
|
||||
_registrationPipeline->isUserDataRequired()?&userBuf:0);
|
||||
|
||||
|
||||
// compute transform fromId -> toId
|
||||
std::vector<int> inliersV;
|
||||
if(_reextractLoopClosureFeatures || (fromS->getWords().size() && toS->getWords().size()))
|
||||
{
|
||||
Signature tmpFrom = *fromS;
|
||||
Signature tmpTo = *toS;
|
||||
|
||||
// make a guess fast with known correspondences (if there are)
|
||||
RegistrationVis regVis(parameters_);
|
||||
if(tmpFrom.getWords().size() &&
|
||||
tmpTo.getWords().size() &&
|
||||
tmpFrom.getWords3().size() &&
|
||||
tmpTo.getWords3().size())
|
||||
{
|
||||
UDEBUG("");
|
||||
// Remove descriptors, this will avoid recomputation of the correspondences in regVis
|
||||
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
|
||||
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
|
||||
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
|
||||
// set back descriptors
|
||||
tmpFrom.setWordsDescriptors(fromS->getWordsDescriptors());
|
||||
tmpTo.setWordsDescriptors(toS->getWordsDescriptors());
|
||||
}
|
||||
|
||||
if(_reextractLoopClosureFeatures)
|
||||
{
|
||||
UDEBUG("");
|
||||
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
|
||||
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
|
||||
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
|
||||
tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
|
||||
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
|
||||
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
|
||||
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
|
||||
tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
|
||||
}
|
||||
|
||||
if(guess.isNull())
|
||||
{
|
||||
if(!_registrationPipeline->isImageRequired())
|
||||
{
|
||||
UDEBUG("");
|
||||
// no visual in the pipeline, make visual registration for guess
|
||||
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
|
||||
}
|
||||
else
|
||||
{
|
||||
UDEBUG("");
|
||||
guess.setIdentity();
|
||||
}
|
||||
}
|
||||
|
||||
if(!guess.isNull())
|
||||
{
|
||||
UDEBUG("");
|
||||
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
|
||||
|
||||
if(!transform.isNull())
|
||||
{
|
||||
UDEBUG("");
|
||||
// verify if it is a 180 degree transform, well verify > 90
|
||||
float x,y,z, roll,pitch,yaw;
|
||||
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
|
||||
if(fabs(roll) > CV_PI/2 ||
|
||||
fabs(pitch) > CV_PI/2 ||
|
||||
fabs(yaw) > CV_PI/2)
|
||||
{
|
||||
transform.setNull();
|
||||
std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
|
||||
roll, pitch, yaw);
|
||||
UINFO(msg.c_str());
|
||||
if(info)
|
||||
{
|
||||
info->rejectedMsg = msg;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return computeTransform(*fromS, *toS, guess, info);
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -2186,6 +2080,125 @@ Transform Memory::computeTransform(
|
||||
return transform;
|
||||
}
|
||||
|
||||
// compute transform fromId -> toId
|
||||
Transform Memory::computeTransform(
|
||||
Signature & fromS,
|
||||
Signature & toS,
|
||||
Transform guess,
|
||||
RegistrationInfo * info) const
|
||||
{
|
||||
Transform transform;
|
||||
|
||||
// make sure we have all data needed
|
||||
// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
|
||||
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS.sensorData().imageCompressed().empty()) ||
|
||||
(_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) ||
|
||||
(_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
|
||||
{
|
||||
fromS.sensorData() = getNodeData(fromS.id());
|
||||
}
|
||||
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS.sensorData().imageCompressed().empty()) ||
|
||||
(_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) ||
|
||||
(_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
|
||||
{
|
||||
toS.sensorData() = getNodeData(toS.id());
|
||||
}
|
||||
// uncompress only what we need
|
||||
cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
|
||||
fromS.sensorData().uncompressData(
|
||||
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
|
||||
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
|
||||
_registrationPipeline->isScanRequired()?&laserBuf:0,
|
||||
_registrationPipeline->isUserDataRequired()?&userBuf:0);
|
||||
toS.sensorData().uncompressData(
|
||||
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
|
||||
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
|
||||
_registrationPipeline->isScanRequired()?&laserBuf:0,
|
||||
_registrationPipeline->isUserDataRequired()?&userBuf:0);
|
||||
|
||||
|
||||
// compute transform fromId -> toId
|
||||
std::vector<int> inliersV;
|
||||
if(_reextractLoopClosureFeatures || (fromS.getWords().size() && toS.getWords().size()))
|
||||
{
|
||||
Signature tmpFrom = fromS;
|
||||
Signature tmpTo = toS;
|
||||
|
||||
// make a guess fast with known correspondences (if there are)
|
||||
RegistrationVis regVis(parameters_);
|
||||
if(tmpFrom.getWords().size() &&
|
||||
tmpTo.getWords().size() &&
|
||||
tmpFrom.getWords3().size() &&
|
||||
tmpTo.getWords3().size())
|
||||
{
|
||||
UDEBUG("");
|
||||
// Remove descriptors, this will avoid recomputation of the correspondences in regVis
|
||||
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
|
||||
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
|
||||
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
|
||||
// set back descriptors
|
||||
tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
|
||||
tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
|
||||
}
|
||||
|
||||
if(_reextractLoopClosureFeatures)
|
||||
{
|
||||
UDEBUG("");
|
||||
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
|
||||
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
|
||||
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
|
||||
tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
|
||||
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
|
||||
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
|
||||
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
|
||||
tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
|
||||
}
|
||||
|
||||
if(guess.isNull())
|
||||
{
|
||||
if(!_registrationPipeline->isImageRequired())
|
||||
{
|
||||
UDEBUG("");
|
||||
// no visual in the pipeline, make visual registration for guess
|
||||
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
|
||||
}
|
||||
else
|
||||
{
|
||||
UDEBUG("");
|
||||
guess.setIdentity();
|
||||
}
|
||||
}
|
||||
|
||||
if(!guess.isNull())
|
||||
{
|
||||
UDEBUG("");
|
||||
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
|
||||
|
||||
if(!transform.isNull())
|
||||
{
|
||||
UDEBUG("");
|
||||
// verify if it is a 180 degree transform, well verify > 90
|
||||
float x,y,z, roll,pitch,yaw;
|
||||
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
|
||||
if(fabs(roll) > CV_PI/2 ||
|
||||
fabs(pitch) > CV_PI/2 ||
|
||||
fabs(yaw) > CV_PI/2)
|
||||
{
|
||||
transform.setNull();
|
||||
std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
|
||||
roll, pitch, yaw);
|
||||
UINFO(msg.c_str());
|
||||
if(info)
|
||||
{
|
||||
info->rejectedMsg = msg;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return transform;
|
||||
}
|
||||
|
||||
// compute transform fromId -> toId
|
||||
Transform Memory::computeIcpTransform(
|
||||
int fromId,
|
||||
@@ -2868,45 +2881,24 @@ cv::Mat Memory::getImageCompressed(int signatureId) const
|
||||
return image;
|
||||
}
|
||||
|
||||
SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoadedDataInMemory)
|
||||
SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
|
||||
{
|
||||
UDEBUG("nodeId=%d", nodeId);
|
||||
SensorData r;
|
||||
Signature * s = this->_getSignature(nodeId);
|
||||
if(s && !s->sensorData().imageCompressed().empty())
|
||||
{
|
||||
if(keepLoadedDataInMemory && uncompressedData)
|
||||
{
|
||||
s->sensorData().uncompressData();
|
||||
}
|
||||
r = s->sensorData();
|
||||
if(!keepLoadedDataInMemory && uncompressedData)
|
||||
{
|
||||
r.uncompressData();
|
||||
}
|
||||
}
|
||||
else if(_dbDriver)
|
||||
{
|
||||
// load from database
|
||||
if(s && keepLoadedDataInMemory)
|
||||
{
|
||||
std::list<Signature*> signatures;
|
||||
signatures.push_back(s);
|
||||
_dbDriver->loadNodeData(signatures);
|
||||
if(uncompressedData)
|
||||
{
|
||||
s->sensorData().uncompressData();
|
||||
}
|
||||
r = s->sensorData();
|
||||
}
|
||||
else
|
||||
{
|
||||
_dbDriver->getNodeData(nodeId, r);
|
||||
if(uncompressedData)
|
||||
{
|
||||
r.uncompressData();
|
||||
}
|
||||
}
|
||||
_dbDriver->getNodeData(nodeId, r);
|
||||
}
|
||||
|
||||
if(uncompressedData)
|
||||
{
|
||||
r.uncompressData();
|
||||
}
|
||||
|
||||
return r;
|
||||
@@ -2951,6 +2943,24 @@ void Memory::getNodeWords(int nodeId,
|
||||
}
|
||||
}
|
||||
|
||||
void Memory::getNodeCalibration(int nodeId,
|
||||
std::vector<CameraModel> & models,
|
||||
StereoCameraModel & stereoModel)
|
||||
{
|
||||
UDEBUG("nodeId=%d", nodeId);
|
||||
Signature * s = this->_getSignature(nodeId);
|
||||
if(s)
|
||||
{
|
||||
models = s->sensorData().cameraModels();
|
||||
stereoModel = s->sensorData().stereoCameraModel();
|
||||
}
|
||||
else if(_dbDriver)
|
||||
{
|
||||
// load from database
|
||||
_dbDriver->getCalibration(nodeId, models, stereoModel);
|
||||
}
|
||||
}
|
||||
|
||||
SensorData Memory::getSignatureDataConst(int locationId) const
|
||||
{
|
||||
UDEBUG("");
|
||||
|
||||
@@ -752,7 +752,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
|
||||
UASSERT(optimizer.verifyInformationMatrices());
|
||||
|
||||
UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%d)", iterations(), robustKernel?1:0);
|
||||
UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%d)", iterations(), this->epsilon(), robustKernel?1:0);
|
||||
|
||||
int it = 0;
|
||||
UTimer timer;
|
||||
|
||||
@@ -1639,27 +1639,29 @@ bool Rtabmap::process(
|
||||
++iter)
|
||||
{
|
||||
const Signature * s = _memory->getSignature(iter->second);
|
||||
UASSERT(s!=0);
|
||||
// If there is a change of direction, better to be retrieving
|
||||
// ALL nearest signatures than only newest neighbors
|
||||
const std::map<int, Link> & links = s->getLinks();
|
||||
for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
|
||||
jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
|
||||
++jter)
|
||||
if(s!=0)
|
||||
{
|
||||
if(_memory->getSignature(jter->first) == 0)
|
||||
// If there is a change of direction, better to be retrieving
|
||||
// ALL nearest signatures than only newest neighbors
|
||||
const std::map<int, Link> & links = s->getLinks();
|
||||
for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
|
||||
jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
|
||||
++jter)
|
||||
{
|
||||
UINFO("retrieval of node %d on local map", jter->first);
|
||||
retrievalLocalIds.push_back(jter->first);
|
||||
if(_memory->getSignature(jter->first) == 0)
|
||||
{
|
||||
UINFO("retrieval of node %d on local map", jter->first);
|
||||
retrievalLocalIds.push_back(jter->first);
|
||||
}
|
||||
}
|
||||
}
|
||||
if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
|
||||
{
|
||||
if(immunizedLocations.insert(s->id()).second)
|
||||
if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
|
||||
{
|
||||
++immunizedLocally;
|
||||
if(immunizedLocations.insert(s->id()).second)
|
||||
{
|
||||
++immunizedLocally;
|
||||
}
|
||||
UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
|
||||
}
|
||||
UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
|
||||
}
|
||||
}
|
||||
// well, if the maximum retrieved is not reached, look for neighbors in database
|
||||
@@ -2685,6 +2687,11 @@ void Rtabmap::rejectLoopClosure(int oldId, int newId)
|
||||
}
|
||||
}
|
||||
|
||||
void Rtabmap::setOptimizedPoses(const std::map<int, Transform> & poses)
|
||||
{
|
||||
_optimizedPoses = poses;
|
||||
}
|
||||
|
||||
void Rtabmap::dumpData() const
|
||||
{
|
||||
UDEBUG("");
|
||||
@@ -3236,6 +3243,20 @@ void Rtabmap::getGraph(
|
||||
label,
|
||||
odomPose,
|
||||
groundTruth)));
|
||||
|
||||
std::multimap<int, cv::KeyPoint> words;
|
||||
std::multimap<int, cv::Point3f> words3;
|
||||
std::multimap<int, cv::Mat> wordsDescriptors;
|
||||
_memory->getNodeWords(iter->first, words, words3, wordsDescriptors);
|
||||
signatures->at(iter->first).setWords(words);
|
||||
signatures->at(iter->first).setWords3(words3);
|
||||
signatures->at(iter->first).setWordsDescriptors(wordsDescriptors);
|
||||
|
||||
std::vector<CameraModel> models;
|
||||
StereoCameraModel stereoModel;
|
||||
_memory->getNodeCalibration(iter->first, models, stereoModel);
|
||||
signatures->at(iter->first).sensorData().setCameraModels(models);
|
||||
signatures->at(iter->first).sensorData().setStereoCameraModel(stereoModel);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3249,6 +3270,119 @@ void Rtabmap::getGraph(
|
||||
}
|
||||
}
|
||||
|
||||
int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int iterations)
|
||||
{
|
||||
UASSERT(iterations>0);
|
||||
|
||||
if(_graphOptimizer->iterations() <= 0)
|
||||
{
|
||||
UERROR("Cannot detect more loop closures if graph optimization iterations = 0");
|
||||
return 0;
|
||||
}
|
||||
if(!_rgbdSlamMode)
|
||||
{
|
||||
UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::list<Link> loopClosuresAdded;
|
||||
std::multimap<int, int> checkedLoopClosures;
|
||||
|
||||
std::map<int, Transform> poses;
|
||||
std::multimap<int, Link> links;
|
||||
this->getGraph(poses, links, true, true);
|
||||
|
||||
for(int n=0; n<iterations; ++n)
|
||||
{
|
||||
UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
|
||||
n+1, iterations, clusterRadius, clusterAngle);
|
||||
|
||||
std::multimap<int, int> clusters = graph::radiusPosesClustering(
|
||||
poses,
|
||||
clusterRadius,
|
||||
clusterAngle);
|
||||
|
||||
UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
|
||||
|
||||
int i=0;
|
||||
std::set<int> addedLinks;
|
||||
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
|
||||
{
|
||||
int from = iter->first;
|
||||
int to = iter->second;
|
||||
if(iter->first < iter->second)
|
||||
{
|
||||
from = iter->second;
|
||||
to = iter->first;
|
||||
}
|
||||
|
||||
bool alreadyChecked = false;
|
||||
for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
|
||||
!alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
|
||||
++jter)
|
||||
{
|
||||
if(to == jter->second)
|
||||
{
|
||||
alreadyChecked = true;
|
||||
}
|
||||
}
|
||||
|
||||
if(!alreadyChecked)
|
||||
{
|
||||
// only add new links and one per cluster per iteration
|
||||
if(addedLinks.find(from) == addedLinks.end() &&
|
||||
rtabmap::graph::findLink(links, from, to) == links.end())
|
||||
{
|
||||
checkedLoopClosures.insert(std::make_pair(from, to));
|
||||
|
||||
RegistrationInfo info;
|
||||
Transform t = _memory->computeTransform(from, to, Transform(), &info);
|
||||
|
||||
if(!t.isNull())
|
||||
{
|
||||
UINFO("Added new loop closure between %d and %d.", from, to);
|
||||
addedLinks.insert(from);
|
||||
addedLinks.insert(to);
|
||||
links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
|
||||
loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.variance, info.variance));
|
||||
UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
UINFO("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2);
|
||||
if(addedLinks.size() == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if(n+1 < iterations)
|
||||
{
|
||||
UINFO("Optimizing graph with new links (%d nodes, %d constraints)...",
|
||||
(int)poses.size(), (int)links.size());
|
||||
int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
|
||||
poses = _graphOptimizer->optimize(fromId, poses, links, 0);
|
||||
if(poses.size() == 0)
|
||||
{
|
||||
UERROR("Optimization failed! Rejecting all loop closures...");
|
||||
loopClosuresAdded.clear();
|
||||
break;
|
||||
}
|
||||
UINFO("Optimizing graph with new links... done!");
|
||||
}
|
||||
}
|
||||
UINFO("Total added %d loop closures.", (int)loopClosuresAdded.size());
|
||||
|
||||
if(loopClosuresAdded.size())
|
||||
{
|
||||
for(std::list<Link>::iterator iter=loopClosuresAdded.begin(); iter!=loopClosuresAdded.end(); ++iter)
|
||||
{
|
||||
_memory->addLink(*iter);
|
||||
}
|
||||
}
|
||||
return (int)loopClosuresAdded.size();
|
||||
}
|
||||
|
||||
void Rtabmap::clearPath(int status)
|
||||
{
|
||||
UINFO("status=%d", status);
|
||||
|
||||
Reference in New Issue
Block a user