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

now initialization is aware of rotation noise model

release/4.3a0
lucacarlone 2019-03-19 18:44:09 -04:00
parent a89e422a8a
commit 1e944fb86a
2 changed files with 34 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/slam/InitializePose3.cpp
View File

@ -36,16 +36,21 @@ static const Key kAnchorKey = symbol('Z', 9999999);
GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
GaussianFactorGraph linearGraph;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
for(const auto& factor: g) {
Matrix3 Rij;
double rotationPrecision = 1.0;
auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between)
if (pose3Between){
Rij = pose3Between->measured().rotation().matrix();
else
Vector precisions = Vector::Zero(6);
precisions[0] = 1.0; // vector of all zeros except first entry equal to 1
pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable
rotationPrecision = precisions[0]; // rotations first
}else{
cout << "Error in buildLinearOrientationGraph" << endl;
}
const auto& keys = factor->keys();
Key key1 = keys[0], key2 = keys[1];
@ -53,14 +58,14 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear
M9.block(0,0,3,3) = Rij;
M9.block(3,3,3,3) = Rij;
M9.block(6,6,3,3) = Rij;
linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, model);
linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision));
}
// prior on the anchor orientation
linearGraph.add(
kAnchorKey, I_9x9,
(Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0)
.finished(),
model);
noiseModel::Isotropic::Precision(9, 1));
return linearGraph;
}

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

@ -70,6 +70,17 @@ NonlinearFactorGraph graph() {
g.add(PriorFactor<Pose3>(x0, pose0, model));
return g;
}
NonlinearFactorGraph graph2() {
NonlinearFactorGraph g;
g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information
g.add(BetweenFactor<Pose3>(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin
g.add(PriorFactor<Pose3>(x0, pose0, model));
return g;
}
}
/* *************************************************************************** */
@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) {
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, orientationsPrecisions ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2());
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));
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, orientationsGradientSymbolicGraph ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());