41 for(
const auto& factor: g) {
43 double rotationPrecision = 1.0;
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());
88 validRot3.
insert(key, initRot);
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;
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;
149 double a = (M_PI*
M_PI)/(2*f0);
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();
171 if (key ==
keys[0]) {
173 const Rot3& Rj = inverseRot.at(key1);
174 gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
175 }
else if (key ==
keys[1]) {
177 const Rot3& Rj = inverseRot.at(key0);
178 gradKey = gradKey + gradientTron(Ri, Rij.
between(Rj),
a,
b);
180 cout <<
"Error in gradient computation" << endl;
183 grad.
insert(key, stepsize * gradKey);
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) {
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);
Initialize Pose3 in a factor graph.
const gtsam::Symbol key('X', 0)
Matrix< RealScalar, Dynamic, Dynamic > M
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
common code between lago.* (2D) and InitializePose3.* (3D)
Rot2 R(Rot2::fromAngle(0.1))
Pose2_ Expmap(const Vector3_ &xi)
std::map< Key, Rot3 > KeyRotMap
iterator insert(const std::pair< Key, Vector > &key_value)
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
static Values computePoses(const Values &initialRot, NonlinearFactorGraph *posegraph, bool singleIter=true)
void g(const string &key, int i)
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Rot3 inverse() const
inverse of a rotation
EIGEN_DEVICE_FUNC const ExpReturnType exp() const
std::map< Key, std::vector< size_t > > KeyVectorMap
void add(const GaussianFactor &factor)
const Symbol key1('v', 1)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
VectorValues initializeOrientations(const NonlinearFactorGraph &graph, bool useOdometricPath)
Class compose(const Class &g) const
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
GaussianFactorGraph buildLinearOrientationGraph(const vector< size_t > &spanningTreeIds, const vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap &tree)
const sharedFactor at(size_t i) const
Class between(const Class &g) const
static constexpr Key kAnchorKey
void insert(Key j, const Value &val)
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
const VALUE & measured() const
return the measurement
std::uint64_t Key
Integer nonlinear key type.
const Symbol key2('v', 2)