Fixing prior and gravity constraints support for SBA with orbslam dep… (#1683)

* Fixing prior and gravity constraints support for SBA with orbslam dependency

* Updated an error log msg

corelib/src/odometry/OdometryORBSLAM3.cpp

@@ -430,7 +430,14 @@ Transform OdometryORBSLAM3::computeTransform(
 		(data.stereoCameraModels().size() == 1 &&
 			data.stereoCameraModels()[0].isValidForProjection())))
 	{
-		UERROR("Invalid camera model!");
+		if(data.cameraModels().size() > 1 || data.stereoCameraModels().size() > 1)
+		{
+			UERROR("Multi-camera not supported with ORB_SLAM integration!");
+		}
+		else
+		{
+			UERROR("Invalid camera model!");
+		}
 		return t;
 	}
 

corelib/src/optimizer/OptimizerG2O.cpp

@@ -61,8 +61,6 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
 #include "g2o/types/slam3d/types_slam3d.h"
 #include "g2o/edge_se3_xyzprior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
 #include "g2o/edge_se3_gravity.h"
-#include "g2o/edge_sbacam_gravity.h"
-#include "g2o/edge_sbacam_prior.h"
 #include "g2o/edge_xy_prior.h"  // Include after types_slam2d.h to be ignored on newest g2o versions
 #include "g2o/edge_xyz_prior.h" // Include after types_slam3d.h to be ignored on newest g2o versions
 #ifdef G2O_HAVE_CSPARSE
@@ -78,6 +76,19 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
 #include "g2o/types/types_sba.h"
 #include "g2o/types/types_six_dof_expmap.h"
 #include "g2o/solvers/linear_solver_eigen.h"
+#include "g2o/edge_se3_expmap.h"
+#endif
+
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+namespace rtabmap {
+#ifdef RTABMAP_ORB_SLAM
+typedef g2o::VertexSE3Expmap VertexCam;
+#else
+typedef g2o::VertexCam VertexCam;
+#endif
+}
+#include "g2o/edge_sbacam_gravity.h"
+#include "g2o/edge_sbacam_prior.h"
 #endif
 
 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
@@ -1415,81 +1426,6 @@ std::map<int, Transform> OptimizerG2O::optimize(
 	return optimizedPoses;
 }
 
-#ifdef RTABMAP_ORB_SLAM
-/**
- * \brief 3D edge between two SBAcam
- */
- class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
-{
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-    EdgeSE3Expmap():  BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
-    bool read(std::istream& is)
-      {
-        return false;
-      }
-
-      bool write(std::ostream& os) const
-      {
-        return false;
-      }
-
-    void computeError()
-    {
-      const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-      const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-      g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
-      _error[0]=delta.translation().x();
-      _error[1]=delta.translation().y();
-      _error[2]=delta.translation().z();
-      _error[3]=delta.rotation().x();
-      _error[4]=delta.rotation().y();
-      _error[5]=delta.rotation().z();
-    }
-
-    virtual void setMeasurement(const g2o::SE3Quat& meas){
-      _measurement=meas;
-      _inverseMeasurement=meas.inverse();
-    }
-
-    virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
-    virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
-    	g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
-    	g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
-		if (from_.count(from) > 0)
-		  to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
-		else
-		  from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
-    }
-
-    virtual bool setMeasurementData(const double* d){
-      Eigen::Map<const g2o::Vector7d> v(d);
-      _measurement.fromVector(v);
-      _inverseMeasurement = _measurement.inverse();
-      return true;
-    }
-
-    virtual bool getMeasurementData(double* d) const{
-      Eigen::Map<g2o::Vector7d> v(d);
-      v = _measurement.toVector();
-      return true;
-    }
-
-    virtual int measurementDimension() const {return 7;}
-
-    virtual bool setMeasurementFromState() {
-    	const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
-		const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
-		_measurement = (v1->estimate().inverse()*v2->estimate());
-		_inverseMeasurement = _measurement.inverse();
-		return true;
-    }
-
-  protected:
-    g2o::SE3Quat _inverseMeasurement;
-};
-#endif
-
 std::map<int, Transform> OptimizerG2O::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
@@ -1588,7 +1524,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		// detect if there are gravity constraints
 		bool hasGravityConstraints = false;
-#ifndef RTABMAP_ORB_SLAM
 		if(!isSlam2d() && gravitySigma() > 0)
 		{
 			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -1601,7 +1536,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				}
 			}
 		}
