Switched to struct with static methods as apparently global methods in namespaces are not wrapped.
Frank Dellaert
parent
3a371a1cf2
commit
e2cf42773a
33
gtsam.h
33
gtsam.h
|
|
@ -2524,21 +2524,24 @@ class BetweenFactorPose3s
|
|||
};
|
||||
|
||||
#include <gtsam/slam/InitializePose3.h>
|
||||
namespace initialize_pose3 {
|
||||
gtsam::Values computeOrientationsChordal(
|
||||
const gtsam::NonlinearFactorGraph& pose3Graph);
|
||||
gtsam::Values computeOrientationsGradient(
|
||||
const gtsam::NonlinearFactorGraph& pose3Graph,
|
||||
const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
|
||||
gtsam::Values computeOrientationsGradient(
|
||||
const gtsam::NonlinearFactorGraph& pose3Graph,
|
||||
const gtsam::Values& givenGuess);
|
||||
gtsam::NonlinearFactorGraph buildPose3graph(
|
||||
const gtsam::NonlinearFactorGraph& graph);
|
||||
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
|
||||
const gtsam::Values& givenGuess, bool useGradient);
|
||||
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
|
||||
} // namespace initialize_pose3
|
||||
class InitializePose3 {
|
||||
static gtsam::Values computeOrientationsChordal(
|
||||
const gtsam::NonlinearFactorGraph& pose3Graph);
|
||||
static gtsam::Values computeOrientationsGradient(
|
||||
const gtsam::NonlinearFactorGraph& pose3Graph,
|
||||
const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
|
||||
static gtsam::Values computeOrientationsGradient(
|
||||
const gtsam::NonlinearFactorGraph& pose3Graph,
|
||||
const gtsam::Values& givenGuess);
|
||||
static gtsam::NonlinearFactorGraph buildPose3graph(
|
||||
const gtsam::NonlinearFactorGraph& graph);
|
||||
static gtsam::Values initializeOrientations(
|
||||
const gtsam::NonlinearFactorGraph& graph);
|
||||
static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
|
||||
const gtsam::Values& givenGuess,
|
||||
bool useGradient);
|
||||
static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
|
||||
};
|
||||
|
||||
gtsam::BetweenFactorPose3s parse3DFactors(string filename);
|
||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
|
||||
|
|
|
|||
|
|
@ -29,12 +29,11 @@
|
|||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
namespace initialize_pose3 {
|
||||
|
||||
static const Key kAnchorKey = symbol('Z', 9999999);
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
|
||||
GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
|
||||
|
||||
GaussianFactorGraph linearGraph;
|
||||
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
|
||||
|
|
@ -67,7 +66,8 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Transform VectorValues into valid Rot3
|
||||
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
|
||||
Values InitializePose3::normalizeRelaxedRotations(
|
||||
const VectorValues& relaxedRot3) {
|
||||
gttic(InitializePose3_computeOrientationsChordal);
|
||||
|
||||
Matrix ppm = Z_3x3; // plus plus minus
|
||||
|
|
@ -94,7 +94,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
|
||||
NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) {
|
||||
gttic(InitializePose3_buildPose3graph);
|
||||
NonlinearFactorGraph pose3Graph;
|
||||
|
||||
|
|
@ -115,7 +115,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
|
||||
Values InitializePose3::computeOrientationsChordal(
|
||||
const NonlinearFactorGraph& pose3Graph) {
|
||||
gttic(InitializePose3_computeOrientationsChordal);
|
||||
|
||||
// regularize measurements and plug everything in a factor graph
|
||||
|
|
@ -129,10 +130,9 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
|
||||
const Values& givenGuess,
|
||||
const size_t maxIter,
|
||||
const bool setRefFrame) {
|
||||
Values InitializePose3::computeOrientationsGradient(
|
||||
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
|
||||
const size_t maxIter, const bool setRefFrame) {
|
||||
gttic(InitializePose3_computeOrientationsGradient);
|
||||
|
||||
// this works on the inverse rotations, according to Tron&Vidal,2011
|
||||
|
|
@ -231,7 +231,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
|
||||
void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
|
||||
const NonlinearFactorGraph& pose3Graph) {
|
||||
size_t factorId = 0;
|
||||
for(const auto& factor: pose3Graph) {
|
||||
|
|
@ -264,7 +264,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
|
||||
Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
|
||||
Vector3 logRot = Rot3::Logmap(R1.between(R2));
|
||||
|
||||
double th = logRot.norm();
|
||||
|
|
@ -286,8 +286,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values initializeOrientations(const NonlinearFactorGraph& graph) {
|
||||
|
||||
Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) {
|
||||
// We "extract" the Pose3 subgraph of the original graph: this
|
||||
// is done to properly model priors and avoiding operating on a larger graph
|
||||
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
|
||||
|
|
@ -297,7 +296,7 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) {
|
|||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
|
||||
Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
|
||||
gttic(InitializePose3_computePoses);
|
||||
|
||||
// put into Values structure
|
||||
|
|
@ -339,7 +338,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
|
||||
Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
|
||||
bool useGradient) {
|
||||
gttic(InitializePose3_initialize);
|
||||
Values initialValues;
|
||||
|
|
@ -360,9 +359,8 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Values initialize(const NonlinearFactorGraph& graph) {
|
||||
Values InitializePose3::initialize(const NonlinearFactorGraph& graph) {
|
||||
return initialize(graph, Values(), false);
|
||||
}
|
||||
|
||||
} // namespace initialize_pose3
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -31,54 +31,57 @@ namespace gtsam {
|
|||
typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
|
||||
typedef std::map<Key, Rot3> KeyRotMap;
|
||||
|
||||
namespace initialize_pose3 {
|
||||
struct GTSAM_EXPORT InitializePose3 {
|
||||
static GaussianFactorGraph buildLinearOrientationGraph(
|
||||
const NonlinearFactorGraph& g);
|
||||
|
||||
GTSAM_EXPORT GaussianFactorGraph
|
||||
buildLinearOrientationGraph(const NonlinearFactorGraph& g);
|
||||
static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
|
||||
|
||||
GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
|
||||
/**
|
||||
* Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||
*/
|
||||
static Values computeOrientationsChordal(
|
||||
const NonlinearFactorGraph& pose3Graph);
|
||||
|
||||
/**
|
||||
* Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||
*/
|
||||
GTSAM_EXPORT Values
|
||||
computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
|
||||
/**
|
||||
* Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||
*/
|
||||
static Values computeOrientationsGradient(
|
||||
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
|
||||
size_t maxIter = 10000, const bool setRefFrame = true);
|
||||
|
||||
/**
|
||||
* Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||
*/
|
||||
GTSAM_EXPORT Values computeOrientationsGradient(
|
||||
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
|
||||
size_t maxIter = 10000, const bool setRefFrame = true);
|
||||
static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
|
||||
KeyRotMap& factorId2RotMap,
|
||||
const NonlinearFactorGraph& pose3Graph);
|
||||
|
||||
GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
|
||||
KeyRotMap& factorId2RotMap,
|
||||
const NonlinearFactorGraph& pose3Graph);
|
||||
static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
|
||||
const double b);
|
||||
|
||||
GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2,
|
||||
const double a, const double b);
|
||||
/**
|
||||
* Select the subgraph of betweenFactors and transforms priors into between
|
||||
* wrt a fictitious node
|
||||
*/
|
||||
static NonlinearFactorGraph buildPose3graph(
|
||||
const NonlinearFactorGraph& graph);
|
||||
|
||||
/**
|
||||
* Select the subgraph of betweenFactors and transforms priors into between wrt
|
||||
* a fictitious node
|
||||
*/
|
||||
GTSAM_EXPORT NonlinearFactorGraph
|
||||
buildPose3graph(const NonlinearFactorGraph& graph);
|
||||
static Values computePoses(NonlinearFactorGraph& pose3graph,
|
||||
Values& initialRot);
|
||||
|
||||
GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph,
|
||||
Values& initialRot);
|
||||
/**
|
||||
* "extract" the Pose3 subgraph of the original graph, get orientations from
|
||||
* relative orientation measurements using chordal method.
|
||||
*/
|
||||
static Values initializeOrientations(const NonlinearFactorGraph& graph);
|
||||
|
||||
/**
|
||||
* "extract" the Pose3 subgraph of the original graph, get orientations from
|
||||
* relative orientation measurements (using either gradient or chordal method),
|
||||
* and finish up with 1 GN iteration on full poses.
|
||||
*/
|
||||
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
|
||||
const Values& givenGuess,
|
||||
bool useGradient = false);
|
||||
/**
|
||||
* "extract" the Pose3 subgraph of the original graph, get orientations from
|
||||
* relative orientation measurements (using either gradient or chordal
|
||||
* method), and finish up with 1 GN iteration on full poses.
|
||||
*/
|
||||
static Values initialize(const NonlinearFactorGraph& graph,
|
||||
const Values& givenGuess, bool useGradient = false);
|
||||
|
||||
/// Calls initialize above using Chordal method.
|
||||
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
|
||||
|
||||
} // namespace initialize_pose3
|
||||
/// Calls initialize above using Chordal method.
|
||||
static Values initialize(const NonlinearFactorGraph& graph);
|
||||
};
|
||||
} // end of namespace gtsam
|
||||
|
|
|
|||
|
|
@ -74,15 +74,15 @@ NonlinearFactorGraph graph() {
|
|||
|
||||
/* *************************************************************************** */
|
||||
TEST( InitializePose3, buildPose3graph ) {
|
||||
NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph());
|
||||
NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph());
|
||||
// pose3graph.print("");
|
||||
}
|
||||
|
||||
/* *************************************************************************** */
|
||||
TEST( InitializePose3, orientations ) {
|
||||
NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
|
||||
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||
|
||||
Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph);
|
||||
Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
|
||||
|
||||
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
|
||||
EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
|
||||
|
|
@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) {
|
|||
|
||||
/* *************************************************************************** */
|
||||
TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
|
||||
NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
|
||||
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||
|
||||
KeyVectorMap adjEdgesMap;
|
||||
KeyRotMap factorId2RotMap;
|
||||
|
||||
initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
|
||||
InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
|
||||
|
||||
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
|
||||
|
|
@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) {
|
|||
double a = 6.010534238540223;
|
||||
double b = 1.0;
|
||||
|
||||
Vector actual = initialize_pose3::gradientTron(R1, R2, a, b);
|
||||
Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
|
||||
Vector expected = Vector3::Zero();
|
||||
expected(2) = 1.962658662803917;
|
||||
|
||||
|
|
@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) {
|
|||
|
||||
/* *************************************************************************** */
|
||||
TEST( InitializePose3, iterationGradient ) {
|
||||
NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
|
||||
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||
|
||||
// Wrong initial guess - initialization should fix the rotations
|
||||
Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
|
||||
|
|
@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) {
|
|||
|
||||
size_t maxIter = 1; // test gradient at the first iteration
|
||||
bool setRefFrame = false;
|
||||
Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
|
||||
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
|
||||
|
||||
Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281,
|
||||
0.033572116359134, 0.999436104312325, -0.000621610948719,
|
||||
|
|
@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) {
|
|||
|
||||
/* *************************************************************************** */
|
||||
TEST( InitializePose3, orientationsGradient ) {
|
||||
NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
|
||||
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||
|
||||
// Wrong initial guess - initialization should fix the rotations
|
||||
Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
|
||||
|
|
@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) {
|
|||
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
|
||||
// do 10 gradient iterations
|
||||
bool setRefFrame = false;
|
||||
Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
|
||||
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
|
||||
|
||||
// const Key keyAnchor = symbol('Z', 9999999);
|
||||
// givenPoses.insert(keyAnchor,simple::pose0);
|
||||
|
|
@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) {
|
|||
givenPoses.insert(x2,simple::pose2);
|
||||
givenPoses.insert(x3,simple::pose3);
|
||||
|
||||
Values initial = initialize_pose3::initialize(simple::graph(), givenPoses);
|
||||
Values initial = InitializePose3::initialize(simple::graph(), givenPoses);
|
||||
|
||||
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
|
||||
EXPECT(assert_equal(givenPoses, initial, 1e-6));
|
||||
|
|
@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses )
|
|||
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
|
||||
inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
|
||||
|
||||
Values initial = initialize_pose3::initialize(*inputGraph);
|
||||
Values initial = InitializePose3::initialize(*inputGraph);
|
||||
EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !!
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue