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

Marker priors (#859)

Browse Source 
* Added MarkerPriors parameter

* Fixed Marker/Priors format to use '|' instead ';'. Fixed landmark priors not used.

* Marker: added priors variance parameters

* g2o: refactored backward compatibility includes

* fixed build with old g2o
This commit is contained in:
matlabbe
2022-04-28 09:18:30 -04:00
committed by GitHub
parent 190071678f
commit b646c5e1db
14 changed files with 731 additions and 283 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+5 -2
View File
@@ -780,8 +780,11 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Marker, VarianceLinear, float, 0.001, "Linear variance to set on marker detections.");
RTABMAP_PARAM(Marker, VarianceAngular, float, 0.01, "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
RTABMAP_PARAM(Marker, CornerRefinementMethod, int, 0, "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
RTABMAP_PARAM(Marker, MaxRange, float, 0.0, "Maximum range in which markers will be detected. <=0 for unlimited range.");
RTABMAP_PARAM(Marker, MinRange, float, 0.0, "Miniminum range in which markers will be detected. <=0 for unlimited range.");
RTABMAP_PARAM(Marker, MaxRange, float, 0.0, "Maximum range in which markers will be detected. <=0 for unlimited range.");
RTABMAP_PARAM(Marker, MinRange, float, 0.0, "Miniminum range in which markers will be detected. <=0 for unlimited range.");
RTABMAP_PARAM_STR(Marker, Priors, "", "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example: \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
RTABMAP_PARAM(Marker, PriorsVarianceLinear, float, 0.001, "Linear variance to set on marker priors.");
RTABMAP_PARAM(Marker, PriorsVarianceAngular, float, 0.001, "Angular variance to set on marker priors.");
RTABMAP_PARAM(ImuFilter, MadgwickGain, double, 0.1, "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
RTABMAP_PARAM(ImuFilter, MadgwickZeta, double, 0.0, "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
corelib/include/rtabmap/core/Rtabmap.h
+3
View File
@@ -324,6 +324,8 @@ private:
bool _loopGPS;
int _maxOdomCacheSize;
bool _createGlobalScanMap;
float _markerPriorsLinearVariance;
float _markerPriorsAngularVariance;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
@@ -364,6 +366,7 @@ private:
std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures
std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
std::vector<float> _odomCorrectionAcc;
std::map<int, Transform> _markerPriors;
// Planning stuff
int _pathStatus;
corelib/src/CMakeLists.txt
-3
View File
@@ -400,9 +400,6 @@ IF(G2O_FOUND)
${G2O_LIBRARIES}
)
ENDIF()
SET(SRC_FILES ${SRC_FILES}
optimizer/g2o/edge_se3_xyzprior.cpp
)
IF(WITH_VERTIGO)
SET(SRC_FILES ${SRC_FILES}
optimizer/vertigo/g2o/edge_se2Switchable.cpp
corelib/src/Rtabmap.cpp
+53
View File
@@ -604,6 +604,44 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
UASSERT(_markerPriorsLinearVariance>0.0f);
Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), _markerPriorsAngularVariance);
UASSERT(_markerPriorsAngularVariance>0.0f);
std::string markerPriorsStr;
if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
{
_markerPriors.clear();
std::list<std::string> strList = uSplit(markerPriorsStr, '|');
for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
{
std::string markerStr = *iter;
while(!markerStr.empty() && !uIsDigit(markerStr[0]))
{
markerStr.erase(markerStr.begin());
}
if(!markerStr.empty())
{
std::string idStr = uSplitNumChar(markerStr).front();
int id = uStr2Int(idStr);
Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
if(!prior.isNull() && id>0)
{
_markerPriors.insert(std::make_pair(-id, prior));
UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
}
else
{
UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
}
}
else if(!iter->empty())
{
UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
}
}
}
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
UASSERT(_rgbdLinearSpeedUpdate >= 0.0f);
@@ -1554,6 +1592,7 @@ bool Rtabmap::process(
if(_optimizedPoses.find(iter->first) == _optimizedPoses.end())
{
_optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform()));
UDEBUG("Added landmark %d : %s", iter->first, (newPose*iter->second.transform()).prettyPrint().c_str());
addedNewLandmark = true;
}
_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
@@ -4738,6 +4777,20 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored());
UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
// add landmark priors if there are some
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0; ++iter)
{
if(_markerPriors.find(iter->first) != _markerPriors.end())
{
cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
infMatrix(cv::Range(0,3), cv::Range(0,3)) /= _markerPriorsLinearVariance;
infMatrix(cv::Range(3,6), cv::Range(3,6)) /= _markerPriorsAngularVariance;
edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPosePrior, _markerPriors.at(iter->first), infMatrix)));
UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", iter->first, _markerPriors.at(iter->first).prettyPrint().c_str(),
_markerPriorsLinearVariance, _markerPriorsAngularVariance);
}
}
if(_graphOptimizer->iterations() > 0)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
corelib/src/optimizer/OptimizerG2O.cpp
+57 -5
View File
@@ -59,9 +59,11 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
#include "g2o/config.h"
#include "g2o/types/slam2d/types_slam2d.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/edge_se3_xyzprior.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_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
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif
@@ -531,11 +533,37 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(id1 == id2)
{
if(iter->second.type() == Link::kPosePrior && !priorsIgnored())
if(iter->second.type() == Link::kPosePrior && !priorsIgnored() &&
(!landmarksIgnored() || id1>0))
{
int idTag= id1;
if(id1<0)
{
// landmark prior, offset ids
id1 = landmarkVertexOffset - id1;
id2 = landmarkVertexOffset - id2;
}
if(isSlam2d())
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
if(idTag < 0 && !isLandmarkWithRotation.at(idTag))
{
g2o::EdgeXYPrior * priorEdge = new g2o::EdgeXYPrior();
g2o::VertexPointXY* v1 = (g2o::VertexPointXY*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(Eigen::Vector2d(iter->second.transform().x(), iter->second.transform().y()));
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
}
priorEdge->setInformation(information);
edge = priorEdge;
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
g2o::EdgeSE2XYPrior * priorEdge = new g2o::EdgeSE2XYPrior();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
@@ -578,12 +606,36 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
else
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
if(idTag < 0 && !isLandmarkWithRotation.at(idTag))
{
//XYZ case
g2o::EdgeXYZPrior * priorEdge = new g2o::EdgeXYZPrior();
g2o::VertexPointXYZ* v1 = (g2o::VertexPointXYZ*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(Eigen::Vector3d(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,2); // x-z
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,2); // y-z
information(2,0) = iter->second.infMatrix().at<double>(2,0); // z-x
information(2,1) = iter->second.infMatrix().at<double>(2,1); // z-y
information(2,2) = iter->second.infMatrix().at<double>(2,2); // z-z
}
priorEdge->setInformation(information);
edge = priorEdge;
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
//GPS XYZ case
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
g2o::EdgeSE3XYZPrior * priorEdge = new g2o::EdgeSE3XYZPrior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
priorEdge->setVertex(0, v1);
priorEdge->setMeasurement(Eigen::Vector3d(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()));
corelib/src/optimizer/OptimizerGTSAM.cpp
+23 -7
View File
@@ -51,8 +51,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include "gtsam/GravityFactor.h"
#include "gtsam/GPSPose2XYFactor.h"
#include "gtsam/GPSPose3XYZFactor.h"
#include <optimizer/gtsam/XYFactor.h>
#include <optimizer/gtsam/XYZFactor.h>
#ifdef RTABMAP_VERTIGO
#include "vertigo/gtsam/betweenFactorSwitchable.h"
@@ -233,16 +233,24 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UASSERT(!iter->second.transform().isNull());
if(id1 == id2)
{
if(iter->second.type() == Link::kPosePrior && !priorsIgnored())
if(iter->second.type() == Link::kPosePrior && !priorsIgnored() &&
(!landmarksIgnored() || id1>0))
{
if(isSlam2d())
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(GPSPose2XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
}
else
{
@@ -266,7 +274,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else
{
if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
graph.add(XYZFactor<gtsam::Point3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
@@ -274,7 +290,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
graph.add(GPSPose3XYZFactor(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
graph.add(XYZFactor<gtsam::Pose3>(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model));
}
else
{
corelib/src/optimizer/g2o/edge_se3_xyzprior.cpp
-107
View File
@@ -1,107 +0,0 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "edge_se3_xyzprior.h"
namespace rtabmap {
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>()
{
information().setIdentity();
setMeasurement(Eigen::Vector3d::Zero());
_cache = 0;
_offsetParam = 0;
resizeParameters(1);
installParameter(_offsetParam, 0);
}
bool EdgeSE3XYZPrior::resolveCaches(){
assert(_offsetParam);
g2o::ParameterVector pv(1);
pv[0] = _offsetParam;
resolveCache(_cache, (g2o::OptimizableGraph::Vertex*)_vertices[0], "CACHE_SE3_OFFSET", pv);
return _cache != 0;
}
bool EdgeSE3XYZPrior::read(std::istream& is)
{
int pid;
is >> pid;
if (!setParameterId(0, pid))
return false;
// measured keypoint
Eigen::Vector3d meas;
for (int i = 0; i < 3; i++) is >> meas[i];
setMeasurement(meas);
// read covariance matrix (upper triangle)
if (is.good()) {
for (int i = 0; i < 3; i++) {
for (int j = i; j < 3; j++) {
is >> information()(i,j);
if (i != j)
information()(j,i) = information()(i,j);
}
}
}
return !is.fail();
}
bool EdgeSE3XYZPrior::write(std::ostream& os) const {
os << _offsetParam->id() << " ";
for (int i = 0; i < 3; i++) os << measurement()[i] << " ";
for (int i = 0; i < 3; i++) {
for (int j = i; j < 3; j++) {
os << information()(i,j) << " ";
}
}
return os.good();
}
void EdgeSE3XYZPrior::computeError() {
const g2o::VertexSE3* v = static_cast<const g2o::VertexSE3*>(_vertices[0]);
_error = v->estimate().translation() - _measurement;
}
bool EdgeSE3XYZPrior::setMeasurementFromState() {
const g2o::VertexSE3* v = static_cast<const g2o::VertexSE3*>(_vertices[0]);
_measurement = v->estimate().translation();
return true;
}
void EdgeSE3XYZPrior::initialEstimate(const g2o::OptimizableGraph::VertexSet& /*from_*/, g2o::OptimizableGraph::Vertex* /*to_*/) {
g2o::VertexSE3 *v = static_cast<g2o::VertexSE3*>(_vertices[0]);
assert(v && "Vertex for the Prior edge is not set");
Eigen::Isometry3d newEstimate = _offsetParam->offset().inverse() * Eigen::Translation3d(measurement());
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.translation() = v->estimate().translation();
}
v->setEstimate(newEstimate);
}
}
corelib/src/optimizer/g2o/edge_se3_xyzprior.h
+90 -12
View File
@@ -24,36 +24,38 @@
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef RTAB_G2O_EDGE_SE3_PRIOR_XYZ_H
#define RTAB_G2O_EDGE_SE3_PRIOR_XYZ_H
#ifndef G2O_EDGE_SE3_PRIOR_XYZ_H
#define G2O_EDGE_SE3_PRIOR_XYZ_H
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/types/slam3d/parameter_se3_offset.h"
namespace rtabmap {
namespace g2o {
using namespace Eigen;
/**
* \brief Prior for a 3D pose with constraints only in xyz direction
*/
class EdgeSE3XYZPrior : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>
class EdgeSE3XYZPrior : public BaseUnaryEdge<3, Vector3d, VertexSE3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3XYZPrior();
virtual void setMeasurement(const Eigen::Vector3d& m) {
virtual void setMeasurement(const Vector3d& m) {
_measurement = m;
}
virtual bool setMeasurementData(const double * d) {
Eigen::Map<const Eigen::Vector3d> v(d);
Map<const Vector3d> v(d);
_measurement = v;
return true;
}
virtual bool getMeasurementData(double* d) const {
Eigen::Map<Eigen::Vector3d> v(d);
Map<Vector3d> v(d);
v = _measurement;
return true;
}
@@ -65,17 +67,93 @@ public:
virtual void computeError();
virtual bool setMeasurementFromState();
virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& /*from*/, g2o::OptimizableGraph::Vertex* /*to*/) {return 1.;}
virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& /*from_*/, g2o::OptimizableGraph::Vertex* /*to_*/);
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, OptimizableGraph::Vertex* /*to*/) {return 1.;}
virtual void initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/);
const g2o::ParameterSE3Offset* offsetParameter() { return _offsetParam; }
const ParameterSE3Offset* offsetParameter() { return _offsetParam; }
protected:
virtual bool resolveCaches();
g2o::ParameterSE3Offset* _offsetParam;
g2o::CacheSE3Offset* _cache;
ParameterSE3Offset* _offsetParam;
CacheSE3Offset* _cache;
};
EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, Vector3d, VertexSE3>()
{
information().setIdentity();
setMeasurement(Vector3d::Zero());
_cache = 0;
_offsetParam = 0;
resizeParameters(1);
installParameter(_offsetParam, 0);
}
bool EdgeSE3XYZPrior::resolveCaches(){
assert(_offsetParam);
ParameterVector pv(1);
pv[0] = _offsetParam;
resolveCache(_cache, (OptimizableGraph::Vertex*)_vertices[0], "CACHE_SE3_OFFSET", pv);
return _cache != 0;
}
bool EdgeSE3XYZPrior::read(std::istream& is)
{
int pid;
is >> pid;
if (!setParameterId(0, pid))
return false;
// measured keypoint
Vector3d meas;
for (int i = 0; i < 3; i++) is >> meas[i];
setMeasurement(meas);
// read covariance matrix (upper triangle)
if (is.good()) {
for (int i = 0; i < 3; i++) {
for (int j = i; j < 3; j++) {
is >> information()(i,j);
if (i != j)
information()(j,i) = information()(i,j);
}
}
}
return !is.fail();
}
bool EdgeSE3XYZPrior::write(std::ostream& os) const {
os << _offsetParam->id() << " ";
for (int i = 0; i < 3; i++) os << measurement()[i] << " ";
for (int i = 0; i < 3; i++) {
for (int j = i; j < 3; j++) {
os << information()(i,j) << " ";
}
}
return os.good();
}
void EdgeSE3XYZPrior::computeError() {
const VertexSE3* v = static_cast<const VertexSE3*>(_vertices[0]);
_error = v->estimate().translation() - _measurement;
}
bool EdgeSE3XYZPrior::setMeasurementFromState() {
const VertexSE3* v = static_cast<const VertexSE3*>(_vertices[0]);
_measurement = v->estimate().translation();
return true;
}
void EdgeSE3XYZPrior::initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *v = static_cast<VertexSE3*>(_vertices[0]);
assert(v && "Vertex for the Prior edge is not set");
Isometry3d newEstimate = _offsetParam->offset().inverse() * Translation3d(measurement());
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.translation() = v->estimate().translation();
}
v->setEstimate(newEstimate);
}
}
#endif
corelib/src/optimizer/g2o/edge_xy_prior.h
+127
View File
@@ -0,0 +1,127 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
/**
* rtabmap: To be used with older g2o version not having this file
*/
#ifndef G2O_EDGE_XY_PRIOR_H
#define G2O_EDGE_XY_PRIOR_H
#include "g2o/types/slam2d/vertex_point_xy.h"
#include "g2o/config.h"
#include "g2o/core/base_unary_edge.h"
namespace g2o {
using namespace Eigen;
class EdgeXYPrior : public BaseUnaryEdge<2, Vector2d, VertexPointXY>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeXYPrior();
void computeError()
{
const VertexPointXY* v = static_cast<const VertexPointXY*>(_vertices[0]);
_error = v->estimate()-_measurement;
}
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void setMeasurement(const Vector2d& m){
_measurement = m;
}
virtual bool setMeasurementData(const double* d){
_measurement=Vector2d(d[0], d[1]);
return true;
}
virtual bool getMeasurementData(double* d) const {
Eigen::Map<Vector2d> m(d);
m=_measurement;
return true;
}
virtual int measurementDimension() const {return 2;}
virtual bool setMeasurementFromState() {
const VertexPointXY* v = static_cast<const VertexPointXY*>(_vertices[0]);
_measurement = v->estimate();
return true;
}
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 0.;}
#ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
virtual void linearizeOplus();
#endif
};
EdgeXYPrior::EdgeXYPrior() :
BaseUnaryEdge<2, Vector2d, VertexPointXY>()
{
_information.setIdentity();
_error.setZero();
}
bool EdgeXYPrior::read(std::istream& is)
{
Vector2d p;
is >> p[0] >> p[1];
setMeasurement(p);
for (int i = 0; i < 2; ++i)
for (int j = i; j < 2; ++j) {
is >> information()(i, j);
if (i != j)
information()(j, i) = information()(i, j);
}
return true;
}
bool EdgeXYPrior::write(std::ostream& os) const
{
Vector2d p = measurement();
os << p.x() << " " << p.y();
for (int i = 0; i < 2; ++i)
for (int j = i; j < 2; ++j)
os << " " << information()(i, j);
return os.good();
}
#ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
void EdgeXYPrior::linearizeOplus()
{
_jacobianOplusXi=Matrix2d::Identity();
}
#endif
} // end namespace
#endif
corelib/src/optimizer/g2o/edge_xyz_prior.h
+131
View File
@@ -0,0 +1,131 @@
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
/**
* rtabmap: To be used with older g2o version not having this file
*/
#ifndef G2O_EDGE_XYZ_PRIOR_H_
#define G2O_EDGE_XYZ_PRIOR_H_
#include "g2o/core/base_unary_edge.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
namespace g2o {
using namespace Eigen;
/**
* \brief prior for an XYZ vertex (VertexPointXYZ)
*
* Provides a prior for a 3d point vertex. The measurement is represented by a
* Vector3d with a corresponding 3x3 upper triangle covariance matrix (upper triangle only).
*/
class EdgeXYZPrior : public BaseUnaryEdge<3, Vector3d, VertexPointXYZ> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeXYZPrior();
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
void computeError();
// jacobian
virtual void linearizeOplus();
virtual void setMeasurement(const Vector3d& m){
_measurement = m;
}
virtual bool setMeasurementData(const double* d){
Eigen::Map<const Vector3d> v(d);
_measurement = v;
return true;
}
virtual bool getMeasurementData(double* d) const{
Eigen::Map<Vector3d> v(d);
v = _measurement;
return true;
}
virtual int measurementDimension() const { return 3; }
virtual bool setMeasurementFromState() ;
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/,
OptimizableGraph::Vertex* /*to*/) {
return 0;
}
};
EdgeXYZPrior::EdgeXYZPrior() : BaseUnaryEdge<3, Vector3d, VertexPointXYZ>() {
information().setIdentity();
}
bool EdgeXYZPrior::read(std::istream& is) {
// read measurement
Vector3d meas;
for (int i=0; i<3; i++) is >> meas[i];
setMeasurement(meas);
// read covariance matrix (upper triangle)
if (is.good()) {
for ( int i=0; i<information().rows(); i++)
for (int j=i; j<information().cols(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
}
return !is.fail();
}
bool EdgeXYZPrior::write(std::ostream& os) const {
for (int i = 0; i<3; i++) os << measurement()[i] << " ";
for (int i=0; i<information().rows(); i++)
for (int j=i; j<information().cols(); j++) {
os << information()(i,j) << " ";
}
return os.good();
}
void EdgeXYZPrior::computeError() {
const VertexPointXYZ* v = static_cast<const VertexPointXYZ*>(_vertices[0]);
_error = v->estimate() - _measurement;
}
void EdgeXYZPrior::linearizeOplus(){
_jacobianOplusXi = Matrix3d::Identity();
}
bool EdgeXYZPrior::setMeasurementFromState(){
const VertexPointXYZ* v = static_cast<const VertexPointXYZ*>(_vertices[0]);
_measurement = v->estimate();
return true;
}
}
#endif
corelib/src/optimizer/gtsam/GPSPose2XYFactor.h → corelib/src/optimizer/gtsam/XYFactor.h
+5 -4
View File
@@ -20,7 +20,8 @@
namespace rtabmap {
class GPSPose2XYFactor: public gtsam::NoiseModelFactor1<gtsam::Pose2> {
template<class VALUE>
class XYFactor: public gtsam::NoiseModelFactor1<VALUE> {
private:
// measurement information
@@ -34,13 +35,13 @@ public:
* @param model noise model for GPS snesor, in X-Y
* @param m Point2 measurement
*/
GPSPose2XYFactor(gtsam::Key poseKey, const gtsam::Point2 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<gtsam::Pose2>(model, poseKey), mx_(m.x()), my_(m.y()) {}
XYFactor(gtsam::Key poseKey, const gtsam::Point2 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<VALUE>(model, poseKey), mx_(m.x()), my_(m.y()) {}
// error function
// @param p the pose in Pose2
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose2& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
gtsam::Vector evaluateError(const VALUE& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
corelib/src/optimizer/gtsam/GPSPose3XYZFactor.h → corelib/src/optimizer/gtsam/XYZFactor.h
+7 -3
View File
@@ -20,7 +20,8 @@
namespace rtabmap {
class GPSPose3XYZFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
template<class VALUE>
class XYZFactor: public gtsam::NoiseModelFactor1<VALUE> {
private:
// measurement information
@@ -34,8 +35,8 @@ public:
* @param model noise model for GPS sensor, in X-Y
* @param m Point2 measurement
*/
GPSPose3XYZFactor(gtsam::Key poseKey, const gtsam::Point3 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<gtsam::Pose3>(model, poseKey), mx_(m.x()), my_(m.y()), mz_(m.z()) {}
XYZFactor(gtsam::Key poseKey, const gtsam::Point3 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<VALUE>(model, poseKey), mx_(m.x()), my_(m.y()), mz_(m.z()) {}
// error function
// @param p the pose in Pose
@@ -47,6 +48,9 @@ public:
}
return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
}
gtsam::Vector evaluateError(const gtsam::Point3& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
}
};
} // namespace gtsamexamples
guilib/src/PreferencesDialog.cpp
+3
View File
@@ -1460,6 +1460,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
_ui->ArucoMarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
_ui->ArucoMarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
_ui->ArucoMarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str());
_ui->ArucoPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str());
_ui->ArucoPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str());
_ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str());
// IMU filter
guilib/src/ui/preferencesDialog.ui
+227 -140
View File
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>13</number>
<number>18</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -13329,6 +13329,103 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<layout class="QVBoxLayout" name="verticalLayout_136">
<item>
<layout class="QGridLayout" name="gridLayout_63" columnstretch="0,1">
<item row="5" column="1">
<widget class="QLabel" name="label_space2_13">
<property name="text">
<string>Minimum detection range (0=disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="ArucoVarianceAngular">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>6</number>
</property>
<property name="minimum">
<double>0.000001000000000</double>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.001000000000000</double>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_space2_15">
<property name="text">
<string>World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by a vertical line (&quot;id1 x y z roll pitch yaw|id2 x y z roll pitch yaw&quot;). Example: &quot;1 0 0 1 0 0 0|2 1 0 1 0 0 1.57&quot; (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLineEdit" name="ArucoMarkerPriors">
<property name="placeholderText">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="RGBDMarkerDetection">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="ArucoMarkerRangeMax">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_space2_6">
<property name="text">
<string>Linear variance to set on marker detections.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_space2_9">
<property name="text">
@@ -13342,6 +13439,105 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="ArucoMaxDepthError">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000100000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="ArucoMarkerLength">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>-1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_space2_16">
<property name="text">
<string>Linear variance to set on marker priors.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="ArucoMarkerRangeMin">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_space2_8">
<property name="text">
<string>Maximum depth error between all corners of a marker when estimating the marker length (when marker length above is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_space2_14">
<property name="text">
<string>Maximum detection range (0=unlimited).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="ArucoVarianceLinear">
<property name="suffix">
@@ -13374,19 +13570,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_space2_6">
<property name="text">
<string>Linear variance to set on marker detections.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_space2_7">
<property name="text">
@@ -13400,15 +13583,40 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="RGBDMarkerDetection">
<item row="9" column="1">
<widget class="QLabel" name="label_space2_17">
<property name="text">
<string/>
<string>Angular variance to set on marker priors.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="ArucoVarianceAngular">
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="ArucoPriorsVarianceLinear">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>6</number>
</property>
<property name="minimum">
<double>0.000001000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.001000000000000</double>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="ArucoPriorsVarianceAngular">
<property name="suffix">
<string/>
</property>
@@ -13429,127 +13637,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="ArucoMarkerLength">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>-1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_space2_8">
<property name="text">
<string>Maximum depth error between all corners of a marker when estimating the marker length (when marker length above is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="ArucoMaxDepthError">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000100000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_space2_13">
<property name="text">
<string>Minimum detection range (0=disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_space2_14">
<property name="text">
<string>Maximum detection range (0=unlimited).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="ArucoMarkerRangeMin">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="ArucoMarkerRangeMax">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>