12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Switched to struct with static methods as apparently global methods in namespaces are not wrapped.

release/4.3a0
Frank Dellaert 2019-03-19 17:04:31 -04:00
parent 3a371a1cf2
commit e2cf42773a
4 changed files with 88 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam.h
View File

@ -2524,21 +2524,24 @@ class BetweenFactorPose3s
}; };
#include <gtsam/slam/InitializePose3.h> #include <gtsam/slam/InitializePose3.h>
namespace initialize_pose3 { class InitializePose3 {
gtsam::Values computeOrientationsChordal( static gtsam::Values computeOrientationsChordal(
const gtsam::NonlinearFactorGraph& pose3Graph); const gtsam::NonlinearFactorGraph& pose3Graph);
gtsam::Values computeOrientationsGradient( static gtsam::Values computeOrientationsGradient(
const gtsam::NonlinearFactorGraph& pose3Graph, const gtsam::NonlinearFactorGraph& pose3Graph,
const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
gtsam::Values computeOrientationsGradient( static gtsam::Values computeOrientationsGradient(
const gtsam::NonlinearFactorGraph& pose3Graph, const gtsam::NonlinearFactorGraph& pose3Graph,
const gtsam::Values& givenGuess); const gtsam::Values& givenGuess);
gtsam::NonlinearFactorGraph buildPose3graph( static gtsam::NonlinearFactorGraph buildPose3graph(
const gtsam::NonlinearFactorGraph& graph); const gtsam::NonlinearFactorGraph& graph);
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, static gtsam::Values initializeOrientations(
const gtsam::Values& givenGuess, bool useGradient); const gtsam::NonlinearFactorGraph& graph);
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
} // namespace initialize_pose3 const gtsam::Values& givenGuess,
bool useGradient);
static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
};
gtsam::BetweenFactorPose3s parse3DFactors(string filename); gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename); pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);

32
gtsam/slam/InitializePose3.cpp
View File

@ -29,12 +29,11 @@
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
namespace initialize_pose3 {
static const Key kAnchorKey = symbol('Z', 9999999); static const Key kAnchorKey = symbol('Z', 9999999);
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
GaussianFactorGraph linearGraph; GaussianFactorGraph linearGraph;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
@ -67,7 +66,8 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
/* ************************************************************************* */ /* ************************************************************************* */
// Transform VectorValues into valid Rot3 // Transform VectorValues into valid Rot3
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { Values InitializePose3::normalizeRelaxedRotations(
const VectorValues& relaxedRot3) {
gttic(InitializePose3_computeOrientationsChordal); gttic(InitializePose3_computeOrientationsChordal);
Matrix ppm = Z_3x3; // plus plus minus 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); gttic(InitializePose3_buildPose3graph);
NonlinearFactorGraph pose3Graph; 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); gttic(InitializePose3_computeOrientationsChordal);
// regularize measurements and plug everything in a factor graph // regularize measurements and plug everything in a factor graph
@ -129,10 +130,9 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, Values InitializePose3::computeOrientationsGradient(
const Values& givenGuess, const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
const size_t maxIter, const size_t maxIter, const bool setRefFrame) {
const bool setRefFrame) {
gttic(InitializePose3_computeOrientationsGradient); gttic(InitializePose3_computeOrientationsGradient);
// this works on the inverse rotations, according to Tron&Vidal,2011 // 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) { const NonlinearFactorGraph& pose3Graph) {
size_t factorId = 0; size_t factorId = 0;
for(const auto& factor: pose3Graph) { 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)); Vector3 logRot = Rot3::Logmap(R1.between(R2));
double th = logRot.norm(); 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 // We "extract" the Pose3 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph // is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose3Graph = buildPose3graph(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); gttic(InitializePose3_computePoses);
// put into Values structure // 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) { bool useGradient) {
gttic(InitializePose3_initialize); gttic(InitializePose3_initialize);
Values initialValues; 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); return initialize(graph, Values(), false);
} }
} // namespace initialize_pose3
} // namespace gtsam } // namespace gtsam

83
gtsam/slam/InitializePose3.h
View File

