44 noiseModel::Diagonal::Sigmas(
Vector1(0));
46 noiseModel::Diagonal::Variances(
Vector3(1
e-6, 1
e-6, 1
e-8));
61 Key key_child = nodeKey;
65 if (
tree.at(key_child) == key_child)
68 nodeTheta += deltaThetaMap.at(key_child);
70 key_parent =
tree.at(key_child);
72 if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) {
73 nodeTheta += thetaFromRootMap.at(key_parent);
76 key_child = key_parent;
91 for(
const auto& [nodeKey,
_]: deltaThetaMap) {
95 thetaToRootMap.emplace(nodeKey, nodeTheta);
97 return thetaToRootMap;
102 vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
109 for(
const std::shared_ptr<NonlinearFactor>& factor:
g) {
110 if (factor->keys().size() == 2) {
114 std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
115 std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
119 double deltaTheta = pose2Between->measured().theta();
123 deltaThetaMap.insert(pair<Key, double>(
key1, -deltaTheta));
126 deltaThetaMap.insert(pair<Key, double>(
key2, deltaTheta));
131 spanningTreeIds.push_back(
id);
134 chordsIds.push_back(
id);
146 std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
147 std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
149 throw invalid_argument(
150 "buildLinearOrientationGraph: invalid between factor!");
151 deltaTheta = (
Vector(1) << pose2Between->measured().theta()).finished();
155 std::shared_ptr<noiseModel::Diagonal> diagonalModel =
156 std::dynamic_pointer_cast<noiseModel::Diagonal>(
model);
158 throw invalid_argument(
"buildLinearOrientationGraph: invalid noise model "
159 "(current version assumes diagonal noise model)!");
160 Vector std_deltaTheta = (
Vector(1) << diagonalModel->sigma(2)).finished();
161 model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
166 const vector<size_t>& spanningTreeIds,
const vector<size_t>& chordsIds,
175 for(
const size_t& factorId: spanningTreeIds) {
179 lagoGraph.
add(
key1, -
I,
key2,
I, deltaTheta, model_deltaTheta);
182 for(
const size_t& factorId: chordsIds) {
186 double key1_DeltaTheta_key2 = deltaTheta(0);
188 double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(
key1)
189 - orientationsToRoot.at(
key2);
193 << key1_DeltaTheta_key2 - 2 * k *
M_PI).finished();
194 lagoGraph.
add(
key1, -
I,
key2,
I, deltaThetaRegularized, model_deltaTheta);
207 bool minUnassigned =
true;
209 for(
const std::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
215 minUnassigned =
false;
232 const auto edgeWeights = std::vector<double>(pose2Graph.
size(), 1.0);
233 const auto mstEdgeIndices =
240 std::map<Key, bool> visitationMap;
241 std::stack<std::pair<Key, Key>>
stack;
244 while (!
stack.empty()) {
245 auto [u, parent] =
stack.top();
247 if (visitationMap[u])
continue;
248 visitationMap[u] =
true;
249 predecessorMap[u] = parent;
250 for (
const auto& edgeIdx : mstEdgeIndices) {
251 const auto v = pose2Graph[edgeIdx]->
front();
252 const auto w = pose2Graph[edgeIdx]->
back();
253 if ((
v == u ||
w == u) && !visitationMap[
v == u ?
w :
v]) {
259 return predecessorMap;
265 bool useOdometricPath) {
266 gttic(lago_computeOrientations);
270 if (useOdometricPath)
288 spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot,
tree);
293 return orientationsLago;
298 bool useOdometricPath) {
310 gttic(lago_computePoses);
316 for(
const std::shared_ptr<NonlinearFactor>& factor: pose2graph) {
318 std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
319 std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
322 Key key1 = pose2Between->keys()[0];
323 double theta1 = orientationsLago.
at(
key1)(0);
324 double s1 =
sin(theta1);
325 double c1 =
cos(theta1);
327 Key key2 = pose2Between->keys()[1];
328 double theta2 = orientationsLago.
at(
key2)(0);
330 double linearDeltaRot = theta2 - theta1
331 - pose2Between->measured().theta();
332 linearDeltaRot =
Rot2(linearDeltaRot).
theta();
334 double dx = pose2Between->measured().x();
335 double dy = pose2Between->measured().y();
338 (
Vector(2) <<
c1 * dx - s1 * dy, s1 * dx +
c1 * dy).finished();
339 Vector b = (
Vector(3) << globalDeltaCart, linearDeltaRot).finished();
341 J1(0, 2) = s1 * dx +
c1 * dy;
342 J1(1, 2) = -
c1 * dx + s1 * dy;
344 std::shared_ptr<noiseModel::Diagonal> diagonalModel =
345 std::dynamic_pointer_cast<noiseModel::Diagonal>(
346 pose2Between->noiseModel());
350 throw invalid_argument(
351 "computeLagoPoses: cannot manage non between factor here!");
365 const Vector& poseVector = it.second;
366 Pose2 poseLago =
Pose2(poseVector(0), poseVector(1),
367 orientationsLago.
at(
key)(0) + poseVector(2));
371 return initialGuessLago;
376 gttic(lago_initialize);
392 const Values& initialGuess) {
403 const Vector& orientation = it.second;
408 return initialGuessLago;