-#endif
 
 
 		UDEBUG("fill %ld poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", poses.size(), rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
@@ -1620,11 +1554,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 					// Add node's pose
 					UASSERT(!camPose.isNull());
-#ifdef RTABMAP_ORB_SLAM
+
-					g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
+					rtabmap::VertexCam * vCam = new rtabmap::VertexCam();
-#else
-					g2o::VertexCam * vCam = new g2o::VertexCam();
-#endif
 
 					Eigen::Affine3d a = camPose.toEigen3d();
 #ifdef RTABMAP_ORB_SLAM
@@ -1732,7 +1663,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 				if(id1 == id2)
 				{
-#ifndef RTABMAP_ORB_SLAM
 					g2o::HyperGraph::Edge * edge = 0;
 					if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
 					{
@@ -1746,7 +1676,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 						Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma());
 
-						g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
+						rtabmap::VertexCam* v1 = (rtabmap::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
 						EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity());
 						std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
 						// Gravity constraint added only to first camera of a pose
@@ -1763,7 +1693,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
 						return optimizedPoses;
 					}
-#endif
 				}
 				else if(id1>0 && id2>0) // not supporting landmarks
 				{
@@ -1919,14 +1848,14 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 						g2o::OptimizableGraph::Edge * e;
 						double baseline = 0.0;
+						rtabmap::VertexCam* vcam = dynamic_cast<rtabmap::VertexCam*>(optimizer.vertex(camId));
 #ifdef RTABMAP_ORB_SLAM
-						g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
+						
 						std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(poseId);
 
-						UASSERT(iterModel != models.end() && camIndex<iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
+						UASSERT(iterModel != models.end() && camIndex<(int)iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
 						baseline = iterModel->second[camIndex].Tx()<0.0?-iterModel->second[camIndex].Tx()/iterModel->second[camIndex].fx():baseline_;
 #else
-						g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
 						baseline = vcam->estimate().baseline;
 #endif
 						double variance = pixelVariance_;

corelib/src/optimizer/g2o/edge_sbacam_gravity.h

@@ -32,14 +32,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RTAB_G2O_EDGE_SBACAM_GRAVITY_H_
 #define RTAB_G2O_EDGE_SBACAM_GRAVITY_H_
 
+#ifdef RTABMAP_ORB_SLAM
+#include "g2o/types/types_six_dof_expmap.h"
+#else
 #include "g2o/types/sba/types_sba.h"
+#endif
 #include "g2o/core/base_unary_edge.h"
 namespace rtabmap {
   /**
    * \brief EdgeSBACamGravity
     * \brief g2o edge with gravity constraint
    */
-class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, g2o::VertexCam> {
+class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, VertexCam> {
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 	EdgeSBACamGravity(){
@@ -56,30 +60,36 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6,
 
     // return the error estimate as a 3-vector
     void computeError(){
-    	const g2o::VertexCam* v1 = static_cast<const g2o::VertexCam*>(_vertices[0]);
+    	const VertexCam* v = static_cast<const VertexCam*>(_vertices[0]);
 
-		Eigen::Vector3d direction = _measurement.head<3>();
+      Eigen::Vector3d direction = _measurement.head<3>();
-		Eigen::Vector3d measurement = _measurement.tail<3>();
+      Eigen::Vector3d measurement = _measurement.tail<3>();
 
-		Eigen::Vector3d ea;
+      g2o::SE3Quat estimate;
+#ifdef RTABMAP_ORB_SLAM
+      estimate = v->estimate().inverse();
+#else
+      estimate = v->estimate();
+#endif
 
-		// Transform pose from camera frame to world frame
+      // Transform pose from camera frame to world frame
-		Eigen::Matrix3d t = v1->estimate().rotation().toRotationMatrix() * cameraInvLocalTransform_;
+      Eigen::Matrix3d t = estimate.rotation().toRotationMatrix() * cameraInvLocalTransform_;
-		ea[0] = atan2(t (2, 1), t (2, 2));
+      Eigen::Vector3d ea;
-		ea[1] = asin(-t (2, 0));
+      ea[0] = atan2(t (2, 1), t (2, 2));
-		ea[2] = atan2(t (1, 0), t (0, 0));
+      ea[1] = asin(-t (2, 0));
+      ea[2] = atan2(t (1, 0), t (0, 0));
 
-		Eigen::Matrix3d rot =
+      Eigen::Matrix3d rot =
-		   (Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
+        (Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
-			Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX())).toRotationMatrix();
+        Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX())).toRotationMatrix();
 
-		Eigen::Vector3d estimate = rot * -direction;
+      Eigen::Vector3d newEstimate = rot * -direction;
-		_error = estimate - measurement;
+      _error = newEstimate - measurement;
 
-		/*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(),
+      /*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(),
-		  measurement[0], measurement[1], measurement[2],
+        measurement[0], measurement[1], measurement[2],
-		  estimate[0], estimate[1], estimate[2],
+        estimate[0], estimate[1], estimate[2],
-		  _error[0], _error[1], _error[2]);*/
+        _error[0], _error[1], _error[2]);*/
     }
 
     // 6 values:

corelib/src/optimizer/g2o/edge_sbacam_prior.h

@@ -32,14 +32,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RTAB_G2O_EDGE_SBACAM_PRIOR_H_
 #define RTAB_G2O_EDGE_SBACAM_PRIOR_H_
 
+#ifdef RTABMAP_ORB_SLAM
+#include "g2o/types/types_six_dof_expmap.h"
+#else
 #include "g2o/types/sba/types_sba.h"
+#endif
 #include "g2o/core/base_unary_edge.h"
 namespace rtabmap {
   /**
    * \brief EdgeSBACamPrior
     * \brief g2o edge with gravity constraint
    */
-class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, g2o::VertexCam> {
+class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, VertexCam> {
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     EdgeSBACamPrior() {
@@ -54,8 +58,14 @@ class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, g2o::VertexCa
 
     // return the error estimate as a 3-vector
     void computeError() {
-      const g2o::VertexCam* v = static_cast<const g2o::VertexCam*>(_vertices[0]);
+      const VertexCam* v = static_cast<const VertexCam*>(_vertices[0]);
-      g2o::SE3Quat delta = _inverseMeasurement * v->estimate() * _cameraInvLocalTransform;
+      g2o::SE3Quat estimate;
+#ifdef RTABMAP_ORB_SLAM
+      estimate = v->estimate().inverse();
+#else
+      estimate = v->estimate();
+#endif
+      g2o::SE3Quat delta = _inverseMeasurement * estimate * _cameraInvLocalTransform;
       _error[0]=delta.translation().x();
       _error[1]=delta.translation().y();
       _error[2]=delta.translation().z();
@@ -97,10 +107,14 @@ class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, g2o::VertexCa
     }
 
     virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to) {
-      g2o::VertexCam *v = static_cast<g2o::VertexCam*>(_vertices[0]);
+      VertexCam *v = static_cast<VertexCam*>(_vertices[0]);
       assert(v && "Vertex for the Prior edge is not set");
 
+#ifdef RTABMAP_ORB_SLAM
+      g2o::SE3Quat newEstimate = _cameraInvLocalTransform * _inverseMeasurement;
+#else
       g2o::SE3Quat newEstimate = measurement()*_cameraInvLocalTransform.inverse();
+#endif
       if (_information.block<3,3>(0,0).array().abs().sum() == 0){ // do not set translation, as that part of the information is all zero
         newEstimate.setTranslation(v->estimate().translation());
       }

corelib/src/optimizer/g2o/edge_se3_expmap.h

@@ -0,0 +1,74 @@
+#include "g2o/types/types_six_dof_expmap.h"
+
+/**
+ * \brief 3D edge between two SBAcam
+ */
+ class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
+{
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    EdgeSE3Expmap():  BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
+    bool read(std::istream& is)
+      {
+        return false;
+      }
+
+      bool write(std::ostream& os) const
+      {
+        return false;
+      }
+
+    void computeError()
+    {
+      const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+      const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+      g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+      _error[0]=delta.translation().x();
+      _error[1]=delta.translation().y();
+      _error[2]=delta.translation().z();
+      _error[3]=delta.rotation().x();
+      _error[4]=delta.rotation().y();
+      _error[5]=delta.rotation().z();
+    }
+
+    virtual void setMeasurement(const g2o::SE3Quat& meas){
+      _measurement=meas;
+      _inverseMeasurement=meas.inverse();
+    }
+
+    virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
+    virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
+    	g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
+    	g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
+		if (from_.count(from) > 0)
+		  to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
+		else
+		  from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
+    }
+
+    virtual bool setMeasurementData(const double* d){
+      Eigen::Map<const g2o::Vector7d> v(d);
+      _measurement.fromVector(v);
+      _inverseMeasurement = _measurement.inverse();
+      return true;
+    }
+
+    virtual bool getMeasurementData(double* d) const{
+      Eigen::Map<g2o::Vector7d> v(d);
+      v = _measurement.toVector();
+      return true;
+    }
+
+    virtual int measurementDimension() const {return 7;}
+
+    virtual bool setMeasurementFromState() {
+    	const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+		const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+		_measurement = (v1->estimate().inverse()*v2->estimate());
+		_inverseMeasurement = _measurement.inverse();
+		return true;
+    }
+
+  protected:
+    g2o::SE3Quat _inverseMeasurement;
+};

guilib/include/rtabmap/gui/StatsToolBox.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <QWidget>
 #include <QtCore/QMap>
+#include <QTimer>
 
 class QToolButton;
 class QLabel;
@@ -59,6 +60,7 @@ public:
 
 public Q_SLOTS:
 	void updateMenu(const QMenu * menu);
+	void updateLabel();
 
 Q_SIGNALS:
 	void valueAdded(qreal);
@@ -117,6 +119,8 @@ Q_SIGNALS:
 private Q_SLOTS:
 	void plot(const StatItem * stat, const QString & plotName = QString());
 	void figureDeleted(QObject * obj);
+	void requestLabelsUpdate();
+	void updateLabels();
 
 protected:
 	virtual void contextMenuEvent(QContextMenuEvent * event);
@@ -127,6 +131,7 @@ private:
 	QString _workingDirectory;
 	int _newFigureMaxItems;
 	QMap<QString, QWidget*> _figures;
+	QTimer _updateLabelsTimer;
 };
 
 }

guilib/src/StatsToolBox.cpp

@@ -69,6 +69,7 @@ StatItem::StatItem(const QString & name, bool cacheOn, const std::vector<qreal>
 		_y = y;
 	}
 	_unit->setText(unit);
+	_value->setTextFormat(Qt::PlainText);
 	this->updateMenu(menu);
 }
 
@@ -90,7 +91,6 @@ void StatItem::addValue(qreal y)
 	{
 		_y.push_back(y);
 	}
-	_value->setText(QString::number(y, 'g', 3));
 	Q_EMIT valueAdded(y);
 }
 
@@ -107,7 +107,6 @@ void StatItem::addValue(qreal x, qreal y)
 		_x.push_back(x);
 	}
 
-	_value->setText(QString::number(y, 'g', 3));
 	Q_EMIT valueAdded(x,y);
 }
 
@@ -117,18 +116,23 @@ void StatItem::setValues(const std::vector<qreal> & x, const std::vector<qreal>
 	{
 		_x = x;
 		_y = y;
-		if(y.size())
-		{
-			_value->setNum(y[y.size()-1]);
-		}
-	}
-	else
-	{
-		_value->setText("*");
 	}
 	Q_EMIT valuesChanged(x,y);
 }
 
+void StatItem::updateLabel()
+{
+	QString newText;
+	if(_y.size())
+	{
+		newText = QString::number(_y.back(), 'g', 3);
+	}
+	if(newText != _value->text())
+	{
+		_value->setText(newText);
+	}
+}
+
 QString StatItem::value() const
 {
 	return _value->text();
@@ -227,6 +231,8 @@ StatsToolBox::StatsToolBox(QWidget * parent) :
 	_plotMenu->addAction(tr("<New figure>"));
 	_workingDirectory = QDir::homePath();
 	_newFigureMaxItems = 0;
+	_updateLabelsTimer.setSingleShot(true);
+	connect(&_updateLabelsTimer, &QTimer::timeout, this, &StatsToolBox::updateLabels);
 }
 
 StatsToolBox::~StatsToolBox()
@@ -263,6 +269,7 @@ void StatsToolBox::updateStat(const QString & statFullName, qreal y, bool cacheO
 	std::vector<qreal> vx,vy(1);
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
+	requestLabelsUpdate();
 }
 
 void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn)
@@ -271,6 +278,7 @@ void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bo
 	vx[0] = x;
 	vy[0] = y;
 	updateStat(statFullName, vx, vy, cacheOn);
+	requestLabelsUpdate();
 }
 
 void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn)
@@ -295,6 +303,7 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qr
 		{
 			item->setValues(x, y);
 		}
+		requestLabelsUpdate();
 	}
 	else
 	{
@@ -378,6 +387,22 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qr
 	}
 }
 
+void StatsToolBox::requestLabelsUpdate()
+{
+	if(!_updateLabelsTimer.isActive())
+	{
+		_updateLabelsTimer.start(100); // Max 10 Hz
+	}
+}
+void StatsToolBox::updateLabels()
+{
+	QList<StatItem *> items = _statBox->findChildren<StatItem *>();
+	for(int i=0; i<items.size(); ++i)
+	{
+		items[i]->updateLabel();
+	}
+}
+
 void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 {
 	QWidget * fig = _figures.value(plotName, (QWidget*)0);