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

0.13.0 Database: Added velocity field to Node table, renamed Map_Node_Word table to Feature, added information_matrix field to Link table (replacing rot_variance and trans_variance). Added graph::calcKittiSequenceErrors().Added "Mem/IntermediateNodeDataKept" parameter (default false). Updated all interfaces using rotVariance and transVariance values with covariance matrix instead. g2o SBA: always use covariance in links. kitti-dataset tool: added --disp option and kitti statistics are shown at the end. StatsToolBox: units can have "/".

Browse Source
This commit is contained in:
matlabbe
2017-05-11 15:02:18 -04:00
parent 414e3555a5
commit 104c1e6945
45 changed files with 1539 additions and 1148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/DBDriver.h
+2 -2
View File
@@ -131,7 +131,7 @@ public:
void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void getWeight(int signatureId, int & weight) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
@@ -197,7 +197,7 @@ private:
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;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
corelib/include/rtabmap/core/Graph.h
+18
View File
@@ -58,6 +58,20 @@ bool RTABMAP_EXP importPoses(
std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
std::map<int, double> * stamps = 0); // optional for format 1
/**
* Compute translation and rotation errors for KITTI datasets.
* See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
* @param poses_gt, Ground Truth poses
* @param poses_result, Estimated poses
* @param t_err, Output translation error (%)
* @param r_err, Output rotation error (deg/m)
*/
void RTABMAP_EXP calcKittiSequenceErrors(
const std::vector<Transform> &poses_gt,
const std::vector<Transform> &poses_result,
float & t_err,
float & r_err);
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
@@ -205,6 +219,10 @@ float RTABMAP_EXP computePathLength(
unsigned int fromIndex = 0,
unsigned int toIndex = 0);
// assuming they are all linked in map order
float RTABMAP_EXP computePathLength(
const std::map<int, Transform> & path);
std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
std::map<int, Transform> poses,
const std::multimap<int, Link> & links);
corelib/include/rtabmap/core/Link.h
+1 -9
View File
@@ -52,14 +52,7 @@ public:
int to,
Type type,
const Transform & transform,
const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
const cv::Mat & userData = cv::Mat());
Link(int from,
int to,
Type type,
const Transform & transform,
double rotVariance,
double transVariance,
const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
const cv::Mat & userData = cv::Mat());
bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -87,7 +80,6 @@ public:
private:
void setInfMatrix(const cv::Mat & infMatrix);
void setVariance(double rotVariance, double transVariance);
private:
int from_;
corelib/include/rtabmap/core/Memory.h
+3
View File
@@ -75,6 +75,7 @@ public:
bool update(const SensorData & data,
const Transform & pose,
const cv::Mat & covariance,
const std::vector<float> & velocity = std::vector<float>(), // vx,vy,vz,vroll,vpitch,vyaw
Statistics * stats = 0);
bool init(const std::string & dbUrl,
bool dbOverwritten = false,
@@ -153,6 +154,7 @@ public:
std::string & label,
double & stamp,
Transform & groundTruth,
std::vector<float> & velocity,
bool lookInDatabase = false) const;
cv::Mat getImageCompressed(int signatureId) const;
SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
@@ -244,6 +246,7 @@ private:
bool _rawDescriptorsKept;
bool _saveDepth16Format;
bool _notLinkedNodesKeptInDb;
bool _saveIntermediateNodeData;
bool _incrementalMemory;
bool _reduceGraph;
int _maxStMemSize;
corelib/include/rtabmap/core/OdometryEvent.h
+31 -41
View File
@@ -39,53 +39,29 @@ namespace rtabmap {
class OdometryEvent : public UEvent
{
public:
static cv::Mat generateCovarianceMatrix(float rotVariance, float transVariance)
{
UASSERT(uIsFinite(rotVariance) && rotVariance>0);
UASSERT(uIsFinite(transVariance) && transVariance>0);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance.at<double>(0,0) = transVariance;
covariance.at<double>(1,1) = transVariance;
covariance.at<double>(2,2) = transVariance;
covariance.at<double>(3,3) = rotVariance;
covariance.at<double>(4,4) = rotVariance;
covariance.at<double>(5,5) = rotVariance;
return covariance;
}
public:
OdometryEvent() :
_covariance(cv::Mat::eye(6,6,CV_64FC1))
OdometryEvent()
{
_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
OdometryEvent(
const SensorData & data,
const Transform & pose,
const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1),
const OdometryInfo & info = OdometryInfo()) :
_data(data),
_pose(pose),
_info(info)
{
UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(1,1)) && covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(covariance.at<double>(5,5)) && covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
_covariance = covariance;
}
OdometryEvent(
const SensorData & data,
const Transform & pose,
double rotVariance = 1.0,
double transVariance = 1.0,
const OdometryInfo & info = OdometryInfo()) :
_data(data),
_pose(pose),
_covariance(generateCovarianceMatrix(rotVariance, transVariance)),
_info(info)
{
if(_info.covariance.empty())
{
_info.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
UASSERT(_info.covariance.cols == 6 && _info.covariance.rows == 6 && _info.covariance.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(0,0)) && _info.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(1,1)) && _info.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(2,2)) && _info.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(3,3)) && _info.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(4,4)) && _info.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(5,5)) && _info.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
}
virtual ~OdometryEvent() {}
virtual std::string getClassName() const {return "OdometryEvent";}
@@ -93,15 +69,29 @@ public:
SensorData & data() {return _data;}
const SensorData & data() const {return _data;}
const Transform & pose() const {return _pose;}
const cv::Mat & covariance() const {return _covariance;}
const cv::Mat & covariance() const {return _info.covariance;}
std::vector<float> velocity() const {
if(_info.interval>0.0)
{
std::vector<float> velocity(6,0);
float x,y,z,roll,pitch,yaw;
_info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
velocity[0] = x/_info.interval;
velocity[1] = y/_info.interval;
velocity[2] = z/_info.interval;
velocity[3] = roll/_info.interval;
velocity[4] = pitch/_info.interval;
velocity[5] = yaw/_info.interval;
}
return std::vector<float>();
}
const OdometryInfo & info() const {return _info;}
double rotVariance() const {return uMax3(_covariance.at<double>(3,3), _covariance.at<double>(4,4), _covariance.at<double>(5,5));}
double transVariance() const {return uMax3(_covariance.at<double>(0,0), _covariance.at<double>(1,1), _covariance.at<double>(2,2));}
double rotVariance() const {return uMax3(_info.covariance.at<double>(3,3), _info.covariance.at<double>(4,4), _info.covariance.at<double>(5,5));}
double transVariance() const {return uMax3(_info.covariance.at<double>(0,0), _info.covariance.at<double>(1,1), _info.covariance.at<double>(2,2));}
private:
SensorData _data;
Transform _pose;
cv::Mat _covariance;
OdometryInfo _info;
};
corelib/include/rtabmap/core/OdometryInfo.h
+3 -6
View File
@@ -42,8 +42,6 @@ public:
matches(0),
inliers(0),
icpInliersRatio(0.0f),
varianceLin(0.0f),
varianceAng(0.0f),
features(0),
localMapSize(0),
localScanMapSize(0),
@@ -67,8 +65,7 @@ public:
output.matches = matches;
output.inliers = inliers;
output.icpInliersRatio = icpInliersRatio;
output.varianceLin = varianceLin;
output.varianceAng = varianceAng;
output.covariance = covariance.clone();
output.features = features;
output.localMapSize = localMapSize;
output.localScanMapSize = localScanMapSize;
@@ -80,6 +77,7 @@ public:
output.timeEstimation = timeEstimation;
output.timeParticleFiltering = timeParticleFiltering;
output.stamp = stamp;
output.interval = interval;
output.transform = transform;
output.transformFiltered = transformFiltered;
output.transformGroundTruth = transformGroundTruth;
@@ -92,8 +90,7 @@ public:
int matches;
int inliers;
float icpInliersRatio;
float varianceLin;
float varianceAng;
cv::Mat covariance;
int features;
int localMapSize;
int localScanMapSize;
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -197,6 +197,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, MapLabelsAdded, bool, true, "Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
RTABMAP_PARAM(Mem, SaveDepth16Format, bool, false, "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
RTABMAP_PARAM(Mem, IntermediateNodeDataKept, bool, false, "Keep intermediate node data in db.");
RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size.");
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode.");
RTABMAP_PARAM(Mem, ReduceGraph, bool, false, "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
corelib/include/rtabmap/core/RegistrationInfo.h
+1 -4
View File
@@ -35,8 +35,6 @@ class RegistrationInfo
{
public:
RegistrationInfo() :
varianceLin(0),
varianceAng(0),
inliers(0),
matches(0),
icpInliersRatio(0),
@@ -45,8 +43,7 @@ public:
{
}
float varianceLin;
float varianceAng;
cv::Mat covariance;
std::string rejectedMsg;
// RegistrationVis
corelib/include/rtabmap/core/Rtabmap.h
+2
View File
@@ -71,6 +71,7 @@ public:
const SensorData & data,
Transform odomPose,
const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
const std::vector<float> & odomVelocity = std::vector<float>(),
const std::map<std::string, float> & externalStats = std::map<std::string, float>());
// for convenience
bool process(
@@ -78,6 +79,7 @@ public:
Transform odomPose,
float odomLinearVariance,
float odomAngularVariance,
const std::vector<float> & odomVelocity = std::vector<float>(),
const std::map<std::string, float> & externalStats = std::map<std::string, float>());
// for convenience, loop closure detection only
bool process(
corelib/include/rtabmap/core/RtabmapThread.h
+1 -2
View File
@@ -125,8 +125,7 @@ private:
Rtabmap * _rtabmap;
bool _paused;
Transform lastPose_;
double _rotVariance;
double _transVariance;
cv::Mat covariance_;
cv::Mat _userData;
UMutex _userDataMutex;
corelib/include/rtabmap/core/Signature.h
+11
View File
@@ -115,11 +115,21 @@ public:
void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
void setPose(const Transform & pose) {_pose = pose;}
void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
_velocity = std::vector<float>(6,0);
_velocity[0]=vx;
_velocity[1]=vy;
_velocity[2]=vz;
_velocity[3]=vroll;
_velocity[4]=vpitch;
_velocity[5]=vyaw;
}
const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
const Transform & getPose() const {return _pose;}
cv::Mat getPoseCovariance() const;
const Transform & getGroundTruthPose() const {return _groundTruthPose;}
const std::vector<float> & getVelocity() const {return _velocity;}
SensorData & sensorData() {return _sensorData;}
const SensorData & sensorData() const {return _sensorData;}
@@ -147,6 +157,7 @@ private:
Transform _pose;
Transform _groundTruthPose;
std::vector<float> _velocity;
SensorData _sensorData;
};
corelib/include/rtabmap/core/util3d_motion_estimation.h
+2 -2
View File
@@ -50,7 +50,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
int pnpRefineIterations = 1,
const Transform & guess = Transform::getIdentity(),
const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
double * varianceOut = 0, // mean reproj error if words3B is not set
cv::Mat * covariance = 0, // mean reproj error if words3B is not set
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
@@ -61,7 +61,7 @@ Transform RTABMAP_EXP estimateMotion3DTo3D(
double inliersDistance = 0.1,
int iterations = 100,
int refineIterations = 5,
double * varianceOut = 0,
cv::Mat * covariance = 0,
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
corelib/include/rtabmap/core/util3d_registration.h
+1 -1
View File
@@ -57,7 +57,7 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
int refineModelIterations = 10,
double refineModelSigma = 3.0,
std::vector<int> * inliers = 0,
double * variance = 0);
cv::Mat * variance = 0);
void RTABMAP_EXP computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
corelib/src/DBDriver.cpp
+3 -2
View File
@@ -639,7 +639,8 @@ bool DBDriver::getNodeInfo(
int & weight,
std::string & label,
double & stamp,
Transform & groundTruthPose) const
Transform & groundTruthPose,
std::vector<float> & velocity) const
{
bool found = false;
// look in the trash
@@ -659,7 +660,7 @@ bool DBDriver::getNodeInfo(
if(!found)
{
_dbSafeAccessMutex.lock();
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose);
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity);
_dbSafeAccessMutex.unlock();
}
return found;
corelib/src/DBDriverSqlite3.cpp
+236 -67
View File
@@ -864,6 +864,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
std::stringstream fields;
if(images)
{
fields << "image, depth, calibration";
@@ -1442,7 +1443,8 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
int & weight,
std::string & label,
double & stamp,
Transform & groundTruthPose) const
Transform & groundTruthPose,
std::vector<float> & velocity) const
{
bool found = false;
if(_ppDb && signatureId)
@@ -1451,7 +1453,14 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.11.1") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else if(uStrNumCmp(_version, "0.11.1") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose "
"FROM Node "
@@ -1503,15 +1512,26 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
label = reinterpret_cast<const char*>(p); // label
}
stamp = sqlite3_column_double(ppStmt, index++); // stamp
}
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
data = sqlite3_column_blob(ppStmt, index); // ground_truh_pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
memcpy(groundTruthPose.data(), data, dataSize);
data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
{
memcpy(groundTruthPose.data(), data, dataSize);
}
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
velocity.resize(6,0);
data = sqlite3_column_blob(ppStmt, index); // velocity
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
{
memcpy(velocity.data(), data, dataSize);
}
}
}
}
@@ -1546,7 +1566,14 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
}
if(ignoreBadSignatures)
{
query << "WHERE id in (select node_id from Map_Node_Word) ";
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "WHERE id in (select node_id from Feature) ";
}
else
{
query << "WHERE id in (select node_id from Map_Node_Word) ";
}
}
query << "ORDER BY id";
@@ -1581,7 +1608,11 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.10.10") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT from_id, to_id, type, transform, information_matrix, user_data FROM Link ORDER BY from_id, to_id";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
query << "SELECT from_id, to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ORDER BY from_id, to_id";
}
@@ -1604,8 +1635,6 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
int fromId = -1;
int toId = -1;
int type = Link::kUndef;
float rotVariance = 1.0f;
float transVariance = 1.0f;
const void * data = 0;
int dataSize = 0;
@@ -1634,10 +1663,28 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
if(!ignoreNullLinks || !transform.isNull())
{
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
rotVariance = sqlite3_column_double(ppStmt, index++);
transVariance = sqlite3_column_double(ppStmt, index++);
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
UASSERT(dataSize==36*sizeof(double) && data);
informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
}
else
{
double rotVariance = sqlite3_column_double(ppStmt, index++);
double transVariance = sqlite3_column_double(ppStmt, index++);
UASSERT(rotVariance > 0.0 && transVariance>0.0);
informationMatrix.at<double>(0,0) = 1.0/transVariance;
informationMatrix.at<double>(1,1) = 1.0/transVariance;
informationMatrix.at<double>(2,2) = 1.0/transVariance;
informationMatrix.at<double>(3,3) = 1.0/rotVariance;
informationMatrix.at<double>(4,4) = 1.0/rotVariance;
informationMatrix.at<double>(5,5) = 1.0/rotVariance;
}
cv::Mat userDataCompressed;
if(uStrNumCmp(_version, "0.10.10") >= 0)
@@ -1651,17 +1698,19 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
}
}
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
double variance = sqlite3_column_double(ppStmt, index++);
UASSERT(variance>0.0);
informationMatrix *= 1.0/variance;
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, (Link::Type)type, transform, informationMatrix)));
}
else
{
// neighbor is 0, loop closures are 1 and 2 (child)
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
links.insert(links.end(), std::make_pair(fromId, Link(fromId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
}
}
@@ -1726,14 +1775,18 @@ void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
// Create a new entry in table Signature
query << "SELECT count(word_id) "
<< "FROM Map_Node_Word "
<< "WHERE node_id=" << nodeId << ";";
//query.append("COMMIT;");
//ULOGGER_DEBUG("DBDriverSqlite3::getSurfNi() Execute query : %s", query.toStdString().c_str());
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT count(word_id) "
<< "FROM Feature "
<< "WHERE node_id=" << nodeId << ";";
}
else
{
query << "SELECT count(word_id) "
<< "FROM Map_Node_Word "
<< "WHERE node_id=" << nodeId << ";";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -1879,7 +1932,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
unsigned int loaded = 0;
// Load nodes information
if(uStrNumCmp(_version, "0.11.1") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity "
<< "FROM Node "
<< "WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.11.1") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose "
<< "FROM Node "
@@ -1914,6 +1973,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int weight = 0;
Transform pose;
Transform groundTruthPose;
std::vector<float> velocity;
const void * data = 0;
int dataSize = 0;
std::string label;
@@ -1942,15 +2002,26 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
label = reinterpret_cast<const char*>(p);
}
}
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
memcpy(groundTruthPose.data(), data, dataSize);
data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
{
memcpy(groundTruthPose.data(), data, dataSize);
}
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
velocity.resize(6,0);
data = sqlite3_column_blob(ppStmt, index); // velocity
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == velocity.size()*sizeof(float) && data)
{
memcpy(velocity.data(), data, dataSize);
}
}
}
}
@@ -1970,6 +2041,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
label,
pose,
groundTruthPose);
if(velocity.size() == 6)
{
s->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
s->setSaved(true);
nodes.push_back(s);
++loaded;
@@ -1992,7 +2067,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
// Prepare the query... Get the map from signature and visual words
std::stringstream query2;
if(uStrNumCmp(_version, "0.12.0") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Feature "
"WHERE node_id = ? ";
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
"FROM Map_Node_Word "
@@ -2485,7 +2566,11 @@ void DBDriverSqlite3::loadLinksQuery(
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.10.10") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT to_id, type, transform, information_matrix, user_data FROM Link ";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
}
@@ -2524,8 +2609,6 @@ void DBDriverSqlite3::loadLinksQuery(
int toId = -1;
int type = Link::kUndef;
float rotVariance = 1.0f;
float transVariance = 1.0f;
const void * data = 0;
int dataSize = 0;
@@ -2551,10 +2634,28 @@ void DBDriverSqlite3::loadLinksQuery(
UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
}
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
rotVariance = sqlite3_column_double(ppStmt, index++);
transVariance = sqlite3_column_double(ppStmt, index++);
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
UASSERT(dataSize==36*sizeof(double) && data);
informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
}
else
{
double rotVariance = sqlite3_column_double(ppStmt, index++);
double transVariance = sqlite3_column_double(ppStmt, index++);
UASSERT(rotVariance > 0.0 && transVariance>0.0);
informationMatrix.at<double>(0,0) = 1.0/transVariance;
informationMatrix.at<double>(1,1) = 1.0/transVariance;
informationMatrix.at<double>(2,2) = 1.0/transVariance;
informationMatrix.at<double>(3,3) = 1.0/rotVariance;
informationMatrix.at<double>(4,4) = 1.0/rotVariance;
informationMatrix.at<double>(5,5) = 1.0/rotVariance;
}
cv::Mat userDataCompressed;
if(uStrNumCmp(_version, "0.10.10") >= 0)
@@ -2568,17 +2669,19 @@ void DBDriverSqlite3::loadLinksQuery(
}
}
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix, userDataCompressed)));
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
double variance = sqlite3_column_double(ppStmt, index++);
UASSERT(variance>0.0);
informationMatrix *= 1.0/variance;
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, informationMatrix)));
}
else
{
// neighbor is 0, loop closures are 1 and 2 (child)
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix)));
}
rc = sqlite3_step(ppStmt);
@@ -2608,7 +2711,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
std::stringstream query;
int totalLinksLoaded = 0;
if(uStrNumCmp(_version, "0.10.10") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT to_id, type, information_matrix, user_data, transform FROM Link "
<< "WHERE from_id = ? "
<< "ORDER BY to_id";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
<< "WHERE from_id = ? "
@@ -2644,8 +2753,6 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
int toId = -1;
int linkType = -1;
float rotVariance = 1.0f;
float transVariance = 1.0f;
std::list<Link> links;
const void * data = 0;
int dataSize = 0;
@@ -2659,10 +2766,28 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
toId = sqlite3_column_int(ppStmt, index++);
linkType = sqlite3_column_int(ppStmt, index++);
cv::Mat userDataCompressed;
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(uStrNumCmp(_version, "0.8.4") >= 0)
{
rotVariance = sqlite3_column_double(ppStmt, index++);
transVariance = sqlite3_column_double(ppStmt, index++);
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
UASSERT(dataSize==36*sizeof(double) && data);
informationMatrix = cv::Mat(6, 6, CV_64FC1, (void *)data).clone(); // information_matrix
}
else
{
double rotVariance = sqlite3_column_double(ppStmt, index++);
double transVariance = sqlite3_column_double(ppStmt, index++);
UASSERT(rotVariance > 0.0 && transVariance>0.0);
informationMatrix.at<double>(0,0) = 1.0/transVariance;
informationMatrix.at<double>(1,1) = 1.0/transVariance;
informationMatrix.at<double>(2,2) = 1.0/transVariance;
informationMatrix.at<double>(3,3) = 1.0/rotVariance;
informationMatrix.at<double>(4,4) = 1.0/rotVariance;
informationMatrix.at<double>(5,5) = 1.0/rotVariance;
}
if(uStrNumCmp(_version, "0.10.10") >= 0)
{
@@ -2677,7 +2802,9 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
}
else if(uStrNumCmp(_version, "0.7.4") >= 0)
{
rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
double variance = sqlite3_column_double(ppStmt, index++);
UASSERT(variance>0.0);
informationMatrix *= 1.0/variance;
}
//transform
@@ -2697,11 +2824,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
{
if(uStrNumCmp(_version, "0.7.4") >= 0)
{
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance, userDataCompressed));
links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, informationMatrix, userDataCompressed));
}
else // neighbor is 0, loop closures are 1 and 2 (child)
{
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance, userDataCompressed));
links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, informationMatrix, userDataCompressed));
}
}
else
@@ -2960,7 +3087,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
UDEBUG("Time=%fs", timer.ticks());
// Create new entries in table Map_Word_Node
// Create new entries in table Feature
query = queryStepKeypoint();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -3296,7 +3423,11 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
std::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.11.1") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity) VALUES(?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.11.1") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose) VALUES(?,?,?,?,?,?,?);";
}
@@ -3353,6 +3484,26 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
// ignore user_data
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
if(s->getVelocity().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, s->getVelocity().data(), s->getVelocity().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
}
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
@@ -3367,13 +3518,6 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
rc = sqlite3_bind_blob(ppStmt, index++, s->getGroundTruthPose().data(), s->getGroundTruthPose().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//step
rc=sqlite3_step(ppStmt);
@@ -3822,7 +3966,11 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepLinkUpdate() const
{
if(uStrNumCmp(_version, "0.10.10") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "UPDATE Link SET type=?, information_matrix=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;";
}
@@ -3842,7 +3990,11 @@ std::string DBDriverSqlite3::queryStepLinkUpdate() const
std::string DBDriverSqlite3::queryStepLink() const
{
// from_id, to_id are at the end to match the update query above
if(uStrNumCmp(_version, "0.10.10") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "INSERT INTO Link(type, information_matrix, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.10.10") >= 0)
{
return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);";
}
@@ -3881,7 +4033,13 @@ void DBDriverSqlite3::stepLink(
rc = sqlite3_bind_int(ppStmt, index++, link.type());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.4") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
// information_matrix
rc = sqlite3_bind_blob(ppStmt, index++, link.infMatrix().data, (int)link.infMatrix().total()*sizeof(double), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else if(uStrNumCmp(_version, "0.8.4") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -3925,7 +4083,14 @@ void DBDriverSqlite3::stepLink(
std::string DBDriverSqlite3::queryStepWordsChanged() const
{
return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "UPDATE Feature SET word_id = ? WHERE word_id = ? AND node_id = ?;";
}
else
{
return "UPDATE Map_Node_Word SET word_id = ? WHERE word_id = ? AND node_id = ?;";
}
}
void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int oldWordId, int newWordId) const
{
@@ -3951,7 +4116,11 @@ void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int ol
std::string DBDriverSqlite3::queryStepKeypoint() const
{
if(uStrNumCmp(_version, "0.12.0") >= 0)
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "INSERT INTO Feature(node_id, word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.12.0") >= 0)
{
return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
corelib/src/DBDriverSqlite3.h
+1 -1
View File
@@ -99,7 +99,7 @@ private:
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;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
corelib/src/DBReader.cpp
+4 -2
View File
@@ -267,7 +267,8 @@ SensorData DBReader::captureImage(CameraInfo * info)
double stamp;
int mapId;
Transform localTransform, pose, groundTruth;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
std::vector<float> velocity;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
if(previousStamp && stamp && stamp > previousStamp)
{
delay = stamp - previousStamp;
@@ -320,7 +321,8 @@ SensorData DBReader::getNextData(CameraInfo * info)
std::string label;
double stamp;
Transform groundTruth;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
std::vector<float> velocity;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored)
corelib/src/Graph.cpp
+141
View File
@@ -425,6 +425,124 @@ bool importPoses(
return false;
}
// KITTI evaluation
float lengths[] = {100,200,300,400,500,600,700,800};
int32_t num_lengths = 8;
struct errors {
int32_t first_frame;
float r_err;
float t_err;
float len;
float speed;
errors (int32_t first_frame,float r_err,float t_err,float len,float speed) :
first_frame(first_frame),r_err(r_err),t_err(t_err),len(len),speed(speed) {}
};
std::vector<float> trajectoryDistances (const std::vector<Transform> &poses) {
std::vector<float> dist;
dist.push_back(0);
for (int32_t i=1; i<poses.size(); i++) {
Transform P1 = poses[i-1];
Transform P2 = poses[i];
float dx = P1.x()-P2.x();
float dy = P1.y()-P2.y();
float dz = P1.z()-P2.z();
dist.push_back(dist[i-1]+sqrt(dx*dx+dy*dy+dz*dz));
}
return dist;
}
int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,float len) {
for (int32_t i=first_frame; i<dist.size(); i++)
if (dist[i]>dist[first_frame]+len)
return i;
return -1;
}
inline float rotationError(const Transform &pose_error) {
float a = pose_error(0,0);
float b = pose_error(1,1);
float c = pose_error(2,2);
float d = 0.5*(a+b+c-1.0);
return std::acos(std::max(std::min(d,1.0f),-1.0f));
}
inline float translationError(const Transform &pose_error) {
float dx = pose_error.x();
float dy = pose_error.y();
float dz = pose_error.z();
return sqrt(dx*dx+dy*dy+dz*dz);
}
void calcKittiSequenceErrors (
const std::vector<Transform> &poses_gt,
const std::vector<Transform> &poses_result,
float & t_err,
float & r_err) {
UASSERT(poses_gt.size() == poses_result.size());
// error vector
std::vector<errors> err;
// parameters
int32_t step_size = 10; // every second
// pre-compute distances (from ground truth as reference)
std::vector<float> dist = trajectoryDistances(poses_gt);
// for all start positions do
for (int32_t first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) {
// for all segment lengths do
for (int32_t i=0; i<num_lengths; i++) {
// current length
float len = lengths[i];
// compute last frame
int32_t last_frame = lastFrameFromSegmentLength(dist,first_frame,len);
// continue, if sequence not long enough
if (last_frame==-1)
continue;
// compute rotational and translational errors
Transform pose_delta_gt = poses_gt[first_frame].inverse()*poses_gt[last_frame];
Transform pose_delta_result = poses_result[first_frame].inverse()*poses_result[last_frame];
Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
float r_err = rotationError(pose_error);
float t_err = translationError(pose_error);
// compute speed
float num_frames = (float)(last_frame-first_frame+1);
float speed = len/(0.1*num_frames);
// write to file
err.push_back(errors(first_frame,r_err/len,t_err/len,len,speed));
}
}
t_err = 0;
r_err = 0;
// for all errors do => compute sum of t_err, r_err
for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
{
t_err += it->t_err;
r_err += it->r_err;
}
// save errors
float num = err.size();
t_err /= num;
r_err /= num;
t_err *= 100.0f; // Translation error (%)
r_err *= 180/CV_PI; // Rotation error (deg/m)
}
// KITTI evaluation end
////////////////////////////////////////////
// Graph utilities
@@ -1636,6 +1754,29 @@ float computePathLength(
return length;
}
float computePathLength(
const std::map<int, Transform> & path)
{
float length = 0.0f;
if(path.size() > 1)
{
float x=0, y=0, z=0;
std::map<int, Transform>::const_iterator iter=path.begin();
Transform previousPose = iter->second;
++iter;
for(; iter!=path.end(); ++iter)
{
const Transform & currentPose = iter->second;
x += fabs(previousPose.x() - currentPose.x());
y += fabs(previousPose.y() - currentPose.y());
z += fabs(previousPose.z() - currentPose.z());
previousPose = currentPose;
}
length = sqrt(x*x + y*y + z*z);
}
return length;
}
// return all paths linked only by neighbor links
std::list<std::map<int, Transform> > getPaths(
std::map<int, Transform> poses,
corelib/src/Link.cpp
+7 -40
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Link.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
namespace rtabmap {
@@ -61,29 +62,6 @@ Link::Link(int from,
_userDataRaw = userData;
}
}
Link::Link(int from,
int to,
Type type,
const Transform & transform,
double rotVariance,
double transVariance,
const cv::Mat & userData) :
from_(from),
to_(to),
transform_(transform),
type_(type)
{
setVariance(rotVariance, transVariance);
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
double Link::rotVariance() const
{
@@ -100,25 +78,14 @@ double Link::transVariance() const
void Link::setInfMatrix(const cv::Mat & infMatrix) {
UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(0,0)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(1,1)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, uFormat("Linear information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(2,2)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(3,3)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(4,4)).c_str());
UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, uFormat("Angular information should not be null! Value=%f (set to 1 if unknown).", infMatrix.at<double>(5,5)).c_str());
infMatrix_ = infMatrix;
}
void Link::setVariance(double rotVariance, double transVariance) {
UASSERT(uIsFinite(rotVariance) && rotVariance>0);
UASSERT(uIsFinite(transVariance) && transVariance>0);
infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
infMatrix_.at<double>(0,0) = 1.0/transVariance;
infMatrix_.at<double>(1,1) = 1.0/transVariance;
infMatrix_.at<double>(2,2) = 1.0/transVariance;
infMatrix_.at<double>(3,3) = 1.0/rotVariance;
infMatrix_.at<double>(4,4) = 1.0/rotVariance;
infMatrix_.at<double>(5,5) = 1.0/rotVariance;
}
void Link::uncompressUserData()
{
corelib/src/Memory.cpp
+28 -15
View File
@@ -74,6 +74,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
_saveIntermediateNodeData(Parameters::defaultMemIntermediateNodeDataKept()),
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
_reduceGraph(Parameters::defaultMemReduceGraph()),
_maxStMemSize(Parameters::defaultMemSTMSize()),
@@ -425,6 +426,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
Parameters::parse(parameters, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -580,13 +582,14 @@ bool Memory::update(
const SensorData & data,
Statistics * stats)
{
return update(data, Transform(), cv::Mat(), stats);
return update(data, Transform(), cv::Mat(), std::vector<float>(), stats);
}
bool Memory::update(
const SensorData & data,
const Transform & pose,
const cv::Mat & covariance,
const std::vector<float> & velocity,
Statistics * stats)
{
UDEBUG("");
@@ -613,6 +616,10 @@ bool Memory::update(
UERROR("Failed to create a signature...");
return false;
}
if(velocity.size()==6)
{
signature->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
t=timer.ticks()*1000;
if(stats) stats->addStatistic(Statistics::kTimingMemSignature_creation(), t);
@@ -2265,10 +2272,11 @@ Transform Memory::computeTransform(
else if(info && !transform.isIdentity())
{
//normalize variance
info->varianceLin *= transform.getNorm();
info->varianceAng *= transform.getAngle();
info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
info->covariance *= transform.getNorm();
if(info->covariance.at<double>(0,0) < 0.0001)
{
info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
}
}
}
@@ -2321,10 +2329,11 @@ Transform Memory::computeIcpTransform(
if(!t.isNull() && !t.isIdentity() && info)
{
// normalize variance
info->varianceLin *= t.getNorm();
info->varianceAng *= t.getAngle();
info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
info->covariance *= t.getNorm();
if(info->covariance.at<double>(0,0)<=0.0)
{
info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
}
}
else
@@ -2875,7 +2884,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
newS->setLabel(oldS->getLabel());
oldS->setLabel("");
oldS->removeLinks(); // remove all links
oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location
oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location
// Set old image to new signature
this->copyData(oldS, newS);
@@ -2890,7 +2899,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
}
else
{
newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location
newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , cv::Mat::eye(6,6,CV_64FC1))); // to keep track of the merged location
// update weight
oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
@@ -2949,7 +2958,8 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
int mapId, weight;
std::string label;
double stamp;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
std::vector<float> velocity;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, lookInDatabase);
return pose;
}
@@ -2959,7 +2969,8 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
int mapId, weight;
std::string label;
double stamp;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, lookInDatabase);
std::vector<float> velocity;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, lookInDatabase);
return groundTruth;
}
@@ -2970,6 +2981,7 @@ bool Memory::getNodeInfo(int signatureId,
std::string & label,
double & stamp,
Transform & groundTruth,
std::vector<float> & velocity,
bool lookInDatabase) const
{
const Signature * s = this->getSignature(signatureId);
@@ -2981,11 +2993,12 @@ bool Memory::getNodeInfo(int signatureId,
label = s->getLabel();
stamp = s->getStamp();
groundTruth = s->getGroundTruthPose();
velocity = s->getVelocity();
return true;
}
else if(lookInDatabase && _dbDriver)
{
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth);
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity);
}
return false;
}
@@ -3669,7 +3682,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
Signature * s;
if(this->isBinDataKept() && !isIntermediateNode)
if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
{
UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
image.empty()?0:1,
corelib/src/Odometry.cpp
+5 -4
View File
@@ -529,10 +529,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
info->distanceTravelled = distanceTravelled_;
}
info->varianceLin *= t.getNorm();
info->varianceAng *= t.getAngle();
info->varianceLin = info->varianceLin>0.0f?info->varianceLin:0.0001f; // epsilon if exact transform
info->varianceAng = info->varianceAng>0.0f?info->varianceAng:0.0001f; // epsilon if exact transform
info->covariance *= t.getNorm();
if(info->covariance.at<double>(0,0)<=0.0)
{
info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
return _pose *= t; // update
}
corelib/src/OdometryF2F.cpp
+2 -4
View File
@@ -168,8 +168,7 @@ Transform OdometryF2F::computeTransform(
//return Identity
output = Transform::getIdentity();
// a very high variance tells that the new pose is not linked with the previous one
regInfo.varianceLin = 9999;
regInfo.varianceAng = 9999;
regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
}
if(!output.isNull())
@@ -245,8 +244,7 @@ Transform OdometryF2F::computeTransform(
if(info)
{
info->type = 1;
info->varianceLin = regInfo.varianceLin;
info->varianceAng = regInfo.varianceAng;
info->covariance = regInfo.covariance;
info->inliers = regInfo.inliers;
info->icpInliersRatio = regInfo.icpInliersRatio;
info->matches = regInfo.matches;
corelib/src/OdometryF2M.cpp
+6 -7
View File
@@ -250,7 +250,8 @@ Transform OdometryF2M::computeTransform(
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.varianceAng, regInfo.varianceLin)));
regInfo.covariance *= transform.getNorm();
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
@@ -735,8 +736,7 @@ Transform OdometryF2M::computeTransform(
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
// a very high variance tells that the new pose is not linked with the previous one
regInfo.varianceLin = 9999;
regInfo.varianceAng = 9999;
regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
bool frameValid = false;
Transform newFramePose = this->getPose(); // initial pose may be not identity...
@@ -874,8 +874,7 @@ Transform OdometryF2M::computeTransform(
if(info)
{
info->varianceLin = regInfo.varianceLin;
info->varianceAng = regInfo.varianceAng;
info->covariance = regInfo.covariance;
info->inliers = regInfo.inliers;
info->matches = regInfo.matches;
info->icpInliersRatio = regInfo.icpInliersRatio;
@@ -899,8 +898,8 @@ Transform OdometryF2M::computeTransform(
nFeatures,
regInfo.inliers,
regInfo.matches,
regInfo.varianceLin,
regInfo.varianceAng,
regInfo.covariance.at<double>(0,0),
regInfo.covariance.at<double>(5,5),
regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().cols:0);
corelib/src/OdometryMono.cpp
+1 -2
View File
@@ -976,8 +976,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
if(info)
{
// a very high variance tells that the new pose is not linked with the previous one
info->varianceLin = 9999;
info->varianceAng = 9999;
info->covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
}
// generate kpts
corelib/src/OdometryThread.cpp
+1 -3
View File
@@ -102,10 +102,8 @@ void OdometryThread::mainLoop()
OdometryInfo info;
Transform pose = _odometry->process(data, &info);
// a null pose notify that odometry could not be computed
double varianceLin = info.varianceLin>0?info.varianceLin:1;
double varianceAng = info.varianceAng>0?info.varianceAng:1;
UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
this->post(new OdometryEvent(data, pose, varianceAng, varianceLin, info));
this->post(new OdometryEvent(data, pose, info));
}
}
corelib/src/OptimizerG2O.cpp
+2 -5
View File
@@ -665,10 +665,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UASSERT(!iter->second.transform().isNull());
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// between cameras, not base_link
Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
@@ -777,7 +774,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UASSERT(optimizer.verifyInformationMatrices());
UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%f)", iterations(), this->epsilon(), robustKernelDelta_);
UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
int it = 0;
UTimer timer;
corelib/src/OptimizerTORO.cpp
+10 -12
View File
@@ -446,22 +446,20 @@ bool OptimizerTORO::loadGraph(
float roll = uStr2Float(strList[6]);
float pitch = uStr2Float(strList[7]);
float yaw = uStr2Float(strList[8]);
float infR = uStr2Float(strList[9]);
float infP = uStr2Float(strList[15]);
float infW = uStr2Float(strList[20]);
UASSERT_MSG(infR > 0 && infP > 0 && infW > 0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
float rotVariance = infR<=infP && infR<=infW?infR:infP<=infW?infP:infW; // maximum variance
float infX = uStr2Float(strList[24]);
float infY = uStr2Float(strList[27]);
float infZ = uStr2Float(strList[29]);
UASSERT_MSG(infX > 0 && infY > 0 && infZ > 0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
float transVariance = 1.0f/(infX<=infY && infX<=infZ?infX:infY<=infW?infY:infZ); // maximum variance
UINFO("id=%d rotV=%f transV=%f", idFrom, rotVariance, transVariance);
cv::Mat informationMatrix(6,6,CV_64FC1);
informationMatrix.at<double>(3,3) = uStr2Float(strList[9]);
informationMatrix.at<double>(4,4) = uStr2Float(strList[15]);
informationMatrix.at<double>(5,5) = uStr2Float(strList[20]);
UASSERT_MSG(informationMatrix.at<double>(3,3) > 0.0 && informationMatrix.at<double>(4,4) > 0.0 && informationMatrix.at<double>(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
informationMatrix.at<double>(0,0) = uStr2Float(strList[24]);
informationMatrix.at<double>(1,1) = uStr2Float(strList[27]);
informationMatrix.at<double>(2,2) = uStr2Float(strList[29]);
UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(2,2) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
Transform transform(x, y, z, roll, pitch, yaw);
if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
{
//Link type is unknown
Link link(idFrom, idTo, Link::kUndef, transform, rotVariance, transVariance);
Link link(idFrom, idTo, Link::kUndef, transform, informationMatrix);
edgeConstraints.insert(std::pair<int, Link>(idFrom, link));
}
else
corelib/src/Parameters.cpp
+7 -4
View File
@@ -740,16 +740,19 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
addParameter = oldIter->second.first;
if(addParameter)
{
key = oldIter->second.second;
UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second, Parameters::getDefaultParameters().at(oldIter->second.second).c_str());
if(parameters.find(oldIter->second.second) == parameters.end())
{
key = oldIter->second.second;
UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s, default=%s).",
oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second, Parameters::getDefaultParameters().at(oldIter->second.second).c_str());
}
}
else if(oldIter->second.second.empty())
{
UWARN("Parameter \"%s\" doesn't exist anymore.",
oldIter->first.c_str());
}
else
else if(parameters.find(oldIter->second.second) == parameters.end())
{
UWARN("Parameter \"%s\" (value=%s) doesn't exist anymore, you may want to use this similar parameter \"%s (default=%s): %s\".",
oldIter->first.c_str(), iter->second, oldIter->second.second.c_str(), Parameters::getDefaultParameters().at(oldIter->second.second).c_str(), Parameters::getDescription(oldIter->second.second).c_str());
corelib/src/Registration.cpp
+6 -6
View File
@@ -197,16 +197,16 @@ Transform Registration::computeTransformationMod(
{
if(info.icpInliersRatio)
{
info.varianceLin = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
info.varianceAng = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0);
}
else
{
info.varianceLin = info.inliers > 0?1.0f/float(info.inliers):1.0f;
info.varianceAng = info.inliers > 0?1.0f/float(info.inliers):1.0f;
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.inliers > 0?1.0/double(info.inliers):1.0);
}
if(info.covariance.at<double>(0,0)<0.0001)
{
info.covariance =cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
info.varianceLin = info.varianceLin>0.0f?info.varianceLin:0.0001f; // epsilon if exact transform
info.varianceAng = info.varianceAng>0.0f?info.varianceAng:0.0001f; // epsilon if exact transform
}
if(child_)
corelib/src/RegistrationIcp.cpp
+1 -2
View File
@@ -360,8 +360,7 @@ Transform RegistrationIcp::computeTransformationImpl(
info.icpTranslation,
info.icpRotation);
info.varianceLin = variance>0.0f?variance:0.0001; // epsilon if exact transform
info.varianceAng = variance>0.0f?variance:0.0001; // epsilon if exact transform
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(variance>0.0001?variance:0.0001); // epsilon if exact transform
info.icpInliersRatio = correspondencesRatio;
if(correspondencesRatio < _correspondenceRatio)
corelib/src/RegistrationVis.cpp
+17 -15
View File
@@ -1045,7 +1045,7 @@ Transform RegistrationVis::computeTransformationImpl(
// Motion estimation
/////////////////////
Transform transform;
float variance = 1.0f;
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
int inliersCount = 0;
int matchesCount = 0;
if(toSignature.getWords().size())
@@ -1053,7 +1053,9 @@ Transform RegistrationVis::computeTransformationImpl(
Transform transforms[2];
std::vector<int> inliers[2];
std::vector<int> matches[2];
double variances[2] = {1.0f};
cv::Mat covariances[2];
covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
{
// A to B
@@ -1086,6 +1088,7 @@ Transform RegistrationVis::computeTransformationImpl(
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
double variance = 1.0f;
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
uMultimapToMapUnique(signatureA->getWords()),
uMultimapToMapUnique(signatureB->getWords()),
@@ -1098,21 +1101,21 @@ Transform RegistrationVis::computeTransformationImpl(
1.0f,
0.99f,
uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
&variances[dir]);
&variance);
covariances[dir] *= variance;
inliers[dir] = uKeys(inliers3D);
if(!cameraTransform.isNull())
{
if((int)inliers3D.size() >= _minInliers)
{
if(variances[dir] <= _epipolarGeometryVar)
if(variance <= _epipolarGeometryVar)
{
transforms[dir] = cameraTransform;
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variances[dir]);
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
UINFO(msg.c_str());
}
}
@@ -1175,7 +1178,7 @@ Transform RegistrationVis::computeTransformationImpl(
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureB->getWords3()),
varianceFromInliersCount()?0:&variances[dir],
varianceFromInliersCount()?0:&covariances[dir],
&matchesV,
&inliersV);
inliers[dir] = inliersV;
@@ -1212,7 +1215,7 @@ Transform RegistrationVis::computeTransformationImpl(
_inlierDistance,
_iterations,
_refineIterations,
&variances[dir],
&covariances[dir],
&matchesV,
&inliersV);
inliers[dir] = inliersV;
@@ -1288,10 +1291,10 @@ Transform RegistrationVis::computeTransformationImpl(
poses.insert(std::make_pair(1, Transform::getIdentity()));
poses.insert(std::make_pair(2, transforms[0]));
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], variances[0], variances[0])));
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], (covariances[0]*transforms[0].getNorm()).inv())));
if(!transforms[1].isNull() && inliers[1].size())
{
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], variances[1], variances[1])));
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], (covariances[1]*transforms[1].getNorm()).inv())));
}
std::map<int, Transform> optimizedPoses;
@@ -1422,18 +1425,18 @@ Transform RegistrationVis::computeTransformationImpl(
if(transforms[0].isNull())
{
transform = transforms[1];
variance = variances[1];
covariance = covariances[1];
}
else
{
transform = transforms[0].interpolate(0.5f, transforms[1]);
variance = (variances[0]+variances[1])/2.0f;
covariance = (covariances[0]+covariances[1])/2.0f;
}
}
else
{
transform = transforms[0];
variance = variances[0];
covariance = covariances[0];
}
}
else if(toSignature.sensorData().isValid())
@@ -1447,8 +1450,7 @@ Transform RegistrationVis::computeTransformationImpl(
info.inliers = inliersCount;
info.matches = matchesCount;
info.rejectedMsg = msg;
info.varianceLin = variance>0.0f?variance:0.0001f; // epsilon if exact transform
info.varianceAng = info.varianceLin;
info.covariance = covariance.at<double>(0,0)>0.0001?covariance:cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;
corelib/src/Rtabmap.cpp
+49 -30
View File
@@ -768,7 +768,8 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
int m, w;
std::string l;
double stamp = 0.0;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, true);
std::vector<float> v;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, true);
stamps.insert(std::make_pair(iter->first, stamp));
}
}
@@ -827,6 +828,7 @@ bool Rtabmap::process(
Transform odomPose,
float odomLinearVariance,
float odomAngularVariance,
const std::vector<float> & odomVelocity,
const std::map<std::string, float> & externalStats)
{
if(!odomPose.isNull())
@@ -841,12 +843,13 @@ bool Rtabmap::process(
covariance.at<double>(3,3) = odomAngularVariance;
covariance.at<double>(4,4) = odomAngularVariance;
covariance.at<double>(5,5) = odomAngularVariance;
return process(data, odomPose, covariance, externalStats);
return process(data, odomPose, covariance, odomVelocity, externalStats);
}
bool Rtabmap::process(
const SensorData & data,
Transform odomPose,
const cv::Mat & odomCovariance,
const std::vector<float> & odomVelocity,
const std::map<std::string, float> & externalStats)
{
UDEBUG("");
@@ -985,14 +988,14 @@ bool Rtabmap::process(
ULOGGER_INFO("Updating memory...");
if(_rgbdSlamMode)
{
if(!_memory->update(data, odomPose, odomCovariance, &statistics_))
if(!_memory->update(data, odomPose, odomCovariance, odomVelocity, &statistics_))
{
return false;
}
}
else
{
if(!_memory->update(data, Transform(), cv::Mat(), &statistics_))
if(!_memory->update(data, Transform(), cv::Mat(), std::vector<float>(), &statistics_))
{
return false;
}
@@ -1076,7 +1079,7 @@ bool Rtabmap::process(
{
// set small variance
UDEBUG("Set small variance. The robot is not moving.");
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, cv::Mat::eye(6,6,CV_64FC1)*1000));
}
}
else
@@ -1094,12 +1097,12 @@ bool Rtabmap::process(
UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
oldId,
signature->id(),
info.varianceLin,
info.varianceAng,
info.covariance.at<double>(0,0),
info.covariance.at<double>(5,5),
guess.prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.varianceAng, info.varianceLin));
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));
if(_optimizeFromGraphEnd)
{
@@ -1120,9 +1123,9 @@ bool Rtabmap::process(
else
{
UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
if(info.varianceLin > 0 && info.varianceAng > 0)
if(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0)
{
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrt(info.varianceAng), sqrt(info.varianceLin)));
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
@@ -1265,8 +1268,8 @@ bool Rtabmap::process(
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.varianceAng, info.varianceLin)))
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.covariance.inv())))
{
++proximityDetectionsInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1822,7 +1825,7 @@ bool Rtabmap::process(
//Compute transform if metric data are present
Transform transform;
RegistrationInfo info;
info.varianceLin = info.varianceAng = 1.0f;
info.covariance = cv::Mat::eye(6,6,CV_64FC1);
if(_rgbdSlamMode)
{
transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
@@ -1842,8 +1845,8 @@ bool Rtabmap::process(
if(!rejectedHypothesis)
{
// Make the new one the parent of the old one
UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.varianceAng, info.varianceLin));
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.covariance.inv()));
if(!rejectedHypothesis)
{
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -1948,8 +1951,8 @@ bool Rtabmap::process(
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.varianceAng, info.varianceLin));
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.covariance.inv()));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(loopClosureVisualInliers == 0)
@@ -2077,8 +2080,8 @@ bool Rtabmap::process(
}
// set Identify covariance for laser scan matching only
UASSERT(info.varianceLin>0.0 && info.varianceAng>0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrt(info.varianceAng), sqrt(info.varianceLin), scanMatchingIds));
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, (info.covariance*100.0).inv(), scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++proximityDetectionsAddedByICPOnly;
@@ -2130,7 +2133,7 @@ bool Rtabmap::process(
if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius)
{
_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // set high variance
_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // set high variance
}
else
{
@@ -2649,8 +2652,8 @@ bool Rtabmap::process(
std::string label;
double stamp = 0;
Transform groundTruth;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, false);
std::vector<float> velocity;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, false);
signatures.insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
@@ -2659,6 +2662,10 @@ bool Rtabmap::process(
label,
odomPoseLocal,
groundTruth)));
if(!velocity.empty())
{
signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
}
localGraphSize = (int)poses.size();
if(!lastSignatureLocalizedPose.isNull())
@@ -3327,7 +3334,8 @@ void Rtabmap::get3DMap(
std::string label;
double stamp = 0;
Transform groundTruth;
_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, true);
std::vector<float> velocity;
_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, true);
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
std::multimap<int, cv::KeyPoint> words;
@@ -3346,6 +3354,10 @@ void Rtabmap::get3DMap(
signatures.at(*iter).setWords(words);
signatures.at(*iter).setWords3(words3);
signatures.at(*iter).setWordsDescriptors(wordsDescriptors);
if(!velocity.empty())
{
signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -3397,7 +3409,8 @@ void Rtabmap::getGraph(
std::string label;
double stamp = 0;
Transform groundTruth;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, global);
std::vector<float> velocity;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, global);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
@@ -3420,6 +3433,11 @@ void Rtabmap::getGraph(
_memory->getNodeCalibration(iter->first, models, stereoModel);
signatures->at(iter->first).sensorData().setCameraModels(models);
signatures->at(iter->first).sensorData().setStereoCameraModel(stereoModel);
if(!velocity.empty())
{
signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
}
}
}
@@ -3534,7 +3552,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
}
}
std::multimap<int, Link> linksIn = links;
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin)));
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.covariance.inv())));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
@@ -3627,8 +3645,9 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin)));
loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin));
cv::Mat inf = info.covariance.inv();
links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
}
}
@@ -3712,7 +3731,7 @@ int Rtabmap::refineLinks()
if(!t.isNull())
{
linksRefined.push_back(Link(from, to, iter->second.type(), t, info.varianceAng, info.varianceLin));
linksRefined.push_back(Link(from, to, iter->second.type(), t, info.covariance.inv()));
UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size());
}
}
@@ -4104,7 +4123,7 @@ void Rtabmap::updateGoalIndex()
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // on the optimized path
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}
corelib/src/RtabmapThread.cpp
+24 -32
View File
@@ -51,9 +51,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_previousStamp(0.0),
_rtabmap(rtabmap),
_paused(false),
lastPose_(Transform::getIdentity()),
_rotVariance(0),
_transVariance(0)
lastPose_(Transform::getIdentity())
{
UASSERT(rtabmap != 0);
@@ -88,8 +86,7 @@ void RtabmapThread::clearBufferedData()
{
_dataBuffer.clear();
lastPose_.setIdentity();
_rotVariance = 0;
_transVariance = 0;
covariance_ = cv::Mat();
_previousStamp = 0;
}
_dataMutex.unlock();
@@ -338,7 +335,9 @@ bool RtabmapThread::handleEvent(UEvent* event)
{
if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
{
this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
OdometryInfo infoCov;
infoCov.covariance = e->info().odomCovariance;
this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
}
else
{
@@ -347,7 +346,9 @@ bool RtabmapThread::handleEvent(UEvent* event)
}
else
{
this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
OdometryInfo infoCov;
infoCov.covariance = e->info().odomCovariance;
this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
}
}
@@ -531,7 +532,7 @@ void RtabmapThread::process()
if(_rtabmap->getMemory())
{
bool wasPlanning = _rtabmap->getPath().size()>0;
if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
if(_rtabmap->process(data.data(), data.pose(), data.covariance(), data.velocity()))
{
Statistics stats = _rtabmap->getStatistics();
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
@@ -569,8 +570,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
if(!lastPose_.isIdentity() &&
(odomEvent.pose().isIdentity() ||
odomEvent.info().varianceLin>=9999 ||
odomEvent.info().varianceAng>=9999 ||
odomEvent.rotVariance()>=9999 ||
odomEvent.transVariance()>=9999))
{
@@ -580,25 +579,21 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
else
{
UWARN("Odometry is reset (high variance (%f/%f >=9999 detected). Increment map id!",
odomEvent.info().varianceLin>odomEvent.transVariance()?odomEvent.info().varianceLin:odomEvent.transVariance(),
odomEvent.info().varianceAng>odomEvent.rotVariance()?odomEvent.info().varianceAng:odomEvent.rotVariance());
UWARN("Odometry is reset (high variance (%f/%f >=9999 detected). Increment map id!", odomEvent.transVariance(), odomEvent.rotVariance());
}
pushNewState(kStateTriggeringMap);
_rotVariance = 0;
_transVariance = 0;
covariance_ = cv::Mat();
}
double maxRotVar = odomEvent.rotVariance();
double maxTransVar = odomEvent.transVariance();
// FIXME: should merge the transformations/variances like Link::merge();
if(maxRotVar != 1.0f)
if(maxRotVar != 1.0f && maxTransVar != 1.0f && !covariance_.empty())
{
_rotVariance += maxRotVar;
covariance_ = (covariance_.inv() + odomEvent.covariance().inv()).inv();
}
if(maxTransVar != 1.0f)
else
{
_transVariance += maxTransVar;
covariance_ = odomEvent.covariance();
}
if(ignoreFrame && !_createIntermediateNodes)
@@ -614,31 +609,28 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
lastPose_ = odomEvent.pose();
bool notify = true;
if(_rotVariance <= 0)
if(covariance_.empty())
{
_rotVariance = 1.0;
}
if(_transVariance <= 0)
{
_transVariance = 1.0;
covariance_ = cv::Mat::eye(6,6,CV_64FC1);
}
OdometryInfo odomInfo = odomEvent.info().copyWithoutData();
odomInfo.covariance = covariance_;
if(ignoreFrame)
{
// set negative id so rtabmap will detect it as an intermediate node
SensorData tmp = odomEvent.data();
tmp.setId(-1);
tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
}
else
{
_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), odomInfo));
}
UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
UINFO("Added data %d", odomEvent.data().id());
_rotVariance = 0;
_transVariance = 0;
covariance_ = cv::Mat();
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
{
if(_rate > 0.0f)
corelib/src/resources/DatabaseSchema.sql.in
+9 -9
View File
@@ -18,8 +18,9 @@ CREATE TABLE Node (
map_id INTEGER NOT NULL,
weight INTEGER,
stamp FLOAT,
pose BLOB,
ground_truth_pose BLOB,
pose BLOB, -- 3x4 float
ground_truth_pose BLOB, -- 3x4 float
velocity BLOB, -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
label TEXT,
time_enter DATE,
@@ -51,9 +52,8 @@ CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
rot_variance FLOAT NOT NULL,
trans_variance FLOAT NOT NULL,
transform BLOB,
information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
transform BLOB, -- 3x4 float
user_data BLOB, -- compressed data (User data)
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
@@ -68,7 +68,7 @@ CREATE TABLE Word (
PRIMARY KEY (id)
);
CREATE TABLE Map_Node_Word (
CREATE TABLE Feature (
node_id INTEGER NOT NULL,
word_id INTEGER NOT NULL,
pos_x FLOAT NOT NULL,
@@ -111,10 +111,10 @@ CREATE TABLE Admin (
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word
CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature
WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
BEGIN
SELECT RAISE(ABORT, 'Foreign key constraint failed in Map_Node_Word table');
SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
END;
-- Creating a trigger for time_enter
@@ -137,7 +137,7 @@ END;
-- INDEXES
-- *******************************************************************
CREATE UNIQUE INDEX IDX_Node_id on Node (id);
CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
CREATE INDEX IDX_Feature_node_id on Feature (node_id);
CREATE INDEX IDX_Link_from_id on Link (from_id);
CREATE UNIQUE INDEX IDX_node_label on Node (label);
CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
corelib/src/util3d_motion_estimation.cpp
+11 -11
View File
@@ -56,7 +56,7 @@ Transform estimateMotion3DTo2D(
int refineIterations,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
double * varianceOut,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
@@ -65,9 +65,9 @@ Transform estimateMotion3DTo2D(
Transform transform;
std::vector<int> matches, inliers;
if(varianceOut)
if(covariance)
{
*varianceOut = 1.0;
*covariance = cv::Mat::eye(6,6,CV_64FC1);
}
// find correspondences
@@ -138,7 +138,7 @@ Transform estimateMotion3DTo2D(
transform = (cameraModel.localTransform() * pnp).inverse();
// compute variance (like in PCL computeVariance() method of sac_model.h)
if(varianceOut && words3B.size())
if(covariance && words3B.size())
{
std::vector<float> errorSqrdDists(inliers.size());
oi = 0;
@@ -162,10 +162,10 @@ Transform estimateMotion3DTo2D(
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
*varianceOut = 2.1981 * median_error_sqr;
*covariance *= 2.1981 * median_error_sqr;
}
}
else if(varianceOut)
else if(covariance)
{
// compute variance, which is the rms of reprojection errors
std::vector<cv::Point2f> imagePointsReproj;
@@ -175,7 +175,7 @@ Transform estimateMotion3DTo2D(
{
err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
}
*varianceOut = std::sqrt(err/float(inliers.size()));
*covariance *= std::sqrt(err/float(inliers.size()));
}
}
}
@@ -203,7 +203,7 @@ Transform estimateMotion3DTo3D(
double inliersDistance,
int iterations,
int refineIterations,
double * varianceOut,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
@@ -222,9 +222,9 @@ Transform estimateMotion3DTo3D(
UASSERT(inliers1.size() == inliers2.size());
UDEBUG("Unique correspondences = %d", (int)inliers1.size());
if(varianceOut)
if(covariance)
{
*varianceOut = 1.0;
*covariance = cv::Mat::eye(6,6,CV_64FC1);
}
std::vector<int> inliers;
@@ -251,7 +251,7 @@ Transform estimateMotion3DTo3D(
refineIterations,
3.0,
&inliers,
varianceOut);
covariance);
if(!t.isNull() && (int)inliers.size() >= minInliers)
{
corelib/src/util3d_registration.cpp
+5 -5
View File
@@ -66,15 +66,15 @@ Transform transformFromXYZCorrespondences(
int refineIterations,
double refineSigma,
std::vector<int> * inliersOut,
double * varianceOut)
cv::Mat * covariance)
{
//NOTE: this method is a mix of two methods:
// - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
// - refineModel() in pcl/sample_consensus/sac.h
if(varianceOut)
if(covariance)
{
*varianceOut = 1.0;
*covariance = cv::Mat::eye(6,6,CV_64FC1);
}
Transform transform;
if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
@@ -199,9 +199,9 @@ Transform transformFromXYZCorrespondences(
{
*inliersOut = inliers;
}
if(varianceOut)
if(covariance)
{
*varianceOut = model->computeVariance();
*covariance *= model->computeVariance();
}
// get best transformation