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

Memory: Added parameter Mem/UseOdomGravity, adding kGravity links when creating a node if IMU is present or Mem/UseOdomGravity is set. Fixed OpenCV4 related build errors on stereo fisheye rectification code. Signature: changed links from map to multimap to support having multiple self references (prior, gravity constraints...). DBReader: publish IMU orientation if a gravity link is detected.

Browse Source
This commit is contained in:
matlabbe
2019-05-31 15:36:35 -04:00
parent 71f7515775
commit e887d462ce
28 changed files with 1264 additions and 344 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/jni/RTABMapApp.cpp
+1
View File
@@ -102,6 +102,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), std::string("0.05")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMarkerLength(), std::string("0.0")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemUseOdomGravity(), "true"));
if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end())
{
if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("2") == 0) // GTSAM
corelib/include/rtabmap/core/DBDriver.h
+2 -2
View File
@@ -166,7 +166,7 @@ public:
bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
bool getLaserScanInfo(int signatureId, LaserScan & info) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
void getWeight(int signatureId, int & weight) const;
void getLastNodeIds(std::set<int> & ids) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
@@ -260,7 +260,7 @@ protected:
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
corelib/include/rtabmap/core/DBDriverSqlite3.h
+1 -1
View File
@@ -130,7 +130,7 @@ protected:
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
corelib/include/rtabmap/core/Graph.h
+2 -1
View File
@@ -142,7 +142,8 @@ std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
const std::multimap<int, Link> & links,
int from,
int to,
bool checkBothWays = true);
bool checkBothWays = true,
Link::Type type = Link::kUndef);
std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
const std::multimap<int, int> & links,
int from,
corelib/include/rtabmap/core/Link.h
+3 -2
View File
@@ -47,9 +47,10 @@ public:
kVirtualClosure,
kNeighborMerged,
kPosePrior, // Absolute pose in /world frame, From == To
kLandmark, // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To"
kPoseOdom, // Pose in /odom frame, From == To (mainly used for gravity constraints)
kLandmark, // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To" (landmark is negative id)
kGravity, // Orientation of the base frame accordingly to gravity (From == To)
kEnd,
kSelfRefLink = 97, // Include kPosePrior and kGravity (all links where From=To)
kAllWithLandmarks = 98,
kAllWithoutLandmarks = 99,
kUndef = 99};
corelib/include/rtabmap/core/Memory.h
+4 -3
View File
@@ -147,11 +147,11 @@ public:
const std::map<int, double> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
int getMaxStMemSize() const {return _maxStMemSize;}
std::map<int, Link> getNeighborLinks(int signatureId,
std::multimap<int, Link> getNeighborLinks(int signatureId,
bool lookInDatabase = false) const;
std::map<int, Link> getLoopClosureLinks(int signatureId,
std::multimap<int, Link> getLoopClosureLinks(int signatureId,
bool lookInDatabase = false) const;
std::map<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
std::multimap<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
bool lookInDatabase = false,
bool withLandmarks = false) const;
std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true, bool withLandmarks = false) const;
@@ -312,6 +312,7 @@ private:
float _rehearsalMaxAngle;
bool _rehearsalWeightIgnoredWhileMoving;
bool _useOdometryFeatures;
bool _useOdometryGravity;
bool _createOccupancyGrid;
int _visMaxFeatures;
bool _imagesAlreadyRectified;
corelib/include/rtabmap/core/Parameters.h
+3 -2
View File
@@ -220,10 +220,11 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, ImagePostDecimation, int, 1, "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
RTABMAP_PARAM(Mem, CompressionParallelized, bool, true, "Compression of sensor data is multi-threaded.");
RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()));
RTABMAP_PARAM(Mem, LaserScanNormalK, int, 0, "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanNormalRadius, float, 0.0, "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features instead of regenerating them.");
RTABMAP_PARAM(Mem, UseOdomGravity, bool, false, uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
RTABMAP_PARAM(Mem, CovOffDiagIgnored, bool, true, "Ignore off diagonal values of the covariance matrix.");
// KeypointMemory (Keypoint-based)
corelib/include/rtabmap/core/Signature.h
+2 -2
View File
@@ -96,7 +96,7 @@ public:
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _linksModified = modified;}
const std::map<int, Link> & getLinks() const {return _links;}
const std::multimap<int, Link> & getLinks() const {return _links;}
bool isSaved() const {return _saved;}
bool isModified() const {return _modified || _linksModified;}
bool isLinksModified() const {return _linksModified;}
@@ -143,7 +143,7 @@ private:
int _id;
int _mapId;
double _stamp;
std::map<int, Link> _links; // id, transform
std::multimap<int, Link> _links; // id, transform
std::map<int, Link> _landmarks;
int _weight;
std::string _label;
corelib/include/rtabmap/core/odometry/OdometryF2M.h
+1 -1
View File
@@ -50,7 +50,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity());
const Signature & getMap() const {return *map_;}
const Signature & getLastFrame() const {return *lastFrame_;}
virtual bool canProcessIMU() const {return true;}
virtual bool canProcessIMU() const;
virtual Odometry::Type getType() {return Odometry::kTypeF2M;}
corelib/src/DBDriver.cpp
+6 -6
View File
@@ -762,7 +762,7 @@ bool DBDriver::getNodeInfo(
return found;
}
void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
void DBDriver::loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type) const
{
bool found = false;
// look in the trash
@@ -782,7 +782,7 @@ void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Typ
}
if(type == Link::kLandmark || type == Link::kAllWithLandmarks)
{
uInsert(links, s->getLandmarks());
links.insert(s->getLandmarks().begin(), s->getLandmarks().end());
}
found = true;
}
@@ -1219,11 +1219,11 @@ void DBDriver::generateGraph(
if(otherSignatures.find(*i) == otherSignatures.end())
{
int id = *i;
std::map<int, Link> links;
std::multimap<int, Link> links;
this->loadLinks(id, links);
int weight = 0;
this->getWeight(id, weight);
for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
int weightNeighbor = 0;
if(otherSignatures.find(iter->first) == otherSignatures.end())
@@ -1281,9 +1281,9 @@ void DBDriver::generateGraph(
if(ids.find(i->first) != ids.end())
{
int id = i->second->id();
const std::map<int, Link> & links = i->second->getLinks();
const std::multimap<int, Link> & links = i->second->getLinks();
int weight = i->second->getWeight();
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
int weightNeighbor = 0;
const Signature * s = uValue(otherSignatures, iter->first, (Signature*)0);
corelib/src/DBDriverSqlite3.cpp
+5 -5
View File
@@ -3605,7 +3605,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
void DBDriverSqlite3::loadLinksQuery(
int signatureId,
std::map<int, Link> & links,
std::multimap<int, Link> & links,
Link::Type typeIn) const
{
links.clear();
@@ -4041,8 +4041,8 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
if((*j)->isLinksModified())
{
// Save links
const std::map<int, Link> & links = (*j)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
const std::multimap<int, Link> & links = (*j)->getLinks();
for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, i->second);
}
@@ -4155,8 +4155,8 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
{
// Save links
const std::map<int, Link> & links = (*jter)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
const std::multimap<int, Link> & links = (*jter)->getLinks();
for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, i->second);
}
corelib/src/DBReader.cpp
+26 -4
View File
@@ -344,7 +344,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
Transform globalPose;
cv::Mat globalPoseCov;
std::map<int, Link> priorLinks;
std::multimap<int, Link> priorLinks;
_dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior);
if( priorLinks.size() &&
!priorLinks.begin()->second.transform().isNull() &&
@@ -355,10 +355,21 @@ SensorData DBReader::getNextData(CameraInfo * info)
globalPoseCov = priorLinks.begin()->second.infMatrix().inv();
}
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)
{
gravityTransform = gravityLinks.begin()->second.transform();
}
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored)
{
std::map<int, Link> links;
std::multimap<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size() && links.begin()->first < *_currentId)
{
@@ -467,13 +478,24 @@ SensorData DBReader::getNextData(CameraInfo * info)
{
data.setGlobalPose(globalPose, globalPoseCov);
}
if(!gravityTransform.isNull())
{
Eigen::Quaterniond q = gravityTransform.getQuaterniond();
data.setIMU(IMU(
cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
cv::Vec3d(), cv::Mat(),
cv::Vec3d(), cv::Mat(),
Transform::getIdentity())); // we assume that gravity links are already transformed in base_link
}
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d",
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, IMU=%d",
data.laserScanRaw().isEmpty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1,
data.gridCellSize()==0.0f?0:1,
data.userDataRaw().empty()?0:1);
data.userDataRaw().empty()?0:1,
globalPose.isNull()?0:1,
gravityTransform.isNull()?0:1);
cv::Mat descriptors;
if(!s->getWordsDescriptors().empty())
corelib/src/Graph.cpp
+14 -6
View File
@@ -1031,12 +1031,13 @@ std::multimap<int, Link>::const_iterator findLink(
const std::multimap<int, Link> & links,
int from,
int to,
bool checkBothWays)
bool checkBothWays,
Link::Type type)
{
std::multimap<int, Link>::const_iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
{
return iter;
}
@@ -1049,7 +1050,7 @@ std::multimap<int, Link>::const_iterator findLink(
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second.to() == from)
if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
{
return iter;
}
@@ -1146,7 +1147,14 @@ std::map<int, Link> filterLinks(
std::map<int, Link> output;
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() != filteredType)
if(filteredType == Link::kSelfRefLink)
{
if(iter->second.from() != iter->second.to())
{
output.insert(*iter);
}
}
else if(iter->second.type() != filteredType)
{
output.insert(*iter);
}
@@ -1897,7 +1905,7 @@ std::list<std::pair<int, Transform> > computePath(
}
// lookup neighbors
std::map<int, Link> links;
std::multimap<int, Link> links;
if(allLinks.size() == 0)
{
links = memory->getLinks(currentNode->id(), lookInDatabase, true);
@@ -1911,7 +1919,7 @@ std::list<std::pair<int, Transform> > computePath(
links.insert(std::make_pair(iter->second.to(), iter->second));
}
}
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
{
corelib/src/Memory.cpp
+135 -112
View File
@@ -102,6 +102,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
_useOdometryGravity(Parameters::defaultMemUseOdomGravity()),
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
@@ -561,6 +562,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
Parameters::parse(params, Parameters::kMemUseOdomGravity(), _useOdometryGravity);
Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
@@ -1040,9 +1042,9 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
if(_reduceGraph)
{
bool merge = false;
const std::map<int, Link> & links = s->getLinks();
const std::multimap<int, Link> & links = s->getLinks();
std::map<int, Link> neighbors;
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(!merge)
{
@@ -1072,7 +1074,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
{
if(s->getLabel().empty())
{
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
merge = true;
Signature * sTo = this->_getSignature(iter->first);
@@ -1101,8 +1103,8 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
}
//remove neighbor links
std::map<int, Link> linksCopy = links;
for(std::map<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
std::multimap<int, Link> linksCopy = links;
for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
@@ -1146,16 +1148,16 @@ const VWDictionary * Memory::getVWDictionary() const
return _vwd;
}
std::map<int, Link> Memory::getNeighborLinks(
std::multimap<int, Link> Memory::getNeighborLinks(
int signatureId,
bool lookInDatabase) const
{
std::map<int, Link> links;
std::multimap<int, Link> links;
Signature * s = uValue(_signatures, signatureId, (Signature*)0);
if(s)
{
const std::map<int, Link> & allLinks = s->getLinks();
for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
const std::multimap<int, Link> & allLinks = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
@@ -1166,14 +1168,13 @@ std::map<int, Link> Memory::getNeighborLinks(
}
else if(lookInDatabase && _dbDriver)
{
std::map<int, Link> neighbors;
_dbDriver->loadLinks(signatureId, neighbors);
for(std::map<int, Link>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
_dbDriver->loadLinks(signatureId, links);
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
neighbors.erase(iter++);
links.erase(iter++);
}
else
{
@@ -1188,20 +1189,20 @@ std::map<int, Link> Memory::getNeighborLinks(
return links;
}
std::map<int, Link> Memory::getLoopClosureLinks(
std::multimap<int, Link> Memory::getLoopClosureLinks(
int signatureId,
bool lookInDatabase) const
{
const Signature * s = this->getSignature(signatureId);
std::map<int, Link> loopClosures;
std::multimap<int, Link> loopClosures;
if(s)
{
const std::map<int, Link> & allLinks = s->getLinks();
for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
const std::multimap<int, Link> & allLinks = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior &&
iter->second.from() != iter->second.to() &&
iter->second.type() != Link::kUndef)
{
loopClosures.insert(*iter);
@@ -1211,7 +1212,7 @@ std::map<int, Link> Memory::getLoopClosureLinks(
else if(lookInDatabase && _dbDriver)
{
_dbDriver->loadLinks(signatureId, loopClosures);
for(std::map<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
for(std::multimap<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged ||
@@ -1228,12 +1229,12 @@ std::map<int, Link> Memory::getLoopClosureLinks(
return loopClosures;
}
std::map<int, Link> Memory::getLinks(
std::multimap<int, Link> Memory::getLinks(
int signatureId,
bool lookInDatabase,
bool withLandmarks) const
{
std::map<int, Link> links;
std::multimap<int, Link> links;
if(signatureId > 0)
{
Signature * s = uValue(_signatures, signatureId, (Signature*)0);
@@ -1242,7 +1243,7 @@ std::map<int, Link> Memory::getLinks(
links = s->getLinks();
if(withLandmarks)
{
uInsert(links, s->getLandmarks());
links.insert(s->getLandmarks().begin(), s->getLandmarks().end());
}
}
else if(lookInDatabase && _dbDriver)
@@ -1372,9 +1373,9 @@ std::map<int, int> Memory::getNeighborsId(
//UDEBUG("Added %d with margin %d", *jter, m);
// Look up in STM/WM if all ids are here, if not... load them from the database
const Signature * s = this->getSignature(*jter);
std::map<int, Link> tmpLinks;
std::multimap<int, Link> tmpLinks;
std::map<int, Link> tmpLandmarks;
const std::map<int, Link> * links = &tmpLinks;
const std::multimap<int, Link> * links = &tmpLinks;
const std::map<int, Link> * landmarks = &tmpLandmarks;
if(s)
{
@@ -1402,7 +1403,7 @@ std::map<int, int> Memory::getNeighborsId(
_dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks);
if(!ignoreLoopIds)
{
for(std::map<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();)
for(std::multimap<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();)
{
if(kter->first < 0)
{
@@ -1422,7 +1423,7 @@ std::map<int, int> Memory::getNeighborsId(
}
// links
for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
{
if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
{
@@ -1525,8 +1526,6 @@ std::map<int, float> Memory::getNeighborsIdRadius(
//UDEBUG("Added %d with margin %d", *jter, m);
// Look up in STM/WM if all ids are here, if not... load them from the database
const Signature * s = this->getSignature(*jter);
std::map<int, Link> tmpLinks;
const std::map<int, Link> * links = &tmpLinks;
if(s)
{
const Transform & t = optimizedPoses.at(*jter);
@@ -1537,17 +1536,15 @@ std::map<int, float> Memory::getNeighborsIdRadius(
ids.insert(std::pair<int, float>(*jter,distanceSqrd));
}
links = &s->getLinks();
}
// links
for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
{
if(!uContains(ids, iter->first) &&
uContains(optimizedPoses, iter->first) &&
iter->second.type()!=Link::kVirtualClosure)
// links
for(std::multimap<int, Link>::const_iterator iter=s->getLinks().begin(); iter!=s->getLinks().end(); ++iter)
{
nextMargin.insert(iter->first);
if(!uContains(ids, iter->first) &&
uContains(optimizedPoses, iter->first) &&
iter->second.type()!=Link::kVirtualClosure)
{
nextMargin.insert(iter->first);
}
}
}
}
@@ -2316,8 +2313,8 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
UASSERT_MSG(this->isInSTM(s->id()),
uFormat("Deleting location (%d) outside the "
"STM is not implemented!", s->id()).c_str());
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
const std::multimap<int, Link> & links = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() != iter->second.to() && iter->first > 0)
{
@@ -2585,7 +2582,7 @@ void Memory::removeLink(int oldId, int newId)
if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id()))
{
Link::Type type = oldS->getLinks().at(newS->id()).type();
Link::Type type = oldS->getLinks().find(newS->id())->second.type();
if(type == Link::kGlobalClosure && newS->getWeight() > 0)
{
// adjust the weight
@@ -2607,7 +2604,7 @@ void Memory::removeLink(int oldId, int newId)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior &&
iter->second.from() != iter->second.to() &&
iter->first < newS->id())
{
noChildrenAnymore = false;
@@ -2779,7 +2776,7 @@ Transform Memory::computeTransform(
std::multimap<int, cv::KeyPoint> wordsMap;
std::multimap<int, cv::Mat> wordsDescriptorsMap;
const std::map<int, Link> & links = fromS.getLinks();
const std::multimap<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());
@@ -2795,22 +2792,25 @@ Transform Memory::computeTransform(
}
UDEBUG("words3DMap=%d", (int)words3DMap.size());
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
int id = iter->first;
const Signature * s = this->getSignature(id);
if(s)
if(id != fromS.id())
{
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)
const Signature * s = this->getSignature(id);
if(s)
{
if( jter->first > 0 &&
util3d::isFinite(jter->second) &&
words3DMap.find(jter->first) == words3DMap.end())
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)
{
words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
wordsMap.insert(*s->getWords().find(jter->first));
wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
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));
wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
}
}
}
}
@@ -2831,65 +2831,68 @@ Transform Memory::computeTransform(
std::map<int, CameraModel> bundleModels;
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::map<int, Link> links = fromS.getLinks();
std::multimap<int, Link> links = fromS.getLinks();
links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
links.insert(std::make_pair(fromS.id(), Link()));
for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
int id = iter->first;
const Signature * s;
if(id == tmpTo.id())
if(id != fromS.id())
{
s = &tmpTo; // reuse matched words
}
else
{
s = this->getSignature(id);
}
if(s)
{
CameraModel model;
if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
const Signature * s;
if(id == tmpTo.id())
{
model = s->sensorData().cameraModels()[0];
}
else if(s->sensorData().stereoCameraModel().isValidForProjection())
{
model = s->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s->sensorData().stereoCameraModel().baseline()*model.fx());
s = &tmpTo; // reuse matched words
}
else
{
UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
s = this->getSignature(id);
}
bundleModels.insert(std::make_pair(id, model));
Transform invLocalTransform = model.localTransform().inverse();
if(iter->second.isValid())
if(s)
{
bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
bundlePoses.insert(std::make_pair(id, iter->second.transform()));
}
else
{
bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
}
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() &&
(id == tmpTo.id() || jter->first > 0))
CameraModel model;
if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
{
std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
model = s->sensorData().cameraModels()[0];
}
else if(s->sensorData().stereoCameraModel().isValidForProjection())
{
model = s->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
}
bundleModels.insert(std::make_pair(id, model));
Transform invLocalTransform = model.localTransform().inverse();
if(iter->second.isValid())
{
bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
bundlePoses.insert(std::make_pair(id, iter->second.transform()));
}
else
{
bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
}
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() &&
(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);
wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
}
}
}
}
@@ -3231,7 +3234,7 @@ void Memory::updateLink(const Link & link, bool updateInDatabase)
{
if(fromS->hasLink(link.to()) && toS->hasLink(link.from()))
{
Link::Type oldType = fromS->getLinks().at(link.to()).type();
Link::Type oldType = fromS->getLinks().find(link.to())->second.type();
fromS->removeLink(link.to());
toS->removeLink(link.from());
@@ -3297,8 +3300,8 @@ void Memory::removeVirtualLinks(int signatureId)
Signature * s = this->_getSignature(signatureId);
if(s)
{
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
const std::multimap<int, Link> & links = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kVirtualClosure)
{
@@ -3557,15 +3560,15 @@ bool Memory::rehearsalMerge(int oldId, int newId)
if(fullMerge)
{
//remove mutual links
Link newToOldLink = newS->getLinks().at(oldS->id());
Link newToOldLink = newS->getLinks().find(oldS->id())->second;
oldS->removeLink(newId);
newS->removeLink(oldId);
if(_idUpdatedToNewOneRehearsal)
{
// redirect neighbor links
const std::map<int, Link> & links = oldS->getLinks();
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
const std::multimap<int, Link> & links = oldS->getLinks();
for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
{
@@ -4601,7 +4604,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
Landmarks landmarks = data.landmarks();
if(_detectMarkers)
if(_detectMarkers && !isIntermediateNode)
{
UDEBUG("Detecting markers...");
if(landmarks.empty())
@@ -5069,9 +5072,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
}
// prior
if(!isIntermediateNode)
{
// prior
if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
{
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
@@ -5105,6 +5108,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
}
}
// IMU / Gravity constraint
if(_useOdometryGravity && !pose.isNull())
{
s->addLink(Link(s->id(), s->id(), Link::kGravity, pose.rotation()));
UDEBUG("Added gravity constraint from odom pose: %s", pose.rotation().prettyPrint().c_str());
}
else if(!data.imu().localTransform().isNull() &&
(data.imu().orientation()[0] != 0 ||
data.imu().orientation()[1] != 0 ||
data.imu().orientation()[2] != 0 ||
data.imu().orientation()[3] != 0))
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
orientation*=data.imu().localTransform().rotation().inverse();
s->addLink(Link(s->id(), s->id(), Link::kGravity, orientation));
UDEBUG("Added gravity constraint: %s", orientation.prettyPrint().c_str());
}
}
//landmarks
@@ -5404,8 +5427,8 @@ void Memory::getMetricConstraints(
{
if(uContains(poses, *iter))
{
std::map<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true);
for(std::map<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
std::multimap<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true);
for(std::multimap<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
{
if( jter->second.isValid() &&
graph::findLink(links, *iter, jter->first) == links.end() &&
@@ -5424,9 +5447,9 @@ void Memory::getMetricConstraints(
{
// skip to next neighbor, well we assume that bad signatures
// are only linked by max 2 neighbor links.
std::map<int, Link> n = this->getNeighborLinks(s->id(), false);
std::multimap<int, Link> n = this->getNeighborLinks(s->id(), false);
UASSERT(n.size() <= 2);
std::map<int, Link>::iterator uter = n.upper_bound(s->id());
std::multimap<int, Link>::iterator uter = n.upper_bound(s->id());
if(uter != n.end())
{
const Signature * s2 = this->getSignature(uter->first);
corelib/src/Rtabmap.cpp
+67 -61
View File
@@ -1172,7 +1172,7 @@ bool Rtabmap::process(
//============================================================
// Minimum displacement required to add to Memory
//============================================================
const std::map<int, Link> & links = signature->getLinks();
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{
// don't do this if there are intermediate nodes
@@ -1292,9 +1292,9 @@ bool Rtabmap::process(
UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId));
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().at(signature->id()).transVariance());
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().find(signature->id())->second.transVariance());
newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
newPose = _optimizedPoses.at(oldId) * oldS->getLinks().find(signature->id())->second.transform();
_mapCorrection = newPose * signature->getPose().inverse();
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
{
@@ -1667,7 +1667,7 @@ bool Rtabmap::process(
{
float loopThr = _loopThr;
if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
_rgbdSlamMode)
@@ -2038,8 +2038,8 @@ bool Rtabmap::process(
{
// 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();
const std::multimap<int, Link> & links = s->getLinks();
for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin();
jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
++jter)
{
@@ -2175,7 +2175,7 @@ bool Rtabmap::process(
// Landmark
//============================================================
int landmarkDetected = 0;
int landmarkDetectedNodeRef = 0;
std::set<int> landmarkDetectedNodesRef;
if(!signature->getLandmarks().empty())
{
for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
@@ -2184,8 +2184,8 @@ bool Rtabmap::process(
_memory->getLandmarksInvertedIndex().find(iter->first)->second.size()>1)
{
landmarkDetected = iter->first;
landmarkDetectedNodeRef = *_memory->getLandmarksInvertedIndex().find(iter->first)->second.begin();
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, landmarkDetectedNodeRef);
landmarkDetectedNodesRef = _memory->getLandmarksInvertedIndex().find(iter->first)->second;
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *landmarkDetectedNodesRef.begin());
break;
}
}
@@ -2536,14 +2536,14 @@ bool Rtabmap::process(
(signature->hasLink(signature->id()) && !_graphOptimizer->priorsIgnored()) || // prior edge
proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kPosePrior).size()) && // In localization mode, the new node should be linked
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked
signaturesRetrieved.size()))) // can be different map of the current one
{
UASSERT(uContains(_optimizedPoses, signature->id()));
//used in localization mode: filter virtual links
std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
localizationLinks = graph::filterLinks(localizationLinks, Link::kPosePrior);
std::multimap<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
localizationLinks = graph::filterLinks(localizationLinks, Link::kSelfRefLink);
if(landmarkDetected!=0 && !_memory->isIncremental() && _optimizedPoses.find(landmarkDetected)!=_optimizedPoses.end())
{
UASSERT(uContains(signature->getLandmarks(), landmarkDetected));
@@ -2696,20 +2696,30 @@ bool Rtabmap::process(
//For landmarks, use transform against other node looking the landmark
// (because we don't assume that landmarks are aligned with gravity)
int landmarkId = loopId;
const Signature * loopS = _memory->getSignature(landmarkDetectedNodeRef);
UASSERT(!landmarkDetectedNodesRef.empty());
loopId = *landmarkDetectedNodesRef.begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * loopS->getLandmarks().at(landmarkId).transform().inverse();
loopId = landmarkDetectedNodeRef;
UASSERT(_optimizedPoses.find(loopId) != _optimizedPoses.end());
oldPose = _optimizedPoses.at(loopId);
}
float roll,pitch,yaw;
_memory->getSignature(loopId)->getPose().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = signature->getPose().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * signature->getPose().rotation().inverse() * targetRotation;
transform *= error;
const Signature * loopS = _memory->getSignature(loopId);
UASSERT(loopS !=0);
std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
if(iterGravityLoop!=loopS->getLinks().end() &&
iterGravitySign!=signature->getLinks().end())
{
float roll,pitch,yaw;
iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
transform *= error;
u = signature->getPose() * transform;
u = signature->getPose() * transform;
}
}
Transform up = u * oldPose.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
@@ -2736,19 +2746,32 @@ bool Rtabmap::process(
//For landmarks, use transform against other node looking the landmark
// (because we don't assume that landmarks are aligned with gravity)
int landmarkId = loopId;
const Signature * loopS = _memory->getSignature(landmarkDetectedNodeRef);
UASSERT(!landmarkDetectedNodesRef.empty());
loopId = *landmarkDetectedNodesRef.begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * loopS->getLandmarks().at(landmarkId).transform().inverse();
loopId = landmarkDetectedNodeRef;
}
float roll,pitch,yaw;
_memory->getSignature(loopId)->getPose().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = signature->getPose().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * signature->getPose().rotation().inverse() * targetRotation;
transform *= error;
const Signature * loopS = _memory->getSignature(loopId);
UASSERT(loopS !=0);
std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
if(iterGravityLoop!=loopS->getLinks().end() &&
iterGravitySign!=signature->getLinks().end())
{
float roll,pitch,yaw;
iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
transform *= error;
newPose = _optimizedPoses.at(loopId) * transform.inverse();
newPose = _optimizedPoses.at(loopId) * transform.inverse();
}
else
{
UWARN("Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->id());
}
}
_optimizedPoses.at(signature->id()) = newPose;
}
@@ -2930,7 +2953,7 @@ bool Rtabmap::process(
}
}
}
if(!hasPrior || _graphOptimizer->priorsIgnored())
if((!hasPrior || _graphOptimizer->priorsIgnored()) && _graphOptimizer->gravitySigma()==0.0f)
{
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
}
@@ -3010,7 +3033,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
statistics_.addStatistic(Statistics::kLoopLandmark_detected(), -landmarkDetected);
statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodeRef);
statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodesRef.empty()?0:*landmarkDetectedNodesRef.begin());
statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
@@ -3024,8 +3047,8 @@ bool Rtabmap::process(
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
{
UASSERT(uContains(sLoop->getLinks(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform());
UINFO("Set loop closure transform = %s", sLoop->getLinks().find(signature->id())->second.transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().find(signature->id())->second.transform());
}
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
@@ -3097,7 +3120,8 @@ bool Rtabmap::process(
Signature lastSignatureData(signature->id());
Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && graph::filterLinks(signature->getLinks(), Link::kPosePrior).size())
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() &&
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size())
{
// only if localized set it
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
@@ -3126,7 +3150,7 @@ bool Rtabmap::process(
{
if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
(landmarkDetected == 0 || rejectedHypothesis) && // if we re not seeing a landmark from a previous map
_memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
{
@@ -3137,7 +3161,7 @@ bool Rtabmap::process(
}
else if(_startNewMapOnGoodSignature &&
(signature->getLandmarks().empty() && signature->isBadSignature()) &&
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0) // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in the current map
{
UWARN("Ignoring location %d because a good signature (with enough features or with a landmark detected) is required before starting a new map!",
signature->id());
@@ -3587,9 +3611,9 @@ void Rtabmap::rejectLastLoopClosure()
{
if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
{
std::map<int, Link> links = _memory->getLinks(getLastLocationId(), false);
std::multimap<int, Link> links = _memory->getLinks(getLastLocationId(), false);
bool linksRemoved = false;
for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kGlobalClosure ||
iter->second.type() == Link::kLocalSpaceClosure ||
@@ -3872,8 +3896,8 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(const std::map<int, T
if(!valid)
{
// make sure it has a neighbor added to path
std::map<int, Link> links = _memory->getNeighborLinks(iter->first);
for(std::map<int, Link>::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter)
std::multimap<int, Link> links = _memory->getNeighborLinks(iter->first);
for(std::multimap<int, Link>::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter)
{
valid = path.find(kter->first) != path.end();
}
@@ -3980,12 +4004,6 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0 && _graphOptimizer->gravitySigma() > 0.0f)
{
// add odometry constraints
edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
// Apply guess poses (if some)
std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(iter->first);
if(foundGuess!=guessPoses.end())
@@ -4482,18 +4500,6 @@ int Rtabmap::detectMoreLoopClosures(
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());
if(_graphOptimizer->gravitySigma() > 0.0f)
{
for(std::map<int, Transform>::iterator jter=poses.lower_bound(1); jter!=poses.end(); ++jter)
{
std::map<int, Signature>::iterator ster = signatures.find(iter->first);
if(ster != signatures.end() && !ster->second.getPose().isNull())
{
// add odometry constraints
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, ster->second.getPose())));
}
}
}
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
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());
@@ -5090,9 +5096,9 @@ void Rtabmap::updateGoalIndex()
UASSERT(_pathCurrentIndex < _path.size());
const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first);
UASSERT_MSG(currentIndexS != 0, uFormat("_path[%d].first=%d", _pathCurrentIndex, _path[_pathCurrentIndex].first).c_str());
std::map<int, Link> links = currentIndexS->getLinks(); // make a copy
std::multimap<int, Link> links = currentIndexS->getLinks(); // make a copy
bool latestVirtualLinkFound = false;
for(std::map<int, Link>::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter)
for(std::multimap<int, Link>::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter)
{
if(iter->second.type() == Link::kVirtualClosure)
{
corelib/src/Signature.cpp
+8 -8
View File
@@ -120,9 +120,9 @@ void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
_links.insert(std::make_pair(link.to(), link));
_linksModified = true;
}
@@ -133,11 +133,11 @@ bool Signature::hasLink(int idTo) const
void Signature::changeLinkIds(int idFrom, int idTo)
{
std::map<int, Link>::iterator iter = _links.find(idFrom);
if(iter != _links.end())
std::multimap<int, Link>::iterator iter = _links.find(idFrom);
while(iter != _links.end() && iter->first == idFrom)
{
Link link = iter->second;
_links.erase(iter);
_links.erase(iter++);
link.setTo(idTo);
_links.insert(std::make_pair(idTo, link));
_linksModified = true;
@@ -164,7 +164,7 @@ void Signature::removeLink(int idTo)
void Signature::removeVirtualLinks()
{
for(std::map<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
for(std::multimap<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
{
if(iter->second.type() == Link::kVirtualClosure)
{
@@ -276,7 +276,7 @@ cv::Mat Signature::getPoseCovariance() const
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(_links.size())
{
for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
{
if(iter->second.kNeighbor)
{
corelib/src/VWDictionary.cpp
+2 -4
View File
@@ -1173,7 +1173,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
std::map<int, int> mapIndexIdNotIndexed;
std::vector<std::vector<cv::DMatch> > matchesNotIndexed;
if(_notIndexedWords.size())
if(!_notIndexedWords.empty())
{
cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), query.cols, query.type());
unsigned int index = 0;
@@ -1198,12 +1198,10 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
{
descriptor = vw->getDescriptor();
}
UASSERT(vw != 0 && descriptor.cols == query.cols && descriptor.type() == query.type());
vw->getDescriptor().copyTo(dataNotIndexed.row(index));
descriptor.copyTo(dataNotIndexed.row(index));
mapIndexIdNotIndexed.insert(mapIndexIdNotIndexed.end(), std::pair<int,int>(index, vw->id()));
}
// Find nearest neighbor
ULOGGER_DEBUG("Searching in words not indexed...");
cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR);
corelib/src/odometry/OdometryF2M.cpp
+7 -10
View File
@@ -180,6 +180,11 @@ void OdometryF2M::reset(const Transform & initialPose)
initGravity_ = false;
}
bool OdometryF2M::canProcessIMU() const
{
return sba_ && sba_->gravitySigma() > 0.0f;
}
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
SensorData & data,
@@ -332,14 +337,6 @@ Transform OdometryF2M::computeTransform(
{
double stampDiff = 0.0;
imuT = getClosestIMU(lastFrame_->getStamp(), stampDiff);
if(stampDiff < 0.05)
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT)));
}
else
{
UWARN("IMUs are set, but we could not find one matching the current frame stamp %f (stampDiff=%f > 0.05)", lastFrame_->getStamp(), stampDiff);
}
}
// local bundle adjustment
@@ -380,7 +377,7 @@ Transform OdometryF2M::computeTransform(
if(!imuT.isNull())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT)));
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
}
CameraModel model;
@@ -1191,7 +1188,7 @@ Transform OdometryF2M::computeTransform(
if(!imus_.empty())
{
bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, newFramePose)));
bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, newFramePose)));
}
}
corelib/src/optimizer/OptimizerG2O.cpp
+7 -3
View File
@@ -530,7 +530,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Eigen::Matrix<double, 6, 1> m;
// Up vector in robot frame
@@ -1403,7 +1403,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
#ifndef RTABMAP_ORB_SLAM2
g2o::HyperGraph::Edge * edge = 0;
if(gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Eigen::Matrix<double, 6, 1> m;
// Up vector in robot frame
@@ -1932,7 +1932,11 @@ bool OptimizerG2O::saveGraph(
bool isSE2 = true;
bool isSE3 = true;
if (iter->second.type() == Link::kPosePrior)
if (iter->second.type() == Link::kGravity)
{
continue;
}
else if (iter->second.type() == Link::kPosePrior)
{
if (this->priorsIgnored())
{
corelib/src/optimizer/OptimizerGTSAM.cpp
+4 -6
View File
@@ -282,7 +282,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
@@ -376,13 +376,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
else
else // id1 != id2
{
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
iter->second.type() != Link::kNeighborMerged)
{
// create new switch variable
// Sunderhauf IROS 2012:
@@ -451,8 +450,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
corelib/src/optimizer/OptimizerTORO.cpp
+1 -1
View File
@@ -395,7 +395,7 @@ bool OptimizerTORO::saveGraph(
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if (iter->second.type() != Link::kPosePrior)
if (iter->second.type() != Link::kPosePrior && iter->second.type() != Link::kGravity)
{
if (isSlam2d())
{
guilib/src/DatabaseViewer.cpp
+15 -38
View File
@@ -213,6 +213,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
uInsert(parameters, Parameters::getDefaultParameters("Grid"));
uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
uInsert(parameters, Parameters::getDefaultParameters("Marker"));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopCovLimited()));
@@ -1305,7 +1306,7 @@ void DatabaseViewer::exportDatabase()
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(dialog.isOdomExported())
{
std::map<int, Link> links;
std::multimap<int, Link> links;
dbDriver_->loadLinks(id, links, Link::kNeighbor);
if(links.size() && links.begin()->first < id)
{
@@ -1877,7 +1878,7 @@ void DatabaseViewer::updateIds()
{
neighborLinks_.append(iter->second);
}
else if(iter->second.type()!=rtabmap::Link::kPosePrior)
else if(iter->second.from()!=iter->second.to())
{
loopLinks_.append(iter->second);
}
@@ -3133,14 +3134,14 @@ void DatabaseViewer::generateLocalGraph()
{
if(ids.find(*jter) == ids.end())
{
std::map<int, Link> links;
std::multimap<int, Link> links;
ids.insert(std::pair<int, int>(*jter, m));
UTimer timer;
dbDriver_->loadLinks(*jter, links);
// links
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if( !uContains(ids, iter->first))
{
@@ -4495,12 +4496,12 @@ void DatabaseViewer::update(int value,
}
// loops
std::map<int, rtabmap::Link> links;
std::multimap<int, rtabmap::Link> links;
dbDriver_->loadLinks(id, links);
if(links.size())
{
QString strParents, strChildren;
for(std::map<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
@@ -5350,13 +5351,6 @@ void DatabaseViewer::updateConstraintView(
}
}
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
linksOut.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, odomPoses_.at(iter->first))));
}
}
QTime time;
time.start();
@@ -6063,6 +6057,7 @@ void DatabaseViewer::updateGraphView()
int totalUser = 0;
int totalPriors = 0;
int totalLandmarks = 0;
int totalGravity = 0;
for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.type() == Link::kNeighbor)
@@ -6127,6 +6122,10 @@ void DatabaseViewer::updateGraphView()
{
++totalPriors;
}
else if(iter->second.type() == Link::kGravity)
{
++totalGravity;
}
else
{
loopLinks_.push_back(iter->second);
@@ -6135,7 +6134,7 @@ void DatabaseViewer::updateGraphView()
}
updateLoopClosuresSlider();
ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7, %8)")
ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7, %8, %9)")
.arg(totalNeighbor)
.arg(totalNeighborMerged)
.arg(totalGlobal)
@@ -6143,7 +6142,8 @@ void DatabaseViewer::updateGraphView()
.arg(totalLocalTime)
.arg(totalUser)
.arg(totalPriors)
.arg(totalLandmarks));
.arg(totalLandmarks)
.arg(totalGravity));
// remove intermediate nodes?
if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
@@ -6180,14 +6180,6 @@ void DatabaseViewer::updateGraphView()
Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, rtabmap::Transform>::iterator iter=poses.upper_bound(0); iter!=poses.end(); ++iter)
{
links.insert(std::make_pair(iter->first, Link(iter->first,iter->first, Link::kPoseOdom, iter->second)));
}
}
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
@@ -6526,14 +6518,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
}
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
{
linksOut.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, odomPoses_.at(iter->first))));
}
}
scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
delete optimizer;
@@ -6925,13 +6909,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)odomPoses_.size(), (int)linksIn.size()).c_str());
UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)odomPoses_.size(), (int)linksIn.size()).c_str());
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=odomPoses_.begin(); iter!=odomPoses_.end(); ++iter)
{
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
}
optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
// use already optimized poses
if(graphes_.size())
guilib/src/MainWindow.cpp
-21
View File
@@ -5749,13 +5749,6 @@ void MainWindow::postProcessing()
UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
UASSERT_MSG(_currentPosesMap.find(from) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(_currentPosesMap.find(to) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator jter=odomPoses.begin(); jter!=odomPoses.end(); ++jter)
{
linksIn.insert(std::make_pair(jter->first, Link(jter->first, jter->first, Link::kPoseOdom, jter->second)));
}
}
optimizer->getConnectedGraph(fromId, _currentPosesMap, linksIn, poses, links);
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());
@@ -5867,13 +5860,6 @@ void MainWindow::postProcessing()
std::multimap<int, rtabmap::Link> linksOut;
std::map<int, rtabmap::Transform> optimizedPoses;
std::multimap<int, rtabmap::Link> linksIn = _currentLinksMap;
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
{
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
}
optimizer->getConnectedGraph(
fromId,
_currentPosesMap,
@@ -5983,13 +5969,6 @@ void MainWindow::postProcessing()
std::multimap<int, rtabmap::Link> linksOut;
std::map<int, rtabmap::Transform> optimizedPoses;
std::multimap<int, rtabmap::Link> linksIn = _currentLinksMap;
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
{
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
}
optimizer->getConnectedGraph(
fromId,
_currentPosesMap,
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -915,6 +915,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str());
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDNeighborLinkRefining().c_str());
_ui->odomGravity->setObjectName(Parameters::kMemUseOdomGravity().c_str());
_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());
_ui->rgbd_loopCovLimited->setObjectName(Parameters::kRGBDLoopCovLimited().c_str());
guilib/src/opencv/stereoRectifyFisheye.h
+881 -1
View File
@@ -35,7 +35,887 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/calib3d/calib3d.hpp>
#if CV_MAJOR_VERSION >= 3
#include <opencv2/calib3d/calib3d_c.h>
#endif
#if CV_MAJOR_VERSION >= 4
// Opencv4 doesn't expose those functions below anymore, we should recopy all of them!
int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian CV_DEFAULT(0))
{
int depth, elem_size;
int i, k;
double J[27] = {0};
CvMat matJ = cvMat( 3, 9, CV_64F, J );
if( !CV_IS_MAT(src) )
CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" );
if( !CV_IS_MAT(dst) )
CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
"The first output argument is not a valid matrix" );
depth = CV_MAT_DEPTH(src->type);
elem_size = CV_ELEM_SIZE(depth);
if( depth != CV_32F && depth != CV_64F )
CV_Error( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );
if( !CV_ARE_DEPTHS_EQ(src, dst) )
CV_Error( CV_StsUnmatchedFormats, "All the matrices must have the same data type" );
if( jacobian )
{
if( !CV_IS_MAT(jacobian) )
CV_Error( CV_StsBadArg, "Jacobian is not a valid matrix" );
if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 )
CV_Error( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" );
if( (jacobian->rows != 9 || jacobian->cols != 3) &&
(jacobian->rows != 3 || jacobian->cols != 9))
CV_Error( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" );
}
if( src->cols == 1 || src->rows == 1 )
{
int step = src->rows > 1 ? src->step / elem_size : 1;
if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 )
CV_Error( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" );
if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 )
CV_Error( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" );
cv::Point3d r;
if( depth == CV_32F )
{
r.x = src->data.fl[0];
r.y = src->data.fl[step];
r.z = src->data.fl[step*2];
}
else
{
r.x = src->data.db[0];
r.y = src->data.db[step];
r.z = src->data.db[step*2];
}
double theta = cv::norm(r);
if( theta < DBL_EPSILON )
{
cvSetIdentity( dst );
if( jacobian )
{
memset( J, 0, sizeof(J) );
J[5] = J[15] = J[19] = -1;
J[7] = J[11] = J[21] = 1;
}
}
else
{
double c = cos(theta);
double s = sin(theta);
double c1 = 1. - c;
double itheta = theta ? 1./theta : 0.;
r *= itheta;
cv::Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z );
cv::Matx33d r_x( 0, -r.z, r.y,
r.z, 0, -r.x,
-r.y, r.x, 0 );
// R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
cv::Matx33d R = c*cv::Matx33d::eye() + c1*rrt + s*r_x;
cv::Mat(R).convertTo(cv::cvarrToMat(dst), dst->type);
if( jacobian )
{
const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0,
0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0,
0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z };
double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
0, 0, 1, 0, 0, 0, -1, 0, 0,
0, -1, 0, 1, 0, 0, 0, 0, 0 };
for( i = 0; i < 3; i++ )
{
double ri = i == 0 ? r.x : i == 1 ? r.y : r.z;
double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
double a3 = (c - s*itheta)*ri, a4 = s*itheta;
for( k = 0; k < 9; k++ )
J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] +
a3*r_x.val[k] + a4*d_r_x_[i*9+k];
}
}
}
}
else if( src->cols == 3 && src->rows == 3 )
{
cv::Matx33d U, Vt;
cv::Vec3d W;
double theta, s, c;
int step = dst->rows > 1 ? dst->step / elem_size : 1;
if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) &&
(dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
cv::Matx33d R = cv::cvarrToMat(src);
if( !cv::checkRange(R, true, NULL, -100, 100) )
{
cvZero(dst);
if( jacobian )
cvZero(jacobian);
return 0;
}
cv::SVD::compute(R, W, U, Vt);
R = U*Vt;
cv::Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25);
c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5;
c = c > 1. ? 1. : c < -1. ? -1. : c;
theta = acos(c);
if( s < 1e-5 )
{
double t;
if( c > 0 )
r = cv::Point3d(0, 0, 0);
else
{
t = (R(0, 0) + 1)*0.5;
r.x = std::sqrt(MAX(t,0.));
t = (R(1, 1) + 1)*0.5;
r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.);
t = (R(2, 2) + 1)*0.5;
r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.);
if( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) )
r.z = -r.z;
theta /= cv::norm(r);
r *= theta;
}
if( jacobian )
{
memset( J, 0, sizeof(J) );
if( c > 0 )
{
J[5] = J[15] = J[19] = -0.5;
J[7] = J[11] = J[21] = 0.5;
}
}
}
else
{
double vth = 1/(2*s);
if( jacobian )
{
double t, dtheta_dtr = -1./s;
// var1 = [vth;theta]
// var = [om1;var1] = [om1;vth;theta]
double dvth_dtheta = -vth*c/s;
double d1 = 0.5*dvth_dtheta*dtheta_dtr;
double d2 = 0.5*dtheta_dtr;
// dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
double dvardR[5*9] =
{
0, 0, 0, 0, 0, 1, 0, -1, 0,
0, 0, -1, 0, 0, 0, 1, 0, 0,
0, 1, 0, -1, 0, 0, 0, 0, 0,
d1, 0, 0, 0, d1, 0, 0, 0, d1,
d2, 0, 0, 0, d2, 0, 0, 0, d2
};
// var2 = [om;theta]
double dvar2dvar[] =
{
vth, 0, 0, r.x, 0,
0, vth, 0, r.y, 0,
0, 0, vth, r.z, 0,
0, 0, 0, 0, 1
};
double domegadvar2[] =
{
theta, 0, 0, r.x*vth,
0, theta, 0, r.y*vth,
0, 0, theta, r.z*vth
};
CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
double t0[3*5];
CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
cvMatMul( &_t0, &_dvardR, &matJ );
// transpose every row of matJ (treat the rows as 3x3 matrices)
CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);
CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);
CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);
}
vth *= theta;
r *= vth;
}
if( depth == CV_32F )
{
dst->data.fl[0] = (float)r.x;
dst->data.fl[step] = (float)r.y;
dst->data.fl[step*2] = (float)r.z;
}
else
{
dst->data.db[0] = r.x;
dst->data.db[step] = r.y;
dst->data.db[step*2] = r.z;
}
}
if( jacobian )
{
if( depth == CV_32F )
{
if( jacobian->rows == matJ.rows )
cvConvert( &matJ, jacobian );
else
{
float Jf[3*9];
CvMat _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf );
cvConvert( &matJ, &_Jf );
cvTranspose( &_Jf, jacobian );
}
}
else if( jacobian->rows == matJ.rows )
cvCopy( &matJ, jacobian );
else
cvTranspose( &matJ, jacobian );
}
return 1;
}
template <typename FLOAT>
void computeTiltProjectionMatrix(FLOAT tauX,
FLOAT tauY,
cv::Matx<FLOAT, 3, 3>* matTilt = 0,
cv::Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
cv::Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
cv::Matx<FLOAT, 3, 3>* invMatTilt = 0)
{
FLOAT cTauX = cos(tauX);
FLOAT sTauX = sin(tauX);
FLOAT cTauY = cos(tauY);
FLOAT sTauY = sin(tauY);
cv::Matx<FLOAT, 3, 3> matRotX = cv::Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
cv::Matx<FLOAT, 3, 3> matRotY = cv::Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
cv::Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
cv::Matx<FLOAT, 3, 3> matProjZ = cv::Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
if (matTilt)
{
// Matrix for trapezoidal distortion of tilted image sensor
*matTilt = matProjZ * matRotXY;
}
if (dMatTiltdTauX)
{
// Derivative with respect to tauX
cv::Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * cv::Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
cv::Matx<FLOAT, 3, 3> dMatProjZdTauX = cv::Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
*dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
}
if (dMatTiltdTauY)
{
// Derivative with respect to tauY
cv::Matx<FLOAT, 3, 3> dMatRotXYdTauY = cv::Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
cv::Matx<FLOAT, 3, 3> dMatProjZdTauY = cv::Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
*dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
}
if (invMatTilt)
{
FLOAT inv = 1./matRotXY(2,2);
cv::Matx<FLOAT, 3, 3> invMatProjZ = cv::Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
*invMatTilt = matRotXY.t()*invMatProjZ;
}
}
void cvProjectPoints2Internal( const CvMat* objectPoints,
const CvMat* r_vec,
const CvMat* t_vec,
const CvMat* A,
const CvMat* distCoeffs,
CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
CvMat* dpdo CV_DEFAULT(NULL),
double aspectRatio CV_DEFAULT(0) )
{
cv::Ptr<CvMat> matM, _m;
cv::Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
cv::Ptr<CvMat> _dpdo;
int i, j, count;
int calc_derivatives;
const CvPoint3D64f* M;
CvPoint2D64f* m;
double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
cv::Matx33d matTilt = cv::Matx33d::eye();
cv::Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
cv::Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
double* dpdo_p = 0;
int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
int dpdo_step = 0;
bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
!CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
/*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
if(total % 3 != 0)
{
//we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
}
count = total / 3;
if( CV_IS_CONT_MAT(objectPoints->type) &&
(CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
(objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
(objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
{
matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
cvConvert(objectPoints, matM);
}
else
{
// matM = cvCreateMat( 1, count, CV_64FC3 );
// cvConvertPointsHomogeneous( objectPoints, matM );
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
}
if( CV_IS_CONT_MAT(imagePoints->type) &&
(CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) ||
(imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2) ||
(imagePoints->rows == 2 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
{
_m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
cvConvert(imagePoints, _m);
}
else
{
// _m = cvCreateMat( 1, count, CV_64FC2 );
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
}
M = (CvPoint3D64f*)matM->data.db;
m = (CvPoint2D64f*)_m->data.db;
if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
(((r_vec->rows != 1 && r_vec->cols != 1) ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
"floating-point rotation vector, or 3x3 rotation matrix" );
if( r_vec->rows == 3 && r_vec->cols == 3 )
{
_r = cvMat( 3, 1, CV_64FC1, r );
cvRodrigues2( r_vec, &_r );
cvRodrigues2( &_r, &matR, &_dRdr );
cvCopy( r_vec, &matR );
}
else
{
_r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
cvConvert( r_vec, &_r );
cvRodrigues2( &_r, &matR, &_dRdr );
}
if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
(t_vec->rows != 1 && t_vec->cols != 1) ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_Error( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
_t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
cvConvert( t_vec, &_t );
if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
A->rows != 3 || A->cols != 3 )
CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
cvConvert( A, &_a );
fx = a[0]; fy = a[4];
cx = a[2]; cy = a[5];
if( fixedAspectRatio )
fx = fy*aspectRatio;
if( distCoeffs )
{
if( !CV_IS_MAT(distCoeffs) ||
(CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
(distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
(distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
CV_Error( CV_StsBadArg, "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector");
_k = cvMat( distCoeffs->rows, distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
cvConvert( distCoeffs, &_k );
if(k[12] != 0 || k[13] != 0)
{
computeTiltProjectionMatrix(k[12], k[13],
&matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
}
}
if( dpdr )
{
if( !CV_IS_MAT(dpdr) ||
(CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
dpdr->rows != count*2 || dpdr->cols != 3 )
CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
{
_dpdr.reset(cvCloneMat(dpdr));
}
else
_dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
dpdr_p = _dpdr->data.db;
dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
}
if( dpdt )
{
if( !CV_IS_MAT(dpdt) ||
(CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
dpdt->rows != count*2 || dpdt->cols != 3 )
CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
{
_dpdt.reset(cvCloneMat(dpdt));
}
else
_dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
dpdt_p = _dpdt->data.db;
dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
}
if( dpdf )
{
if( !CV_IS_MAT(dpdf) ||
(CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
dpdf->rows != count*2 || dpdf->cols != 2 )
CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
{
_dpdf.reset(cvCloneMat(dpdf));
}
else
_dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
dpdf_p = _dpdf->data.db;
dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
}
if( dpdc )
{
if( !CV_IS_MAT(dpdc) ||
(CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
dpdc->rows != count*2 || dpdc->cols != 2 )
CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
{
_dpdc.reset(cvCloneMat(dpdc));
}
else
_dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
dpdc_p = _dpdc->data.db;
dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
}
if( dpdk )
{
if( !CV_IS_MAT(dpdk) ||
(CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
if( !distCoeffs )
CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
{
_dpdk.reset(cvCloneMat(dpdk));
}
else
_dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
dpdk_p = _dpdk->data.db;
dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
}
if( dpdo )
{
if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
&& CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
|| dpdo->rows != count * 2 || dpdo->cols != count * 3 )
CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
{
_dpdo.reset( cvCloneMat( dpdo ) );
}
else
_dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
cvZero(_dpdo);
dpdo_p = _dpdo->data.db;
dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
}
calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
for( i = 0; i < count; i++ )
{
double X = M[i].x, Y = M[i].y, Z = M[i].z;
double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
double r2, r4, r6, a1, a2, a3, cdist, icdist2;
double xd, yd, xd0, yd0, invProj;
cv::Vec3d vecTilt;
cv::Vec3d dVecTilt;
cv::Matx22d dMatTilt;
cv::Vec2d dXdYd;
double z0 = z;
z = z ? 1./z : 1;
x *= z; y *= z;
r2 = x*x + y*y;
r4 = r2*r2;
r6 = r4*r2;
a1 = 2*x*y;
a2 = r2 + 2*x*x;
a3 = r2 + 2*y*y;
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
// additional distortion by projecting onto a tilt plane
vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
xd = invProj * vecTilt(0);
yd = invProj * vecTilt(1);
m[i].x = xd*fx + cx;
m[i].y = yd*fy + cy;
if( calc_derivatives )
{
if( dpdc_p )
{
dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
dpdc_p[dpdc_step] = 0;
dpdc_p[dpdc_step+1] = 1;
dpdc_p += dpdc_step*2;
}
if( dpdf_p )
{
if( fixedAspectRatio )
{
dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
dpdf_p[dpdf_step] = 0;
dpdf_p[dpdf_step+1] = yd;
}
else
{
dpdf_p[0] = xd; dpdf_p[1] = 0;
dpdf_p[dpdf_step] = 0;
dpdf_p[dpdf_step+1] = yd;
}
dpdf_p += dpdf_step*2;
}
for (int row = 0; row < 2; ++row)
for (int col = 0; col < 2; ++col)
dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
- matTilt(2,col)*vecTilt(row);
double invProjSquare = (invProj*invProj);
dMatTilt *= invProjSquare;
if( dpdk_p )
{
dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r2, y*icdist2*r2);
dpdk_p[0] = fx*dXdYd(0);
dpdk_p[dpdk_step] = fy*dXdYd(1);
dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r4, y*icdist2*r4);
dpdk_p[1] = fx*dXdYd(0);
dpdk_p[dpdk_step+1] = fy*dXdYd(1);
if( _dpdk->cols > 2 )
{
dXdYd = dMatTilt*cv::Vec2d(a1, a3);
dpdk_p[2] = fx*dXdYd(0);
dpdk_p[dpdk_step+2] = fy*dXdYd(1);
dXdYd = dMatTilt*cv::Vec2d(a2, a1);
dpdk_p[3] = fx*dXdYd(0);
dpdk_p[dpdk_step+3] = fy*dXdYd(1);
if( _dpdk->cols > 4 )
{
dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r6, y*icdist2*r6);
dpdk_p[4] = fx*dXdYd(0);
dpdk_p[dpdk_step+4] = fy*dXdYd(1);
if( _dpdk->cols > 5 )
{
dXdYd = dMatTilt*cv::Vec2d(
x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
dpdk_p[5] = fx*dXdYd(0);
dpdk_p[dpdk_step+5] = fy*dXdYd(1);
dXdYd = dMatTilt*cv::Vec2d(
x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
dpdk_p[6] = fx*dXdYd(0);
dpdk_p[dpdk_step+6] = fy*dXdYd(1);
dXdYd = dMatTilt*cv::Vec2d(
x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
dpdk_p[7] = fx*dXdYd(0);
dpdk_p[dpdk_step+7] = fy*dXdYd(1);
if( _dpdk->cols > 8 )
{
dXdYd = dMatTilt*cv::Vec2d(r2, 0);
dpdk_p[8] = fx*dXdYd(0); //s1
dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
dXdYd = dMatTilt*cv::Vec2d(r4, 0);
dpdk_p[9] = fx*dXdYd(0); //s2
dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
dXdYd = dMatTilt*cv::Vec2d(0, r2);
dpdk_p[10] = fx*dXdYd(0);//s3
dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
dXdYd = dMatTilt*cv::Vec2d(0, r4);
dpdk_p[11] = fx*dXdYd(0);//s4
dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
if( _dpdk->cols > 12 )
{
dVecTilt = dMatTiltdTauX * cv::Vec3d(xd0, yd0, 1);
dpdk_p[12] = fx * invProjSquare * (
dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
dpdk_p[dpdk_step+12] = fy*invProjSquare * (
dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
dVecTilt = dMatTiltdTauY * cv::Vec3d(xd0, yd0, 1);
dpdk_p[13] = fx * invProjSquare * (
dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
dpdk_p[dpdk_step+13] = fy * invProjSquare * (
dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
}
}
}
}
}
dpdk_p += dpdk_step*2;
}
if( dpdt_p )
{
double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
for( j = 0; j < 3; j++ )
{
double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
dXdYd = dMatTilt*cv::Vec2d(dmxdt, dmydt);
dpdt_p[j] = fx*dXdYd(0);
dpdt_p[dpdt_step+j] = fy*dXdYd(1);
}
dpdt_p += dpdt_step*2;
}
if( dpdr_p )
{
double dx0dr[] =
{
X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
};
double dy0dr[] =
{
X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
};
double dz0dr[] =
{
X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
};
for( j = 0; j < 3; j++ )
{
double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
double dydr = z*(dy0dr[j] - y*dz0dr[j]);
double dr2dr = 2*x*dxdr + 2*y*dydr;
double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
double da1dr = 2*(x*dydr + y*dxdr);
double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
dXdYd = dMatTilt*cv::Vec2d(dmxdr, dmydr);
dpdr_p[j] = fx*dXdYd(0);
dpdr_p[dpdr_step+j] = fy*dXdYd(1);
}
dpdr_p += dpdr_step*2;
}
if( dpdo_p )
{
double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
z * ( R[1] - x * z * z0 * R[7] ),
z * ( R[2] - x * z * z0 * R[8] ) };
double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
z * ( R[4] - y * z * z0 * R[7] ),
z * ( R[5] - y * z * z0 * R[8] ) };
for( j = 0; j < 3; j++ )
{
double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
double dr4do = 2 * r2 * dr2do;
double dr6do = 3 * r4 * dr2do;
double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
double da2do = dr2do + 4 * x * dxdo[j];
double da3do = dr2do + 4 * y * dydo[j];
double dcdist_do
= k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
double dicdist2_do = -icdist2 * icdist2
* ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
double dxd0_do = cdist * icdist2 * dxdo[j]
+ x * icdist2 * dcdist_do + x * cdist * dicdist2_do
+ k[2] * da1do + k[3] * da2do + k[8] * dr2do
+ k[9] * dr4do;
double dyd0_do = cdist * icdist2 * dydo[j]
+ y * icdist2 * dcdist_do + y * cdist * dicdist2_do
+ k[2] * da3do + k[3] * da1do + k[10] * dr2do
+ k[11] * dr4do;
dXdYd = dMatTilt * cv::Vec2d( dxd0_do, dyd0_do );
dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
}
dpdo_p += dpdo_step * 2;
}
}
}
if( _m != imagePoints )
cvConvert( _m, imagePoints );
if( _dpdr != dpdr )
cvConvert( _dpdr, dpdr );
if( _dpdt != dpdt )
cvConvert( _dpdt, dpdt );
if( _dpdf != dpdf )
cvConvert( _dpdf, dpdf );
if( _dpdc != dpdc )
cvConvert( _dpdc, dpdc );
if( _dpdk != dpdk )
cvConvert( _dpdk, dpdk );
if( _dpdo != dpdo )
cvConvert( _dpdo, dpdo );
}
void cvProjectPoints2( const CvMat* objectPoints,
const CvMat* r_vec,
const CvMat* t_vec,
const CvMat* A,
const CvMat* distCoeffs,
CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
double aspectRatio CV_DEFAULT(0))
{
cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
dpdf, dpdc, dpdk, NULL, aspectRatio );
}
void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst )
{
cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
const cv::Mat dst0 = dst;
int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows);
if( src.channels() == 1 && src.cols > d0 )
cv::transpose(src, src);
int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows);
if( d0 == d1 )
src.copyTo(dst);
else if( d0 < d1 )
cv::convertPointsToHomogeneous(src, dst);
else
cv::convertPointsFromHomogeneous(src, dst);
bool tflag = dst0.channels() == 1 && dst0.cols > d1;
dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows));
if( tflag )
{
CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows );
if( dst0.type() == dst.type() )
transpose( dst, dst0 );
else
{
transpose( dst, dst );
dst.convertTo( dst0, dst0.type() );
}
}
else
{
CV_Assert( dst.size() == dst0.size() );
if( dst.data != dst0.data )
dst.convertTo(dst0, dst0.type());
}
}
#endif // OpenCV4
#endif // OpenCV3
namespace rtabmap
{
guilib/src/ui/DatabaseViewer.ui
+4 -4
View File
@@ -1089,7 +1089,7 @@
<item row="9" column="1">
<widget class="QLabel" name="label_41">
<property name="text">
<string>Links (N, NM, G, LS, LT, U, P, LM)</string>
<string>Links (N, NM, G, LS, LT, U, P, LM, GR)</string>
</property>
</widget>
</item>
@@ -1454,8 +1454,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>519</width>
<height>791</height>
<width>280</width>
<height>875</height>
</rect>
</property>
<attribute name="label">
@@ -1988,7 +1988,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>296</width>
<width>226</width>
<height>160</height>
</rect>
</property>
guilib/src/ui/preferencesDialog.ui
+58 -38
View File
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>12</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -9587,10 +9587,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="11" column="0">
<item row="12" column="0">
<widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QCheckBox" name="loopClosure_reextract">
<property name="text">
<string/>
@@ -9616,7 +9616,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="12" column="1">
<item row="13" column="1">
<widget class="QLabel" name="label_scanMatching_5">
<property name="text">
<string>Ratio of working memory for which local nodes are immunized from transfer.</string>
@@ -9629,7 +9629,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_scanMatching_9">
<property name="text">
<string>Re-extract visual features when computing loop closure transformations.</string>
@@ -9662,7 +9662,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="13" column="0">
<item row="14" column="0">
<widget class="QDoubleSpinBox" name="localDetection_radius">
<property name="suffix">
<string> m</string>
@@ -9679,7 +9679,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="12" column="0">
<item row="13" column="0">
<widget class="QDoubleSpinBox" name="rgdb_localImmunizationRatio">
<property name="suffix">
<string/>
@@ -9698,6 +9698,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QCheckBox" name="memCovOffDiagIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_163">
<property name="text">
@@ -9724,14 +9731,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QCheckBox" name="memCovOffDiagIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_scanMatching_10">
<property name="text">
<string>Do local bundle adjustment with neighborhood of the loop closure.</string>
@@ -9744,7 +9744,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_scanMatching_11">
<property name="text">
<string>Ignore off diagonal values of the odometry covariance matrix.</string>
@@ -9757,6 +9757,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QCheckBox" name="loopClosure_bunlde">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
<property name="suffix">
@@ -9783,13 +9790,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="loopClosure_bunlde">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
<property name="suffix">
@@ -9819,7 +9819,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="13" column="1">
<item row="11" column="0">
<widget class="QCheckBox" name="rgbd_loopCovLimited">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="14" column="1">
<widget class="QLabel" name="label_space2">
<property name="text">
<string>Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels.</string>
@@ -9832,7 +9839,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="11" column="1">
<item row="12" column="1">
<widget class="QLabel" name="label_scanMatching_3">
<property name="text">
<string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
@@ -9845,13 +9852,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QCheckBox" name="rgbd_loopCovLimited">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
<property name="suffix">
@@ -9868,7 +9868,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_scanMatching_12">
<property name="text">
<string>Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.</string>
@@ -9881,7 +9881,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="14" column="1">
<item row="15" column="0">
<widget class="QSpinBox" name="spinBox_maxOdomCacheSize">
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLabel" name="label_space2_5">
<property name="text">
<string>Maximum odometry cache size. Used only in localization mode and when maximum graph error parameter is not null. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching.</string>
@@ -9894,10 +9901,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QSpinBox" name="spinBox_maxOdomCacheSize">
<property name="maximum">
<number>9999</number>
<item row="7" column="1">
<widget class="QLabel" name="label_scanMatching_7">
<property name="text">
<string>Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if &quot;Optimizer/GravitySigma&quot; is not zero.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="odomGravity">
<property name="text">
<string/>
</property>
</widget>
</item>
tools/Report/main.cpp
+4 -2
View File
@@ -317,11 +317,13 @@ int main(int argc, char * argv[])
std::multimap<int, Link> linksOut;
int firstId = *ids.begin();
rtabmap::Optimizer * optimizer = rtabmap::Optimizer::create(params);
if(optimizer->gravitySigma() > 0)
bool useOdomGravity = Parameters::defaultMemUseOdomGravity();
Parameters::parse(params, Parameters::kMemUseOdomGravity(), useOdomGravity);
if(useOdomGravity)
{
for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
{
links.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
links.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kGravity, iter->second)));
}
}
optimizer->getConnectedGraph(firstId, odomPoses, links, posesOut, linksOut);