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

rtabmap: parameters passed by arguments override those in the database loaded from command line. Dictionary: set words saved to avoid saving them in database (causing db error when remapping from an old db created by fixed dictionary). G2O: ba supporting words with negative ids.

Browse Source
This commit is contained in:
matlabbe
2018-07-27 16:01:17 -04:00
parent b63590bf1d
commit 4d5b42ab79
7 changed files with 80 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/DBDriverSqlite3.cpp
View File

@@ -3957,7 +3957,7 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
//execute query
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 (word=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), w->id()).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

40
corelib/src/Memory.cpp
View File

@@ -2398,6 +2398,7 @@ Transform Memory::computeTransform(
RegistrationInfo * info,
bool useKnownCorrespondencesIfPossible) const
{
UDEBUG("");
Transform transform;
// make sure we have all data needed
@@ -2489,6 +2490,7 @@ Transform Memory::computeTransform(
const std::map<int, Link> & links = fromS.getLinks();
{
const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
UDEBUG("fromS.getWords3()=%d uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
{
if(util3d::isFinite(jter->second))
@@ -2499,6 +2501,7 @@ Transform Memory::computeTransform(
}
}
}
UDEBUG("words3DMap=%d", (int)words3DMap.size());
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
@@ -2507,7 +2510,9 @@ Transform Memory::computeTransform(
const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
{
if(util3d::isFinite(jter->second) && words3DMap.find(jter->first) == words3DMap.end())
if( jter->first > 0 &&
util3d::isFinite(jter->second) &&
words3DMap.find(jter->first) == words3DMap.end())
{
words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
wordsMap.insert(*s->getWords().find(jter->first));
@@ -2515,6 +2520,7 @@ Transform Memory::computeTransform(
}
}
}
UDEBUG("words3DMap=%d", (int)words3DMap.size());
Signature tmpFrom2(fromS.id());
tmpFrom2.setWords3(words3DMap);
tmpFrom2.setWords(wordsMap);
@@ -2537,7 +2543,15 @@ Transform Memory::computeTransform(
for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
int id = iter->first;
const Signature * s = this->getSignature(id);
const Signature * s;
if(id == tmpTo.id())
{
s = &tmpTo; // reuse matched words
}
else
{
s = this->getSignature(id);
}
CameraModel model;
if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
{
@@ -2572,7 +2586,8 @@ Transform Memory::computeTransform(
const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
{
if(points3DMap.find(jter->first)!=points3DMap.end())
if(points3DMap.find(jter->first)!=points3DMap.end() &&
(id == tmpTo.id() || jter->first > 0))
{
std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
@@ -3990,11 +4005,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
allWordIds[quantizedToRawIndices[i]] = *iter;
++i;
}
int negIndex = -1;
for(i=0; i<(int)allWordIds.size(); ++i)
{
if(allWordIds[i] < 0)
{
allWordIds[i] = -1*(i+1);
allWordIds[i] = negIndex--;
}
}
wordIds = uVectorToList(allWordIds);
@@ -4087,14 +4103,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UDEBUG("inliers=%d", (int)inliers.size());
// words3D should have the same size than words
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(words.size() == words3D.size());
int added3DPointsWithoutDepth = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
UASSERT(iter3D!=words3D.end());
if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
if(iter3D == words3D.end())
{
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
++added3DPointsWithoutDepth;
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
{
iter3D->second = jter->second;
++added3DPointsWithoutDepth;

43
corelib/src/OptimizerG2O.cpp
View File

@@ -1051,16 +1051,31 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("fill 3D points to g2o...");
const int stepVertexId = poses.rbegin()->first+1;
int negVertexOffset = stepVertexId;
if(wordReferences.size() && wordReferences.rbegin()->first>0)
{
negVertexOffset += wordReferences.rbegin()->first;
}
UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
std::list<g2o::OptimizableGraph::Edge*> edges;
for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
{
if(points3DMap.find(iter->first) != points3DMap.end())
int id = iter->first;
if(points3DMap.find(id) != points3DMap.end())
{
cv::Point3f pt3d = points3DMap.at(iter->first);
cv::Point3f pt3d = points3DMap.at(id);
g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
vpt3d->setId(stepVertexId + iter->first);
if(id<0)
{
vpt3d->setId(negVertexOffset + id*-1);
}
else
{
vpt3d->setId(stepVertexId + id);
}
UASSERT(vpt3d->id() > 0);
vpt3d->setMarginalized(true);
optimizer.addVertex(vpt3d);
@@ -1218,7 +1233,15 @@ 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 = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
cv::Point3f pt3d;
if((*iter)->vertex(0)->id() > negVertexOffset)
{
pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
}
else
{
pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
}
((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
if(outliers)
@@ -1294,7 +1317,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
const g2o::VertexSBAPointXYZ* v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + iter->first);
const g2o::VertexSBAPointXYZ* v;
int id = iter->first;
if(id<0)
{
v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(negVertexOffset + id*-1);
}
else
{
v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + id);
}
if(v)
{
cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);

3
corelib/src/VWDictionary.cpp
View File

@@ -154,7 +154,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
driver->load(this, false);
for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
{
iter->second->setSaved(false);
iter->second->setSaved(true);
}
_incrementalDictionary = _visualWords.size()==0;
driver->closeConnection(false);
@@ -222,6 +222,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
}
VisualWord * vw = new VisualWord(id, descriptor, 0);
vw->setSaved(true);
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
_notIndexedWords.insert(_notIndexedWords.end(), id);
_unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord*>(id, vw));