Moved common code to InitializePose
Frank dellaert
parent
a3ac89b0fb
commit
0d655e35b4
|
|
@ -0,0 +1,99 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file InitializePose.h
|
||||
* @author Frank Dellaert
|
||||
* @date August, 2020
|
||||
* @brief common code between lago.* (2D) and InitializePose3.* (3D)
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
namespace initialize {
|
||||
|
||||
static constexpr Key kAnchorKey = 99999999;
|
||||
|
||||
/**
|
||||
* Select the subgraph of betweenFactors and transforms priors into between
|
||||
* wrt a fictitious node
|
||||
*/
|
||||
template <class Pose>
|
||||
static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) {
|
||||
NonlinearFactorGraph poseGraph;
|
||||
|
||||
for (const auto& factor : graph) {
|
||||
// recast to a between on Pose
|
||||
if (auto between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
|
||||
poseGraph.add(between);
|
||||
|
||||
// recast PriorFactor<Pose> to BetweenFactor<Pose>
|
||||
if (auto prior = boost::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
|
||||
poseGraph.emplace_shared<BetweenFactor<Pose> >(
|
||||
kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel());
|
||||
}
|
||||
return poseGraph;
|
||||
}
|
||||
|
||||
/**
|
||||
* Use Gauss-Newton optimizer to optimize for poses given rotation estimates
|
||||
*/
|
||||
template <class Pose>
|
||||
static Values computePoses(const Values& initialRot,
|
||||
NonlinearFactorGraph* posegraph,
|
||||
bool singleIter = true) {
|
||||
const auto origin = Pose().translation();
|
||||
|
||||
// Upgrade rotations to full poses
|
||||
Values initialPose;
|
||||
for (const auto& key_value : initialRot) {
|
||||
Key key = key_value.key;
|
||||
const auto& rot = initialRot.at<typename Pose::Rotation>(key);
|
||||
Pose initializedPose = Pose(rot, origin);
|
||||
initialPose.insert(key, initializedPose);
|
||||
}
|
||||
|
||||
// add prior on dummy node
|
||||
auto priorModel = noiseModel::Unit::Create(Pose::dimension);
|
||||
initialPose.insert(kAnchorKey, Pose());
|
||||
posegraph->emplace_shared<PriorFactor<Pose> >(kAnchorKey, Pose(), priorModel);
|
||||
|
||||
// Create optimizer
|
||||
GaussNewtonParams params;
|
||||
if (singleIter) {
|
||||
params.maxIterations = 1;
|
||||
} else {
|
||||
params.setVerbosity("TERMINATION");
|
||||
}
|
||||
GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
|
||||
Values GNresult = optimizer.optimize();
|
||||
|
||||
// put into Values structure
|
||||
Values estimate;
|
||||
for (const auto& key_value : GNresult) {
|
||||
Key key = key_value.key;
|
||||
if (key != kAnchorKey) {
|
||||
const Pose& pose = GNresult.at<Pose>(key);
|
||||
estimate.insert(key, pose);
|
||||
}
|
||||
}
|
||||
return estimate;
|
||||
}
|
||||
} // namespace initialize
|
||||
} // namespace gtsam
|
||||
|
|
@ -10,28 +10,31 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file InitializePose3.h
|
||||
* @file InitializePose3.cpp
|
||||
* @author Luca Carlone
|
||||
* @author Frank Dellaert
|
||||
* @date August, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/InitializePose3.h>
|
||||
|
||||
#include <gtsam/slam/InitializePose.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
#include <boost/math/special_functions.hpp>
|
||||
|
||||
#include <utility>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
static const Key kAnchorKey = symbol('Z', 9999999);
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
|
||||
|
||||
|
|
@ -62,7 +65,7 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear
|
|||
}
|
||||
// prior on the anchor orientation
|
||||
linearGraph.add(
|
||||
kAnchorKey, I_9x9,
|
||||
initialize::kAnchorKey, I_9x9,
|
||||
(Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0)
|
||||
.finished(),
|
||||
noiseModel::Isotropic::Precision(9, 1));
|
||||
|
|
@ -78,7 +81,7 @@ Values InitializePose3::normalizeRelaxedRotations(
|
|||
Values validRot3;
|
||||
for(const auto& it: relaxedRot3) {
|
||||
Key key = it.first;
|
||||
if (key != kAnchorKey) {
|
||||
if (key != initialize::kAnchorKey) {
|
||||
Matrix3 M;
|
||||
M << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
|
||||
|
||||
|
|
@ -91,24 +94,10 @@ Values InitializePose3::normalizeRelaxedRotations(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) {
|
||||
NonlinearFactorGraph InitializePose3::buildPose3graph(
|
||||
const NonlinearFactorGraph& graph) {
|
||||
gttic(InitializePose3_buildPose3graph);
|
||||
NonlinearFactorGraph pose3Graph;
|
||||
|
||||
for(const auto& factor: graph) {
|
||||
|
||||
// recast to a between on Pose3
|
||||
const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||
if (pose3Between)
|
||||
pose3Graph.add(pose3Between);
|
||||
|
||||
// recast PriorFactor<Pose3> to BetweenFactor<Pose3>
|
||||
const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
|
||||
if (pose3Prior)
|
||||
pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
|
||||
pose3Prior->prior(), pose3Prior->noiseModel());
|
||||
}
|
||||
return pose3Graph;
|
||||
return initialize::buildPoseGraph<Pose3>(graph);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -134,7 +123,7 @@ Values InitializePose3::computeOrientationsGradient(
|
|||
|
||||
// this works on the inverse rotations, according to Tron&Vidal,2011
|
||||
Values inverseRot;
|
||||
inverseRot.insert(kAnchorKey, Rot3());
|
||||
inverseRot.insert(initialize::kAnchorKey, Rot3());
|
||||
for(const auto& key_value: givenGuess) {
|
||||
Key key = key_value.key;
|
||||
const Pose3& pose = givenGuess.at<Pose3>(key);
|
||||
|
|
@ -145,7 +134,7 @@ Values InitializePose3::computeOrientationsGradient(
|
|||
KeyVectorMap adjEdgesMap;
|
||||
KeyRotMap factorId2RotMap;
|
||||
|
||||
createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
|
||||
createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
|
||||
|
||||
// calculate max node degree & allocate gradient
|
||||
size_t maxNodeDeg = 0;
|
||||
|
|
@ -212,11 +201,11 @@ Values InitializePose3::computeOrientationsGradient(
|
|||
} // enf of gradient iterations
|
||||
|
||||
// Return correct rotations
|
||||
const Rot3& Rref = inverseRot.at<Rot3>(kAnchorKey); // This will be set to the identity as so far we included no prior
|
||||
const Rot3& Rref = inverseRot.at<Rot3>(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior
|
||||
Values estimateRot;
|
||||
for(const auto& key_value: inverseRot) {
|
||||
Key key = key_value.key;
|
||||
if (key != kAnchorKey) {
|
||||
if (key != initialize::kAnchorKey) {
|
||||
const Rot3& R = inverseRot.at<Rot3>(key);
|
||||
if(setRefFrame)
|
||||
estimateRot.insert(key, Rref.compose(R.inverse()));
|
||||
|
|
@ -228,32 +217,34 @@ Values InitializePose3::computeOrientationsGradient(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
|
||||
const NonlinearFactorGraph& pose3Graph) {
|
||||
void InitializePose3::createSymbolicGraph(
|
||||
const NonlinearFactorGraph& pose3Graph, KeyVectorMap* adjEdgesMap,
|
||||
KeyRotMap* factorId2RotMap) {
|
||||
size_t factorId = 0;
|
||||
for(const auto& factor: pose3Graph) {
|
||||
auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||
if (pose3Between){
|
||||
for (const auto& factor : pose3Graph) {
|
||||
auto pose3Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||
if (pose3Between) {
|
||||
Rot3 Rij = pose3Between->measured().rotation();
|
||||
factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
|
||||
factorId2RotMap->emplace(factorId, Rij);
|
||||
|
||||
Key key1 = pose3Between->key1();
|
||||
if (adjEdgesMap.find(key1) != adjEdgesMap.end()){ // key is already in
|
||||
adjEdgesMap.at(key1).push_back(factorId);
|
||||
}else{
|
||||
if (adjEdgesMap->find(key1) != adjEdgesMap->end()) { // key is already in
|
||||
adjEdgesMap->at(key1).push_back(factorId);
|
||||
} else {
|
||||
vector<size_t> edge_id;
|
||||
edge_id.push_back(factorId);
|
||||
adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
|
||||
adjEdgesMap->emplace(key1, edge_id);
|
||||
}
|
||||
Key key2 = pose3Between->key2();
|
||||
if (adjEdgesMap.find(key2) != adjEdgesMap.end()){ // key is already in
|
||||
adjEdgesMap.at(key2).push_back(factorId);
|
||||
}else{
|
||||
if (adjEdgesMap->find(key2) != adjEdgesMap->end()) { // key is already in
|
||||
adjEdgesMap->at(key2).push_back(factorId);
|
||||
} else {
|
||||
vector<size_t> edge_id;
|
||||
edge_id.push_back(factorId);
|
||||
adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
|
||||
adjEdgesMap->emplace(key2, edge_id);
|
||||
}
|
||||
}else{
|
||||
} else {
|
||||
cout << "Error in createSymbolicGraph" << endl;
|
||||
}
|
||||
factorId++;
|
||||
|
|
@ -293,50 +284,16 @@ Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph
|
|||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
|
||||
Values InitializePose3::computePoses(const Values& initialRot,
|
||||
NonlinearFactorGraph* posegraph,
|
||||
bool singleIter) {
|
||||
gttic(InitializePose3_computePoses);
|
||||
|
||||
// put into Values structure
|
||||
Values initialPose;
|
||||
for (const auto& key_value : initialRot) {
|
||||
Key key = key_value.key;
|
||||
const Rot3& rot = initialRot.at<Rot3>(key);
|
||||
Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
|
||||
initialPose.insert(key, initializedPose);
|
||||
}
|
||||
|
||||
// add prior
|
||||
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
|
||||
initialPose.insert(kAnchorKey, Pose3());
|
||||
pose3graph.emplace_shared<PriorFactor<Pose3> >(kAnchorKey, Pose3(), priorModel);
|
||||
|
||||
// Create optimizer
|
||||
GaussNewtonParams params;
|
||||
bool singleIter = true;
|
||||
if (singleIter) {
|
||||
params.maxIterations = 1;
|
||||
} else {
|
||||
cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" << endl;
|
||||
params.setVerbosity("TERMINATION");
|
||||
}
|
||||
GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
|
||||
Values GNresult = optimizer.optimize();
|
||||
|
||||
// put into Values structure
|
||||
Values estimate;
|
||||
for (const auto& key_value : GNresult) {
|
||||
Key key = key_value.key;
|
||||
if (key != kAnchorKey) {
|
||||
const Pose3& pose = GNresult.at<Pose3>(key);
|
||||
estimate.insert(key, pose);
|
||||
}
|
||||
}
|
||||
return estimate;
|
||||
return initialize::computePoses<Pose3>(initialRot, posegraph, singleIter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
|
||||
bool useGradient) {
|
||||
Values InitializePose3::initialize(const NonlinearFactorGraph& graph,
|
||||
const Values& givenGuess, bool useGradient) {
|
||||
gttic(InitializePose3_initialize);
|
||||
Values initialValues;
|
||||
|
||||
|
|
@ -352,7 +309,7 @@ Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Valu
|
|||
orientations = computeOrientationsChordal(pose3Graph);
|
||||
|
||||
// Compute the full poses (1 GN iteration on full poses)
|
||||
return computePoses(pose3Graph, orientations);
|
||||
return computePoses(orientations, &pose3Graph);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -26,6 +26,9 @@
|
|||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
|
||||
|
|
@ -50,9 +53,9 @@ struct GTSAM_EXPORT InitializePose3 {
|
|||
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
|
||||
size_t maxIter = 10000, const bool setRefFrame = true);
|
||||
|
||||
static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
|
||||
KeyRotMap& factorId2RotMap,
|
||||
const NonlinearFactorGraph& pose3Graph);
|
||||
static void createSymbolicGraph(const NonlinearFactorGraph& pose3Graph,
|
||||
KeyVectorMap* adjEdgesMap,
|
||||
KeyRotMap* factorId2RotMap);
|
||||
|
||||
static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
|
||||
const double b);
|
||||
|
|
@ -64,8 +67,12 @@ struct GTSAM_EXPORT InitializePose3 {
|
|||
static NonlinearFactorGraph buildPose3graph(
|
||||
const NonlinearFactorGraph& graph);
|
||||
|
||||
static Values computePoses(NonlinearFactorGraph& pose3graph,
|
||||
Values& initialRot);
|
||||
/**
|
||||
* Use Gauss-Newton optimizer to optimize for poses given rotation estimates
|
||||
*/
|
||||
static Values computePoses(const Values& initialRot,
|
||||
NonlinearFactorGraph* poseGraph,
|
||||
bool singleIter = true);
|
||||
|
||||
/**
|
||||
* "extract" the Pose3 subgraph of the original graph, get orientations from
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/slam/lago.h>
|
||||
|
||||
#include <gtsam/slam/InitializePose.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
|
@ -33,7 +35,6 @@ namespace lago {
|
|||
static const Matrix I = I_1x1;
|
||||
static const Matrix I3 = I_3x3;
|
||||
|
||||
static const Key keyAnchor = symbol('Z', 9999999);
|
||||
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
|
||||
noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished());
|
||||
static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
|
||||
|
|
@ -79,7 +80,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
|
|||
key2doubleMap thetaToRootMap;
|
||||
|
||||
// Orientation of the roo
|
||||
thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
|
||||
thetaToRootMap.insert(pair<Key, double>(initialize::kAnchorKey, 0.0));
|
||||
|
||||
// for all nodes in the tree
|
||||
for(const key2doubleMap::value_type& it: deltaThetaMap) {
|
||||
|
|
@ -189,41 +190,16 @@ GaussianFactorGraph buildLinearOrientationGraph(
|
|||
lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
|
||||
}
|
||||
// prior on the anchor orientation
|
||||
lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
|
||||
lagoGraph.add(initialize::kAnchorKey, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
|
||||
return lagoGraph;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
|
||||
static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
|
||||
gttic(lago_buildPose2graph);
|
||||
NonlinearFactorGraph pose2Graph;
|
||||
|
||||
for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
|
||||
|
||||
// recast to a between on Pose2
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
|
||||
if (pose2Between)
|
||||
pose2Graph.add(pose2Between);
|
||||
|
||||
// recast PriorFactor<Pose2> to BetweenFactor<Pose2>
|
||||
boost::shared_ptr<PriorFactor<Pose2> > pose2Prior =
|
||||
boost::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
|
||||
if (pose2Prior)
|
||||
pose2Graph.add(
|
||||
BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
|
||||
pose2Prior->prior(), pose2Prior->noiseModel()));
|
||||
}
|
||||
return pose2Graph;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static PredecessorMap<Key> findOdometricPath(
|
||||
const NonlinearFactorGraph& pose2Graph) {
|
||||
|
||||
PredecessorMap<Key> tree;
|
||||
Key minKey = keyAnchor; // this initialization does not matter
|
||||
Key minKey = initialize::kAnchorKey; // this initialization does not matter
|
||||
bool minUnassigned = true;
|
||||
|
||||
for(const boost::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
|
||||
|
|
@ -240,8 +216,8 @@ static PredecessorMap<Key> findOdometricPath(
|
|||
minKey = key1;
|
||||
}
|
||||
}
|
||||
tree.insert(minKey, keyAnchor);
|
||||
tree.insert(keyAnchor, keyAnchor); // root
|
||||
tree.insert(minKey, initialize::kAnchorKey);
|
||||
tree.insert(initialize::kAnchorKey, initialize::kAnchorKey); // root
|
||||
return tree;
|
||||
}
|
||||
|
||||
|
|
@ -284,7 +260,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
|
|||
|
||||
// We "extract" the Pose2 subgraph of the original graph: this
|
||||
// is done to properly model priors and avoiding operating on a larger graph
|
||||
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
|
||||
NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
|
||||
|
||||
// Get orientations from relative orientation measurements
|
||||
return computeOrientations(pose2Graph, useOdometricPath);
|
||||
|
|
@ -338,7 +314,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
|
|||
}
|
||||
}
|
||||
// add prior
|
||||
linearPose2graph.add(keyAnchor, I3, Vector3(0.0, 0.0, 0.0),
|
||||
linearPose2graph.add(initialize::kAnchorKey, I3, Vector3(0.0, 0.0, 0.0),
|
||||
priorPose2Noise);
|
||||
|
||||
// optimize
|
||||
|
|
@ -348,7 +324,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
|
|||
Values initialGuessLago;
|
||||
for(const VectorValues::value_type& it: posesLago) {
|
||||
Key key = it.first;
|
||||
if (key != keyAnchor) {
|
||||
if (key != initialize::kAnchorKey) {
|
||||
const Vector& poseVector = it.second;
|
||||
Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
|
||||
orientationsLago.at(key)(0) + poseVector(2));
|
||||
|
|
@ -364,7 +340,7 @@ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
|
|||
|
||||
// We "extract" the Pose2 subgraph of the original graph: this
|
||||
// is done to properly model priors and avoiding operating on a larger graph
|
||||
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
|
||||
NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
|
||||
|
||||
// Get orientations from relative orientation measurements
|
||||
VectorValues orientationsLago = computeOrientations(pose2Graph,
|
||||
|
|
@ -385,7 +361,7 @@ Values initialize(const NonlinearFactorGraph& graph,
|
|||
// for all nodes in the tree
|
||||
for(const VectorValues::value_type& it: orientations) {
|
||||
Key key = it.first;
|
||||
if (key != keyAnchor) {
|
||||
if (key != initialize::kAnchorKey) {
|
||||
const Pose2& pose = initialGuess.at<Pose2>(key);
|
||||
const Vector& orientation = it.second;
|
||||
Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
|
||||
|
|
|
|||
|
|
@ -0,0 +1,82 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testInitializePose3.cpp
|
||||
* @brief Unit tests for 3D SLAM initialization, using rotation relaxation
|
||||
*
|
||||
* @author Luca Carlone
|
||||
* @author Frank Dellaert
|
||||
* @date August, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/InitializePose.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(InitializePose3, computePoses2D) {
|
||||
const string g2oFile = findExampleDataFile("noisyToyGraph.txt");
|
||||
NonlinearFactorGraph::shared_ptr inputGraph;
|
||||
Values::shared_ptr posesInFile;
|
||||
bool is3D = false;
|
||||
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
||||
|
||||
auto priorModel = noiseModel::Unit::Create(3);
|
||||
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
|
||||
|
||||
auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
|
||||
|
||||
auto I = genericValue(Rot3());
|
||||
Values orientations;
|
||||
for (size_t i : {0, 1, 2, 3})
|
||||
orientations.insert(i, posesInFile->at<Pose2>(i).rotation());
|
||||
const Values poses = initialize::computePoses<Pose2>(orientations, &poseGraph);
|
||||
|
||||
// posesInFile is seriously noisy, so we check error of recovered poses
|
||||
EXPECT_DOUBLES_EQUAL(0.0810283, inputGraph->error(poses), 1e-6);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(InitializePose3, computePoses3D) {
|
||||
const string g2oFile = findExampleDataFile("Klaus3");
|
||||
NonlinearFactorGraph::shared_ptr inputGraph;
|
||||
Values::shared_ptr posesInFile;
|
||||
bool is3D = true;
|
||||
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
||||
|
||||
auto priorModel = noiseModel::Unit::Create(6);
|
||||
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
|
||||
|
||||
auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
|
||||
|
||||
auto I = genericValue(Rot3());
|
||||
Values orientations;
|
||||
for (size_t i : {0, 1, 2})
|
||||
orientations.insert(i, posesInFile->at<Pose3>(i).rotation());
|
||||
Values poses = initialize::computePoses<Pose3>(orientations, &poseGraph);
|
||||
EXPECT(assert_equal(*posesInFile, poses, 0.1)); // TODO(frank): very loose !!
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -121,7 +121,7 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
|
|||
KeyVectorMap adjEdgesMap;
|
||||
KeyRotMap factorId2RotMap;
|
||||
|
||||
InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
|
||||
InitializePose3::createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
|
||||
|
||||
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
|
||||
|
|
@ -258,21 +258,21 @@ TEST( InitializePose3, posesWithGivenGuess ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InitializePose3, initializePoses )
|
||||
{
|
||||
TEST(InitializePose3, initializePoses) {
|
||||
const string g2oFile = findExampleDataFile("pose3example-grid");
|
||||
NonlinearFactorGraph::shared_ptr inputGraph;
|
||||
Values::shared_ptr expectedValues;
|
||||
Values::shared_ptr posesInFile;
|
||||
bool is3D = true;
|
||||
boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
|
||||
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
|
||||
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
|
||||
|
||||
auto priorModel = noiseModel::Unit::Create(6);
|
||||
inputGraph->addPrior(0, Pose3(), priorModel);
|
||||
|
||||
Values initial = InitializePose3::initialize(*inputGraph);
|
||||
EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !!
|
||||
EXPECT(assert_equal(*posesInFile, initial,
|
||||
0.1)); // TODO(frank): very loose !!
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue