41 for(
const auto& factor:
g) {
43 double rotationPrecision = 1.0;
45 auto pose3Between = std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
47 Rij = pose3Between->measured().rotation().matrix();
48 Vector precisions = Vector::Zero(6);
50 pose3Between->noiseModel()->whitenInPlace(precisions);
51 rotationPrecision = precisions[0];
53 cout <<
"Error in buildLinearOrientationGraph" << endl;
56 const auto&
keys = factor->keys();
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));
67 (
Vector(9) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)
69 noiseModel::Isotropic::Precision(9, 1));
75 Values InitializePose3::normalizeRelaxedRotations(
77 gttic(InitializePose3_computeOrientationsChordal);
80 for(
const auto& it: relaxedRot3) {
84 M << Eigen::Map<const Matrix3>(it.second.data());
87 Rot3 initRot = Rot3::ClosestTo(
M.transpose());
97 gttic(InitializePose3_buildPose3graph);
98 return initialize::buildPoseGraph<Pose3>(
graph);
102 Values InitializePose3::computeOrientationsChordal(
104 gttic(InitializePose3_computeOrientationsChordal);
113 return normalizeRelaxedRotations(relaxedRot3);
117 Values InitializePose3::computeOrientationsGradient(
119 const size_t maxIter,
const bool setRefFrame) {
120 gttic(InitializePose3_computeOrientationsGradient);
123 std::map<Key,Rot3> inverseRot;
125 for(
const auto& key_pose: givenGuess.
extract<
Pose3>()) {
126 const Key&
key = key_pose.first;
128 inverseRot.emplace(
key,
pose.rotation().inverse());
135 createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
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;
151 double mu_max = maxNodeDeg * rho;
152 double stepsize = 2/mu_max;
157 for (it = 0; it < maxIter; it++) {
162 for (
const auto& key_R : inverseRot) {
163 const Key&
key = key_R.first;
164 const Rot3& Ri = key_R.second;
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();
173 const Rot3& Rj = inverseRot.at(
key1);
174 gradKey = gradKey + gradientTron(Ri, Rij * Rj,
a,
b);
177 const Rot3& Rj = inverseRot.at(key0);
178 gradKey = gradKey + gradientTron(Ri, Rij.
between(Rj),
a,
b);
180 cout <<
"Error in gradient computation" << endl;
185 double normGradKey = (gradKey).norm();
186 if(normGradKey>maxGrad)
187 maxGrad = normGradKey;
192 for (
auto& key_R : inverseRot) {
193 const Key&
key = key_R.first;
194 Rot3& Ri = key_R.second;
200 if (it>20 && maxGrad < 5
e-3)
207 for (
const auto& key_R : inverseRot) {
208 const Key&
key = key_R.first;
210 const Rot3&
R = key_R.second;
221 void InitializePose3::createSymbolicGraph(
225 for (
const auto& factor : pose3Graph) {
227 std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
229 Rot3 Rij = pose3Between->measured().rotation();
230 factorId2RotMap->emplace(factorId, Rij);
232 Key key1 = pose3Between->key<1>();
233 if (adjEdgesMap->find(
key1) != adjEdgesMap->end()) {
234 adjEdgesMap->at(
key1).push_back(factorId);
236 vector<size_t> edge_id;
237 edge_id.push_back(factorId);
238 adjEdgesMap->emplace(
key1, edge_id);
240 Key key2 = pose3Between->key<2>();
241 if (adjEdgesMap->find(
key2) != adjEdgesMap->end()) {
242 adjEdgesMap->at(
key2).push_back(factorId);
244 vector<size_t> edge_id;
245 edge_id.push_back(factorId);
246 adjEdgesMap->emplace(
key2, edge_id);
249 cout <<
"Error in createSymbolicGraph" << endl;
259 double th = logRot.norm();
262 logRot = Rot3::Logmap(R1pert.
between(
R2));
266 if (th > 1
e-5 && th == th) {
267 logRot = logRot / th;
273 double fdot =
a*
b*th*
exp(-
b*th);
284 return computeOrientationsChordal(pose3Graph);
291 gttic(InitializePose3_computePoses);
292 return initialize::computePoses<Pose3>(initialRot, posegraph, singleIter);
297 const Values& givenGuess,
bool useGradient) {
298 gttic(InitializePose3_initialize);
308 orientations = computeOrientationsGradient(pose3Graph, givenGuess);
310 orientations = computeOrientationsChordal(pose3Graph);