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

DbViewer: when align with ground truth is enabled, exported poses are those aligned with ground truth

Browse Source
This commit is contained in:
matlabbe
2018-03-12 18:51:55 -04:00
parent 10724fac3b
commit dc289ba635
2 changed files with 54 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/OdometryOkvis.cpp
View File

@@ -69,6 +69,7 @@ public:
Transform getLastTransform() Transform getLastTransform()
{ {
UDEBUG("");
Transform tf; Transform tf;
mutex_.lock(); mutex_.lock();
tf = transform_; tf = transform_;
@@ -79,6 +80,7 @@ public:
std::map<int, cv::Point3f> getLastLandmarks() std::map<int, cv::Point3f> getLastLandmarks()
{ {
UDEBUG("");
std::map<int, cv::Point3f> landmarks; std::map<int, cv::Point3f> landmarks;
mutexLandmarks_.lock(); mutexLandmarks_.lock();
landmarks = landmarks_; landmarks = landmarks_;
@@ -104,22 +106,14 @@ public:
const okvis::MapPointVector & landmarksVector, const okvis::MapPointVector & landmarksVector,
const okvis::MapPointVector & /*transferredLandmarks*/) const okvis::MapPointVector & /*transferredLandmarks*/)
{ {
bool notify = true; UDEBUG("");
mutexLandmarks_.lock(); mutexLandmarks_.lock();
if(landmarks_.size())
{
notify = false;
}
landmarks_.clear(); landmarks_.clear();
for(unsigned int i=0; i<landmarksVector.size(); ++i) for(unsigned int i=0; i<landmarksVector.size(); ++i)
{ {
landmarks_.insert(std::make_pair((int)landmarksVector[i].id, cv::Point3f(landmarksVector[i].point[0], landmarksVector[i].point[1], landmarksVector[i].point[2]))); landmarks_.insert(std::make_pair((int)landmarksVector[i].id, cv::Point3f(landmarksVector[i].point[0], landmarksVector[i].point[1], landmarksVector[i].point[2])));
} }
mutexLandmarks_.unlock(); mutexLandmarks_.unlock();
if(notify)
{
semLandmarks_.release();
}
} }
@@ -129,8 +123,6 @@ private:
std::map<int, cv::Point3f> landmarks_; std::map<int, cv::Point3f> landmarks_;
UMutex mutex_; UMutex mutex_;
UMutex mutexLandmarks_; UMutex mutexLandmarks_;
USemaphore semTf_;
USemaphore semLandmarks_;
}; };
#endif #endif
@@ -156,6 +148,7 @@ OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
OdometryOkvis::~OdometryOkvis() OdometryOkvis::~OdometryOkvis()
{ {
UDEBUG("");
#ifdef RTABMAP_OKVIS #ifdef RTABMAP_OKVIS
if(okvisEstimator_) if(okvisEstimator_)
{ {

50
guilib/src/DatabaseViewer.cpp
View File

@@ -2051,6 +2051,56 @@ void DatabaseViewer::exportPoses(int format)
else else
{ {
optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->checkBox_alignPosesWithGroundTruth->isChecked())
{
std::map<int, Transform> refPoses = groundTruthPoses_;
if(refPoses.empty())
{
refPoses = gpsPoses_;
}
// Log ground truth statistics (in TUM's RGBD-SLAM format)
if(refPoses.size())
{
float translational_rmse = 0.0f;
float translational_mean = 0.0f;
float translational_median = 0.0f;
float translational_std = 0.0f;
float translational_min = 0.0f;
float translational_max = 0.0f;
float rotational_rmse = 0.0f;
float rotational_mean = 0.0f;
float rotational_median = 0.0f;
float rotational_std = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
Transform gtToMap = graph::calcRMSE(
refPoses,
optimizedPoses,
translational_rmse,
translational_mean,
translational_median,
translational_std,
translational_min,
translational_max,
rotational_rmse,
rotational_mean,
rotational_median,
rotational_std,
rotational_min,
rotational_max);
if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity())
{
for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
iter->second = gtToMap * iter->second;
}
}
}
}
} }
if(optimizedPoses.size()) if(optimizedPoses.size())