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

added initial failing unit test

release/4.3a0
Luca 2014-09-02 18:50:23 -04:00
parent 15abbc484b
commit 366101176c
3 changed files with 46 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/slam/InitializePose3.cpp
View File

@ -38,6 +38,8 @@ static const Matrix zero33= Matrix::Zero(3,3);
static const Key keyAnchor = symbol('Z', 9999999);
typedef std::map<Key, vector<size_t> > KeyVectorMap;
/* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
@ -150,16 +152,31 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess) {
gttic(InitializePose3_computeOrientationsGradient);
// this works on the inverse rotations, according to Tron&Vidal,2011
Values inverseRot;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) {
Key key = key_value.key;
const Pose3& pose = givenGuess.at<Pose3>(key);
inverseRot.insert(key, pose.rotation().inverse());
}
// Create the map of edges incident on each node
KeyVectorMap adjEdgesMap;
//
// // regularize measurements and plug everything in a factor graph
// GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);
//
// // Solve the LFG
// VectorValues relaxedRot3 = relaxedGraph.optimize();
// regularize measurements and plug everything in a factor graph
GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);
// Solve the LFG
VectorValues relaxedRot3 = relaxedGraph.optimize();
// normalize and compute Rot3
return normalizeRelaxedRotations(relaxedRot3);
// Return correct rotations
Values estimateRot;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
Key key = key_value.key;
const Rot3& R = inverseRot.at<Rot3>(key);
estimateRot.insert(key, R.inverse());
}
return estimateRot;
}
/* ************************************************************************* */

2
gtsam/slam/InitializePose3.h
View File

@ -34,6 +34,8 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess);
GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot);

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

@ -91,6 +91,25 @@ TEST( InitializePose3, orientations ) {
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, orientationsGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,simple::pose0);
givenPoses.insert(x2,simple::pose0);
givenPoses.insert(x3,simple::pose0);
Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses);
// 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::R1, initial.at<Rot3>(x1), 1e-6));
EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, posesWithGivenGuess ) {
Values givenPoses;