i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +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-04 16:13:45 -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

47
corelib/src/Link.cpp
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()
{