InitializePose3.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 
25 #include <gtsam/inference/Symbol.h>
26 #include <gtsam/geometry/Pose2.h>
27 #include <gtsam/geometry/Pose3.h>
28 #include <gtsam/base/timing.h>
29 
30 #include <utility>
31 
32 using namespace std;
33 
34 namespace gtsam {
35 
36 /* ************************************************************************* */
38 
39  GaussianFactorGraph linearGraph;
40 
41  for(const auto& factor: g) {
42  Matrix3 Rij;
43  double rotationPrecision = 1.0;
44 
45  auto pose3Between = std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
46  if (pose3Between){
47  Rij = pose3Between->measured().rotation().matrix();
48  Vector precisions = Vector::Zero(6);
49  precisions[0] = 1.0; // vector of all zeros except first entry equal to 1
50  pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable
51  rotationPrecision = precisions[0]; // rotations first
52  }else{
53  cout << "Error in buildLinearOrientationGraph" << endl;
54  }
55 
56  const auto& keys = factor->keys();
57  Key key1 = keys[0], key2 = keys[1];
58  Matrix M9 = Z_9x9;
59  M9.block(0,0,3,3) = Rij;
60  M9.block(3,3,3,3) = Rij;
61  M9.block(6,6,3,3) = Rij;
62  linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision));
63  }
64  // prior on the anchor orientation
65  linearGraph.add(
67  (Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0)
68  .finished(),
69  noiseModel::Isotropic::Precision(9, 1));
70  return linearGraph;
71 }
72 
73 /* ************************************************************************* */
74 // Transform VectorValues into valid Rot3
75 Values InitializePose3::normalizeRelaxedRotations(
76  const VectorValues& relaxedRot3) {
77  gttic(InitializePose3_computeOrientationsChordal);
78 
79  Values validRot3;
80  for(const auto& it: relaxedRot3) {
81  Key key = it.first;
82  if (key != initialize::kAnchorKey) {
83  Matrix3 M;
84  M << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
85 
86  // ClosestTo finds rotation matrix closest to H in Frobenius sense
87  Rot3 initRot = Rot3::ClosestTo(M.transpose());
88  validRot3.insert(key, initRot);
89  }
90  }
91  return validRot3;
92 }
93 
94 /* ************************************************************************* */
95 NonlinearFactorGraph InitializePose3::buildPose3graph(
96  const NonlinearFactorGraph& graph) {
97  gttic(InitializePose3_buildPose3graph);
98  return initialize::buildPoseGraph<Pose3>(graph);
99 }
100 
101 /* ************************************************************************* */
102 Values InitializePose3::computeOrientationsChordal(
103  const NonlinearFactorGraph& pose3Graph) {
104  gttic(InitializePose3_computeOrientationsChordal);
105 
106  // regularize measurements and plug everything in a factor graph
107  GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);
108 
109  // Solve the LFG
110  VectorValues relaxedRot3 = relaxedGraph.optimize();
111 
112  // normalize and compute Rot3
113  return normalizeRelaxedRotations(relaxedRot3);
114 }
115 
116 /* ************************************************************************* */
117 Values InitializePose3::computeOrientationsGradient(
118  const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
119  const size_t maxIter, const bool setRefFrame) {
120  gttic(InitializePose3_computeOrientationsGradient);
121 
122  // this works on the inverse rotations, according to Tron&Vidal,2011
123  std::map<Key,Rot3> inverseRot;
124  inverseRot.emplace(initialize::kAnchorKey, Rot3());
125  for(const auto& key_pose: givenGuess.extract<Pose3>()) {
126  const Key& key = key_pose.first;
127  const Pose3& pose = key_pose.second;
128  inverseRot.emplace(key, pose.rotation().inverse());
129  }
130 
131  // Create the map of edges incident on each node
132  KeyVectorMap adjEdgesMap;
133  KeyRotMap factorId2RotMap;
134 
135  createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
136 
137  // calculate max node degree & allocate gradient
138  size_t maxNodeDeg = 0;
139  for (const auto& key_R : inverseRot) {
140  const Key& key = key_R.first;
141  size_t currNodeDeg = (adjEdgesMap.at(key)).size();
142  if(currNodeDeg > maxNodeDeg)
143  maxNodeDeg = currNodeDeg;
144  }
145 
146  // Create parameters
147  double b = 1;
148  double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI);
149  double a = (M_PI*M_PI)/(2*f0);
150  double rho = 2*a*b;
151  double mu_max = maxNodeDeg * rho;
152  double stepsize = 2/mu_max; // = 1/(a b dG)
153 
154  double maxGrad;
155  // gradient iterations
156  size_t it;
157  for (it = 0; it < maxIter; it++) {
159  // compute the gradient at each node
160  maxGrad = 0;
161  VectorValues grad;
162  for (const auto& key_R : inverseRot) {
163  const Key& key = key_R.first;
164  const Rot3& Ri = key_R.second;
165  Vector gradKey = Z_3x1;
166  // collect the gradient for each edge incident on key
167  for (const size_t& factorId : adjEdgesMap.at(key)) {
168  const Rot3& Rij = factorId2RotMap.at(factorId);
169  auto factor = pose3Graph.at(factorId);
170  const auto& keys = factor->keys();
171  if (key == keys[0]) {
172  Key key1 = keys[1];
173  const Rot3& Rj = inverseRot.at(key1);
174  gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
175  } else if (key == keys[1]) {
176  Key key0 = keys[0];
177  const Rot3& Rj = inverseRot.at(key0);
178  gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
179  } else {
180  cout << "Error in gradient computation" << endl;
181  }
182  } // end of i-th gradient computation
183  grad.insert(key, stepsize * gradKey);
184 
185  double normGradKey = (gradKey).norm();
186  if(normGradKey>maxGrad)
187  maxGrad = normGradKey;
188  } // end of loop over nodes
189 
191  // update estimates
192  for (auto& key_R : inverseRot) {
193  const Key& key = key_R.first;
194  Rot3& Ri = key_R.second;
195  Ri = Ri.retract(grad.at(key));
196  }
197 
199  // check stopping condition
200  if (it>20 && maxGrad < 5e-3)
201  break;
202  } // enf of gradient iterations
203 
204  // Return correct rotations
205  const Rot3& Rref = inverseRot.at(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior
206  Values estimateRot;
207  for (const auto& key_R : inverseRot) {
208  const Key& key = key_R.first;
209  if (key != initialize::kAnchorKey) {
210  const Rot3& R = key_R.second;
211  if (setRefFrame)
212  estimateRot.insert(key, Rref.compose(R.inverse()));
213  else
214  estimateRot.insert(key, R.inverse());
215  }
216  }
217  return estimateRot;
218 }
219 
220 /* ************************************************************************* */
221 void InitializePose3::createSymbolicGraph(
222  const NonlinearFactorGraph& pose3Graph, KeyVectorMap* adjEdgesMap,
223  KeyRotMap* factorId2RotMap) {
224  size_t factorId = 0;
225  for (const auto& factor : pose3Graph) {
226  auto pose3Between =
227  std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
228  if (pose3Between) {
229  Rot3 Rij = pose3Between->measured().rotation();
230  factorId2RotMap->emplace(factorId, Rij);
231 
232  Key key1 = pose3Between->key<1>();
233  if (adjEdgesMap->find(key1) != adjEdgesMap->end()) { // key is already in
234  adjEdgesMap->at(key1).push_back(factorId);
235  } else {
236  vector<size_t> edge_id;
237  edge_id.push_back(factorId);
238  adjEdgesMap->emplace(key1, edge_id);
239  }
240  Key key2 = pose3Between->key<2>();
241  if (adjEdgesMap->find(key2) != adjEdgesMap->end()) { // key is already in
242  adjEdgesMap->at(key2).push_back(factorId);
243  } else {
244  vector<size_t> edge_id;
245  edge_id.push_back(factorId);
246  adjEdgesMap->emplace(key2, edge_id);
247  }
248  } else {
249  cout << "Error in createSymbolicGraph" << endl;
250  }
251  factorId++;
252  }
253 }
254 
255 /* ************************************************************************* */
256 Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
257  Vector3 logRot = Rot3::Logmap(R1.between(R2));
258 
259  double th = logRot.norm();
260  if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
261  Rot3 R1pert = R1.compose( Rot3::Expmap(Vector3(0.01, 0.01, 0.01)) ); // some perturbation
262  logRot = Rot3::Logmap(R1pert.between(R2));
263  th = logRot.norm();
264  }
265  // exclude small or invalid rotations
266  if (th > 1e-5 && th == th) { // nonzero valid rotations
267  logRot = logRot / th;
268  } else {
269  logRot = Z_3x1;
270  th = 0.0;
271  }
272 
273  double fdot = a*b*th*exp(-b*th);
274  return fdot*logRot;
275 }
276 
277 /* ************************************************************************* */
279  // We "extract" the Pose3 subgraph of the original graph: this
280  // is done to properly model priors and avoiding operating on a larger graph
281  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
282 
283  // Get orientations from relative orientation measurements
284  return computeOrientationsChordal(pose3Graph);
285 }
286 
289  NonlinearFactorGraph* posegraph,
290  bool singleIter) {
291  gttic(InitializePose3_computePoses);
292  return initialize::computePoses<Pose3>(initialRot, posegraph, singleIter);
293 }
294 
295 /* ************************************************************************* */
297  const Values& givenGuess, bool useGradient) {
298  gttic(InitializePose3_initialize);
299  Values initialValues;
300 
301  // We "extract" the Pose3 subgraph of the original graph: this
302  // is done to properly model priors and avoiding operating on a larger graph
303  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
304 
305  // Get orientations from relative orientation measurements
306  Values orientations;
307  if (useGradient)
308  orientations = computeOrientationsGradient(pose3Graph, givenGuess);
309  else
310  orientations = computeOrientationsChordal(pose3Graph);
311 
312  // Compute the full poses (1 GN iteration on full poses)
313  return computePoses(orientations, &pose3Graph);
314 }
315 
316 /* ************************************************************************* */
318  return initialize(graph, Values(), false);
319 }
320 
321 } // namespace gtsam
Initialize Pose3 in a factor graph.
const gtsam::Symbol key('X', 0)
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
Scalar * b
Definition: benchVecAdd.cpp:17
Eigen::Vector3d Vector3
Definition: Vector.h:43
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
common code between lago.* (2D) and InitializePose3.* (3D)
#define M_PI
Definition: main.h:106
Rot2 R(Rot2::fromAngle(0.1))
Pose2_ Expmap(const Vector3_ &xi)
Definition: BFloat16.h:88
std::map< Key, Rot3 > KeyRotMap
iterator insert(const std::pair< Key, Vector > &key_value)
Vector & at(Key j)
Definition: VectorValues.h:139
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static Values computePoses(const Values &initialRot, NonlinearFactorGraph *posegraph, bool singleIter=true)
void g(const string &key, int i)
Definition: testBTree.cpp:41
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
EIGEN_DEVICE_FUNC const ExpReturnType exp() const
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
std::map< Key, std::vector< size_t > > KeyVectorMap
#define gttic(label)
Definition: timing.h:295
void add(const GaussianFactor &factor)
Eigen::VectorXd Vector
Definition: Vector.h:38
const Symbol key1('v', 1)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
VectorValues initializeOrientations(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:297
Class compose(const Class &g) const
Definition: Lie.h:48
traits
Definition: chartTesting.h:28
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
std::vector< float > Values
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
GaussianFactorGraph buildLinearOrientationGraph(const vector< size_t > &spanningTreeIds, const vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap &tree)
Definition: lago.cpp:165
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
Class between(const Class &g) const
Definition: Lie.h:52
static constexpr Key kAnchorKey
void insert(Key j, const Value &val)
Definition: Values.cpp:155
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
const KeyVector keys
2D Pose
3D Pose
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:375
const VALUE & measured() const
return the measurement
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Timing utilities.
const Symbol key2('v', 2)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:22