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); // return computePoses(pose2Graph, orientationsLago);
//} //}
// //
///* ************************************************************************* */ /* ************************************************************************* */
//Values initialize(const NonlinearFactorGraph& graph, Values initialize(const NonlinearFactorGraph& graph,
// const Values& initialGuess) { const Values& givenGuess) {
// Values initialGuessLago; Values initialValues;
//
// // get the orientation estimates from LAGO // get the orientation estimates from LAGO
// VectorValues orientations = initializeOrientations(graph); Values orientations = initializeOrientations(graph);
//
// // for all nodes in the tree BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
// BOOST_FOREACH(const VectorValues::value_type& it, orientations) { Key key = key_value.key;
// Key key = it.first; if (key != keyAnchor) {
// if (key != keyAnchor) { const Point3& pos = givenGuess.at<Pose3>(key).translation();
// const Pose2& pose = initialGuess.at<Pose2>(key); const Rot3& rot = orientations.at<Rot3>(key);
// const Vector& orientation = it.second; Pose3 initializedPoses = Pose3(rot, pos);
// Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); initialValues.insert(key, initializedPoses);
// initialGuessLago.insert(key, poseLago); }
// } }
// } return initialValues;
// return initialGuessLago; }
//}
} // end of namespace lago } // end of namespace lago
} // end of namespace gtsam } // 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 NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess);
///** Linear factor graph with regularized orientation measurements */ ///** Linear factor graph with regularized orientation measurements */
//GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( //GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
// const std::vector<size_t>& spanningTreeIds, // 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)); 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() { int main() {