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

Adding support for AprilTag v3 library (#1702)

Browse Source 
* Adding support for AprilTag v3 library

* New parameter: Marker/Strategy (default opencv-aruco as before). Optimized multicameras marker detection (do only once with stitched image)

* Exposed all AprilTag parameters. Sharing Marker/Dictionary between opencv and apriltag implementation. Added new Marker/Lengths parameter.

* Support AprilTAg library not built with aruco

* cleanup

* Added marker range support

* Make apriltag detection in same orientation than opencv

* fixed shenanigans when rendering markers with multi cameras

* Don't update odom cache (localization mode) when not moving (detecting loop/landmark or not)

* fixed some quirks (camera viewer + tag working)

* Setting quad decimate to 1 by default

* typo

* Implemented OptimizerG2O::loadGraph()

* added apriltag's aruco support info in the cmake config summary

* fixed warning

* fixed isam2 assert when using landmarks

* Fixed homography on multicam

* Adding marker detection time for convenience

* Added missing MIP 36h12 tag family in UI
This commit is contained in:
matlabbe
2026-05-26 17:43:34 -07:00
committed by GitHub
parent 49aa566613
commit 4756474367
23 changed files with 3561 additions and 2136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/src/CMakeLists.txt
View File

@@ -533,6 +533,13 @@ IF(FastCV_FOUND)
)
ENDIF(FastCV_FOUND)
IF(apriltag_FOUND)
SET(LIBRARIES
apriltag::apriltag
${LIBRARIES}
)
ENDIF(apriltag_FOUND)
IF(opengv_FOUND)
SET(LIBRARIES
${LIBRARIES}

9
corelib/src/Graph.cpp
View File

@@ -218,7 +218,14 @@ bool importPoses(
else if(format == 4) // g2o
{
std::multimap<int, Link> constraintsTmp;
UERROR("Cannot import from g2o format because it is not yet supported!");
if(OptimizerG2O::loadGraph(filePath, poses, constraintsTmp))
{
if(constraints)
{
*constraints = constraintsTmp;
}
return true;
}
return false;
}
else

961
corelib/src/MarkerDetector.cpp
View File

File diff suppressed because it is too large Load Diff

11
corelib/src/Parameters.cpp
View File

@@ -238,6 +238,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.23.7
removedParameters_.insert(std::make_pair("Marker/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerOpenCVCornerRefinementMethod())));
// 0.23.1
removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath", std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
@@ -290,7 +293,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("Aruco/MaxDepthError", std::make_pair(true, Parameters::kMarkerMaxDepthError())));
removedParameters_.insert(std::make_pair("Aruco/VarianceLinear", std::make_pair(true, Parameters::kMarkerVarianceLinear())));
removedParameters_.insert(std::make_pair("Aruco/VarianceAngular", std::make_pair(true, Parameters::kMarkerVarianceAngular())));
removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerCornerRefinementMethod())));
removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerOpenCVCornerRefinementMethod())));
// 0.17.5
removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr", std::make_pair(true, Parameters::kGridGlobalOccupancyThr())));
@@ -681,6 +684,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With AprilTag:";
#ifdef RTABMAP_APRILTAG
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With OpenGV:";
#ifdef RTABMAP_OPENGV

3
corelib/src/Rtabmap.cpp
View File

@@ -4504,8 +4504,7 @@ bool Rtabmap::process(
(smallDisplacement || tooFastMovement) &&
_loopClosureHypothesis.first == 0 &&
lastProximitySpaceClosureId == 0 &&
!delayedLocalization &&
(rejectedLoopClosure || landmarksDetected.empty()))
!delayedLocalization)
{
_odomCachePoses.erase(signatureRemoved);
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)

384
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -2135,6 +2135,390 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
return optimizedPoses;
}
bool OptimizerG2O::loadGraph(
const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, Link> & edgeConstraints)
{
FILE * file = 0;
#ifdef _MSC_VER
fopen_s(&file, fileName.c_str(), "r");
#else
file = fopen(fileName.c_str(), "r");
#endif
if(!file)
{
UERROR("Cannot open file %s", fileName.c_str());
return false;
}
// saveGraph() writes landmarks (originally negative ids, remapped to
// landmarkOffset - id) first in DESCENDING file-id order, then regular
// poses in ASCENDING file-id order. We recover landmarkOffset from this
// order to restore the original negative landmark ids.
struct VertexEntry {
int fileId;
Transform transform;
bool definitelyLandmark; // VERTEX_XY / VERTEX_TRACKXYZ
};
struct EdgeEntry {
int from;
int to;
Link::Type type;
Transform transform;
cv::Mat info;
bool isPrior; // from==to, prior on a single vertex
bool hasLandmarkEndpoint; // tag implies a landmark on one side
};
std::vector<VertexEntry> verticesList;
std::vector<EdgeEntry> edgesList;
char line[2048];
while(fgets(line, 2048, file) != NULL)
{
std::list<std::string> tokenList = uSplit(uReplaceChar(uReplaceChar(line, '\n', ' '), '\r', ' '), ' ');
std::vector<std::string> v;
v.reserve(tokenList.size());
for(std::list<std::string>::const_iterator iter = tokenList.begin(); iter != tokenList.end(); ++iter)
{
if(!iter->empty())
{
v.push_back(*iter);
}
}
if(v.empty())
{
continue;
}
const std::string & tag = v[0];
// Skip parameters, switch helpers and unrelated entries
if(tag == "PARAMS_SE2OFFSET" || tag == "PARAMS_SE3OFFSET" ||
tag == "VERTEX_SWITCH" || tag == "EDGE_SWITCH_PRIOR")
{
continue;
}
if(tag == "VERTEX_SE2" && v.size() == 5)
{
VertexEntry e;
e.fileId = atoi(v[1].c_str());
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]));
e.definitelyLandmark = false;
verticesList.push_back(e);
}
else if(tag == "VERTEX_XY" && v.size() == 4)
{
VertexEntry e;
e.fileId = atoi(v[1].c_str());
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0);
e.definitelyLandmark = true;
verticesList.push_back(e);
}
else if(tag == "VERTEX_SE3:QUAT" && v.size() == 9)
{
VertexEntry e;
e.fileId = atoi(v[1].c_str());
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]),
uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]));
e.definitelyLandmark = false;
verticesList.push_back(e);
}
else if(tag == "VERTEX_TRACKXYZ" && v.size() == 5)
{
VertexEntry e;
e.fileId = atoi(v[1].c_str());
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0);
e.definitelyLandmark = true;
verticesList.push_back(e);
}
else if(tag == "EDGE_SE2" && v.size() == 12)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[6]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[7]);
e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[8]);
e.info.at<double>(1, 1) = uStr2Double(v[9]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[10]);
e.info.at<double>(5, 5) = uStr2Double(v[11]);
e.type = Link::kUndef; // disambiguated after we know landmarkOffset
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE2_XY" && v.size() == 8)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), 0);
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[5]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
e.info.at<double>(1, 1) = uStr2Double(v[7]);
e.type = Link::kLandmark;
e.isPrior = false;
e.hasLandmarkEndpoint = true;
edgesList.push_back(e);
}
else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() == 31)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]),
uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
int idx = 10;
for(int r = 0; r < 6; ++r)
{
for(int c = r; c < 6; ++c)
{
e.info.at<double>(r, c) = uStr2Double(v[idx++]);
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
}
}
// EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph
bool landmarkTag = (tag == "EDGE_SE3");
e.type = landmarkTag ? Link::kLandmark : Link::kUndef;
e.isPrior = false;
e.hasLandmarkEndpoint = landmarkTag;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() == 13)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
// v[3] = param_offset id, ignored
e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), 0, 0, 0);
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[7]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[8]);
e.info.at<double>(0, 2) = e.info.at<double>(2, 0) = uStr2Double(v[9]);
e.info.at<double>(1, 1) = uStr2Double(v[10]);
e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[11]);
e.info.at<double>(2, 2) = uStr2Double(v[12]);
e.type = Link::kLandmark;
e.isPrior = false;
e.hasLandmarkEndpoint = true;
edgesList.push_back(e);
}
else if(tag == "EDGE_PRIOR_SE2" && v.size() == 11)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = e.from;
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[5]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[7]);
e.info.at<double>(1, 1) = uStr2Double(v[8]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[9]);
e.info.at<double>(5, 5) = uStr2Double(v[10]);
e.type = Link::kPosePrior;
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() == 7)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = e.from;
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0);
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[4]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[5]);
e.info.at<double>(1, 1) = uStr2Double(v[6]);
// no orientation info on this prior
e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
e.type = Link::kPosePrior;
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_PRIOR" && v.size() == 31)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = e.from;
// v[2] = param_offset id, ignored
e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]),
uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
int idx = 10;
for(int r = 0; r < 6; ++r)
{
for(int c = r; c < 6; ++c)
{
e.info.at<double>(r, c) = uStr2Double(v[idx++]);
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
}
}
e.type = Link::kPosePrior;
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() == 11)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = e.from;
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0);
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[5]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
e.info.at<double>(0, 2) = e.info.at<double>(2, 0) = uStr2Double(v[7]);
e.info.at<double>(1, 1) = uStr2Double(v[8]);
e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[9]);
e.info.at<double>(2, 2) = uStr2Double(v[10]);
// no orientation info on this prior
e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
e.type = Link::kPosePrior;
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() == 13)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
// v[3] = switch vertex id, ignored
e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[7]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[8]);
e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[9]);
e.info.at<double>(1, 1) = uStr2Double(v[10]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[11]);
e.info.at<double>(5, 5) = uStr2Double(v[12]);
e.type = Link::kUndef;
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() == 32)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
// v[3] = switch vertex id, ignored
e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]),
uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]), uStr2Float(v[10]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
int idx = 11;
for(int r = 0; r < 6; ++r)
{
for(int c = r; c < 6; ++c)
{
e.info.at<double>(r, c) = uStr2Double(v[idx++]);
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
}
}
e.type = Link::kUndef;
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else
{
UWARN("Unsupported or malformed g2o line: \"%s\" (tag=%s, tokens=%d)", line, tag.c_str(), (int)v.size());
}
}
fclose(file);
// Recover landmarkOffset from vertex order:
// file order = [landmarks with DESCENDING file_ids] + [regular poses with ASCENDING file_ids]
// Walk backwards from the end and take the longest ascending suffix as the regular poses.
// landmarkOffset = max regular pose id (= last fileId of that suffix).
int landmarkOffset = 0;
int firstRegularIdx = (int)verticesList.size();
if(!verticesList.empty())
{
firstRegularIdx = (int)verticesList.size() - 1;
while(firstRegularIdx > 0 &&
verticesList[firstRegularIdx - 1].fileId < verticesList[firstRegularIdx].fileId)
{
--firstRegularIdx;
}
landmarkOffset = verticesList.back().fileId;
// If the alleged regular suffix actually starts on a definite landmark
// (VERTEX_XY / VERTEX_TRACKXYZ), then there are no regular poses and
// saveGraph used landmarkOffset = 0; restore that case.
if(verticesList[firstRegularIdx].definitelyLandmark)
{
landmarkOffset = 0;
firstRegularIdx = (int)verticesList.size();
}
}
// Insert vertices into poses, remapping landmark file ids back to negative.
for(int i = 0; i < (int)verticesList.size(); ++i)
{
int originalId;
if(i < firstRegularIdx)
{
originalId = landmarkOffset - verticesList[i].fileId; // negative
}
else
{
originalId = verticesList[i].fileId;
}
if(poses.find(originalId) == poses.end())
{
poses.insert(std::make_pair(originalId, verticesList[i].transform));
}
else
{
UWARN("Vertex %d (file id %d) already exists, ignoring duplicate", originalId, verticesList[i].fileId);
}
}
// Remap edge endpoints. Any file id > landmarkOffset (or, if landmarkOffset == 0
// and there are any landmarks at all, any id present in the landmark prefix)
// is a landmark and gets the negative id back.
bool allLandmarks = (landmarkOffset == 0 && firstRegularIdx == (int)verticesList.size() && !verticesList.empty());
auto remap = [&](int fileId) -> int {
if(landmarkOffset > 0 && fileId > landmarkOffset)
{
return landmarkOffset - fileId; // negative
}
if(allLandmarks)
{
return -fileId;
}
return fileId;
};
for(const EdgeEntry & e : edgesList)
{
int from = remap(e.from);
int to = e.isPrior ? from : remap(e.to);
Link::Type type = e.type;
// Promote ambiguous edges (EDGE_SE2) to kLandmark when an endpoint
// turns out to be a landmark after remapping.
if(type == Link::kUndef && (from < 0 || to < 0))
{
type = Link::kLandmark;
}
edgeConstraints.insert(std::make_pair(from, Link(from, to, type, e.transform, e.info)));
}
UINFO("Graph loaded from %s (%d poses, %d edges, landmarkOffset=%d)",
fileName.c_str(), (int)poses.size(), (int)edgeConstraints.size(), landmarkOffset);
return true;
}
bool OptimizerG2O::saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,

47
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -239,6 +239,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
isam2_ = new gtsam::ISAM2(params);
addedPoses_.clear();
lastAddedConstraints_.clear();
isLandmarkWithRotation_.clear();
lastRootFactorIndex_.first = 0;
lastSwitchId_ = 1000000000;
}
@@ -308,7 +309,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)",
rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0);
gtsam::Values initialEstimate;
std::map<int, bool> isLandmarkWithRotation;
// In batch (non-iSAM2) mode each optimize() call is independent.
// In iSAM2 mode the map persists so we can resolve landmarks added
// in a previous incremental call but referenced by a new edge.
if(!isam2_)
{
isLandmarkWithRotation_.clear();
}
for(std::map<int, Transform>::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
@@ -328,12 +335,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
isLandmarkWithRotation_.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
isLandmarkWithRotation_.insert(std::make_pair(iter->first, true));
}
addedPoses_.insert(iter->first);
}
@@ -357,12 +364,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
isLandmarkWithRotation_.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
isLandmarkWithRotation_.insert(std::make_pair(iter->first, true));
}
addedPoses_.insert(iter->first);
}
@@ -390,7 +397,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(isSlam2d())
{
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
if(id1 < 0 && !isLandmarkWithRotation_.at(id1))
{
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
1/iter->second.infMatrix().at<double>(0,0),
@@ -429,7 +436,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else
{
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
if(id1 < 0 && !isLandmarkWithRotation_.at(id1))
{
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
iter->second.infMatrix().at<double>(0,0),
@@ -501,9 +508,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
UASSERT(isLandmarkWithRotation_.find(id2) != isLandmarkWithRotation_.end());
#ifdef RTABMAP_VERTIGO
if(this->isRobust() && isLandmarkWithRotation.at(id2))
if(this->isRobust() && isLandmarkWithRotation_.at(id2))
{
// create new switch variable
// Sunderhauf IROS 2012:
@@ -521,7 +528,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel));
}
else if(this->isRobust() && !isLandmarkWithRotation.at(id2))
else if(this->isRobust() && !isLandmarkWithRotation_.at(id2))
{
UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str());
}
@@ -529,7 +536,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
if(isSlam2d())
{
if(isLandmarkWithRotation.at(id2))
if(isLandmarkWithRotation_.at(id2))
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
@@ -592,7 +599,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else
{
if(isLandmarkWithRotation.at(id2))
if(isLandmarkWithRotation_.at(id2))
{
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
@@ -807,9 +814,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
}
else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
{
if(isLandmarkWithRotation.at(key))
if(isLandmarkWithRotation_.at(key))
{
newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
@@ -830,9 +837,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
}
else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
{
if(isLandmarkWithRotation.at(key))
if(isLandmarkWithRotation_.at(key))
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
@@ -1000,9 +1007,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
}
else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
{
if(isLandmarkWithRotation.at(key))
if(isLandmarkWithRotation_.at(key))
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
@@ -1023,9 +1030,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
}
else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
{
if(isLandmarkWithRotation.at(key))
if(isLandmarkWithRotation_.at(key))
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));