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

working version with pose

release/4.3a0
Luca 2014-08-17 20:50:18 -04:00
parent 4cd4023ef8
commit c0f880d52b
3 changed files with 35 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam/slam/InitializePose3.cpp
View File

@ -239,26 +239,25 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) {
// return computePoses(pose2Graph, orientationsLago);
//}
//
///* ************************************************************************* */
//Values initialize(const NonlinearFactorGraph& graph,
// const Values& initialGuess) {
// Values initialGuessLago;
//
// // get the orientation estimates from LAGO
// VectorValues orientations = initializeOrientations(graph);
//
// // for all nodes in the tree
// BOOST_FOREACH(const VectorValues::value_type& it, orientations) {
// Key key = it.first;
// if (key != keyAnchor) {
// const Pose2& pose = initialGuess.at<Pose2>(key);
// const Vector& orientation = it.second;
// Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
// initialGuessLago.insert(key, poseLago);
// }
// }
// return initialGuessLago;
//}
/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph,
const Values& givenGuess) {
Values initialValues;
// get the orientation estimates from LAGO
Values orientations = initializeOrientations(graph);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
Key key = key_value.key;
if (key != keyAnchor) {
const Point3& pos = givenGuess.at<Pose3>(key).translation();
const Rot3& rot = orientations.at<Rot3>(key);
Pose3 initializedPoses = Pose3(rot, pos);
initialValues.insert(key, initializedPoses);
}
}
return initialValues;
}
} // end of namespace lago
} // end of namespace gtsam

2
gtsam/slam/InitializePose3.h
View File

@ -38,6 +38,8 @@ GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph);
GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess);
///** Linear factor graph with regularized orientation measurements */
//GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
// const std::vector<size_t>& spanningTreeIds,

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

@ -89,6 +89,20 @@ TEST( InitializePose3, orientations ) {
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, posesWithGivenGuess ) {
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,simple::pose1);
givenPoses.insert(x2,simple::pose2);
givenPoses.insert(x3,simple::pose3);
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));
}
/* ************************************************************************* */
int main() {