@ -31,54 +31,57 @@ namespace gtsam {
typedef std::map<Key, std::vector<size_t> > KeyVectorMap; typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
typedef std::map<Key, Rot3> KeyRotMap; typedef std::map<Key, Rot3> KeyRotMap;
namespace initialize_pose3 { struct GTSAM_EXPORT InitializePose3 {
static GaussianFactorGraph buildLinearOrientationGraph(
const NonlinearFactorGraph& g);
GTSAM_EXPORT GaussianFactorGraph static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
buildLinearOrientationGraph(const NonlinearFactorGraph& g);
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> * Return the orientations of a graph including only BetweenFactors<Pose3>
*/ */
GTSAM_EXPORT Values static Values computeOrientationsGradient(
computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
size_t maxIter = 10000, const bool setRefFrame = true);
/** static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
* Return the orientations of a graph including only BetweenFactors<Pose3> KeyRotMap& factorId2RotMap,
*/ const NonlinearFactorGraph& pose3Graph);
GTSAM_EXPORT Values computeOrientationsGradient(
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
size_t maxIter = 10000, const bool setRefFrame = true);
GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
KeyRotMap& factorId2RotMap, const double b);
const NonlinearFactorGraph& pose3Graph);
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);
/** static Values computePoses(NonlinearFactorGraph& pose3graph,
* Select the subgraph of betweenFactors and transforms priors into between wrt Values& initialRot);
* a fictitious node
*/
GTSAM_EXPORT NonlinearFactorGraph
buildPose3graph(const NonlinearFactorGraph& graph);
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 * "extract" the Pose3 subgraph of the original graph, get orientations from
* relative orientation measurements (using either gradient or chordal method), * relative orientation measurements (using either gradient or chordal
* and finish up with 1 GN iteration on full poses. * method), and finish up with 1 GN iteration on full poses.
*/ */
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, static Values initialize(const NonlinearFactorGraph& graph,
const Values& givenGuess, const Values& givenGuess, bool useGradient = false);
bool useGradient = false);
/// Calls initialize above using Chordal method. /// Calls initialize above using Chordal method.
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); static Values initialize(const NonlinearFactorGraph& graph);
};
} // namespace initialize_pose3
} // end of namespace gtsam } // end of namespace gtsam

24
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -74,15 +74,15 @@ NonlinearFactorGraph graph() {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, buildPose3graph ) { TEST( InitializePose3, buildPose3graph ) {
NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph());
// pose3graph.print(""); // pose3graph.print("");
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, orientations ) { 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 // 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)); EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, orientationsGradientSymbolicGraph ) { TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
KeyVectorMap adjEdgesMap; KeyVectorMap adjEdgesMap;
KeyRotMap factorId2RotMap; 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)[0], 0, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) {
double a = 6.010534238540223; double a = 6.010534238540223;
double b = 1.0; 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(); Vector expected = Vector3::Zero();
expected(2) = 1.962658662803917; expected(2) = 1.962658662803917;
@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, iterationGradient ) { TEST( InitializePose3, iterationGradient ) {
NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations // Wrong initial guess - initialization should fix the rotations
Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); 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 size_t maxIter = 1; // test gradient at the first iteration
bool setRefFrame = false; 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, Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281,
0.033572116359134, 0.999436104312325, -0.000621610948719, 0.033572116359134, 0.999436104312325, -0.000621610948719,
@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, orientationsGradient ) { TEST( InitializePose3, orientationsGradient ) {
NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations // Wrong initial guess - initialization should fix the rotations
Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); 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)) )); givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
// do 10 gradient iterations // do 10 gradient iterations
bool setRefFrame = false; 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); // const Key keyAnchor = symbol('Z', 9999999);
// givenPoses.insert(keyAnchor,simple::pose0); // givenPoses.insert(keyAnchor,simple::pose0);
@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) {
givenPoses.insert(x2,simple::pose2); givenPoses.insert(x2,simple::pose2);
givenPoses.insert(x3,simple::pose3); 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 // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(givenPoses, initial, 1e-6)); EXPECT(assert_equal(givenPoses, initial, 1e-6));
@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses )
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel)); 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 !! EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !!
} }