mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Adding support for AprilTag v3 library (#1702)
* 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
@@ -57,6 +57,12 @@ private:
|
||||
};
|
||||
|
||||
class RTABMAP_CORE_EXPORT MarkerDetector {
|
||||
|
||||
public:
|
||||
enum Strategy {
|
||||
kStrategyOpencv,
|
||||
kStrategyApriltag
|
||||
};
|
||||
|
||||
public:
|
||||
MarkerDetector(const ParametersMap & parameters = ParametersMap());
|
||||
@@ -84,15 +90,19 @@ public:
|
||||
cv::Mat * imageWithDetections = 0);
|
||||
|
||||
private:
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
|
||||
float markerLength_;
|
||||
Strategy strategy_;
|
||||
float markerLength_;
|
||||
std::map<int, float> markerLengths_;
|
||||
float maxDepthError_;
|
||||
float maxRange_;
|
||||
float minRange_;
|
||||
int dictionaryId_;
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
|
||||
cv::Ptr<cv::aruco::Dictionary> dictionary_;
|
||||
#endif
|
||||
void * apriltagLibDetector_;
|
||||
void * apriltagLibFamily_;
|
||||
};
|
||||
|
||||
} /* namespace rtabmap */
|
||||
|
||||
@@ -917,19 +917,29 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(GridGlobal, ProbClampingMax, float, 0.971, "Probability clamping maximum (value between 0 and 1).");
|
||||
RTABMAP_PARAM(GridGlobal, FloodFillDepth, unsigned int, 0, "Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.");
|
||||
|
||||
RTABMAP_PARAM(Marker, Dictionary, int, 0, "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
|
||||
RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
|
||||
RTABMAP_PARAM(Marker, Strategy, int, 0, "Marker detection implementation: 0=OpenCV, 1=AprilTag");
|
||||
RTABMAP_PARAM(Marker, Dictionary, int, 0, "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20, DICT_ARUCO_MIP_36H12=21");
|
||||
RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. Value <=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). With <0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID.");
|
||||
RTABMAP_PARAM_STR(Marker, Lengths, "", uFormat("List of markers to detect. Format is the marker's ID followed by its length (in meters), multiple markers are separated by a vertical line (\"id1 length|id2 length\"). We can also define a range of markers with \"id1:id2 length\" (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on %s. For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set \"12 0.08|14 0.15|30:40 0.1\".", kMarkerLength().c_str()).c_str());
|
||||
RTABMAP_PARAM(Marker, MaxDepthError, float, 0.01, uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
|
||||
RTABMAP_PARAM(Marker, VarianceLinear, float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str()));
|
||||
RTABMAP_PARAM(Marker, VarianceAngular, float, 0.01, uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str()));
|
||||
RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str()));
|
||||
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_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_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), multiple 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(MarkerAprilTag, NThreads, int, 1, "How many threads should be used?");
|
||||
RTABMAP_PARAM(MarkerAprilTag, QuadDecimate, float, 1.0, "Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution.");
|
||||
RTABMAP_PARAM(MarkerAprilTag, QuadSigma, float, 0.0, "What Gaussian blur should be applied to the segmented image (used for quad detection?) Parameter is the standard deviation in pixels. Very noisy images benefit from non-zero values (e.g. 0.8).");
|
||||
RTABMAP_PARAM(MarkerAprilTag, RefineEdges, bool, true, uFormat("When true, the edges of the each quad are adjusted to \"snap to\" strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (true). Very computationally inexpensive. Option is ignored if %s = 1.", kMarkerAprilTagQuadDecimate().c_str()));
|
||||
RTABMAP_PARAM(MarkerAprilTag, DecodeSharpening, double, 0.25, "How much sharpening should be done to decoded images? This can help decode small tags but may or may not help in odd lighting conditions or low light conditions.");
|
||||
RTABMAP_PARAM(MarkerAprilTag, Debug, bool, false, uFormat("When true, write a variety of debugging images to the working directory where the app started (not %s) at various stages through the detection process. (Somewhat slow).", kRtabmapWorkingDirectory().c_str()));
|
||||
|
||||
RTABMAP_PARAM(MarkerOpenCV, CornerRefinementMethod, int, 0, "Corner refinement method for OpenCV strategy (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(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].");
|
||||
|
||||
|
||||
@@ -41,6 +41,12 @@ public:
|
||||
static bool isCSparseAvailable();
|
||||
static bool isCholmodAvailable();
|
||||
|
||||
public:
|
||||
static bool loadGraph(
|
||||
const std::string & fileName,
|
||||
std::map<int, Transform> & poses,
|
||||
std::multimap<int, Link> & edgeConstraints);
|
||||
|
||||
public:
|
||||
OptimizerG2O(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~OptimizerG2O() {}
|
||||
|
||||
@@ -77,6 +77,7 @@ private:
|
||||
std::vector<ConstraintToFactor> lastAddedConstraints_;
|
||||
int lastSwitchId_;
|
||||
std::set<int> addedPoses_;
|
||||
std::map<int, bool> isLandmarkWithRotation_; // persists across iSAM2 incremental calls
|
||||
std::pair<int, std::uint64_t> lastRootFactorIndex_;
|
||||
};
|
||||
|
||||
|
||||
@@ -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}
|
||||
|
||||
@@ -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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
|
||||
@@ -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();)
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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())));
|
||||
|
||||
Reference in New Issue
Block a user