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

Dealing with Pose3 mojo loss

release/4.3a0
dellaert 2014-12-25 20:04:28 +01:00
parent 47ff09f6c8
commit afe20d83bd
5 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -41,7 +41,7 @@ TEST( AntiFactor, NegativeHessian)
Pose3 pose1(Rot3(), Point3(0, 0, 0));
Pose3 pose2(Rot3(), Point3(2, 1, 3));
Pose3 z(Rot3(), Point3(1, 1, 1));
SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim()));
SharedNoiseModel sigma(noiseModel::Unit::Create(6));
// Create a configuration corresponding to the ground truth
Values values;
@ -92,7 +92,7 @@ TEST( AntiFactor, EquivalentBayesNet)
Pose3 pose1(Rot3(), Point3(0, 0, 0));
Pose3 pose2(Rot3(), Point3(2, 1, 3));
Pose3 z(Rot3(), Point3(1, 1, 1));
SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim()));
SharedNoiseModel sigma(noiseModel::Unit::Create(6));
NonlinearFactorGraph graph;
graph.push_back(PriorFactor<Pose3>(1, pose1, sigma));

2
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -100,7 +100,7 @@ public:
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
assert(false);
return zero(x1.dim());
return Vector6::Zero();
}
private:

2
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -90,7 +90,7 @@ public:
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
assert(false); // no corresponding factor here
return zero(x1.dim());
return Vector6::Zero();
}
private:

12
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -21,7 +21,7 @@ Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0));
//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished());
Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished());
Vector6 V1_g1 = g1.inverse().Adjoint(V1_w);
Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP));
Pose3 g2(g1.expmap(h*V1_g1));
//Vector6 v2 = Pose3::Logmap(g1.between(g2));
double mass = 100.0;
@ -52,12 +52,10 @@ Vector testExpmapDeriv(const Vector6& v) {
TEST(Reconstruction, ExpmapInvDeriv) {
Matrix numericalExpmap = numericalDerivative11(
boost::function<Vector(const Vector6&)>(
boost::bind(testExpmapDeriv, _1)
),
Vector6(Vector::Zero(6)), 1e-5
);
Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1);
boost::function<Vector(const Vector6&)>(boost::bind(testExpmapDeriv, _1)),
Vector6(Vector::Zero(6)), 1e-5);
Pose3 newPose = Pose3::Expmap(h * V1_g1);
Matrix dExpInv = Pose3::LogmapDerivative(newPose);
EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2));
}

2
gtsam_unstable/slam/PosePriorFactor.h
View File

@ -88,7 +88,7 @@ namespace gtsam {
// manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.localCoordinates(p.compose(*body_P_sensor_, H));
} else {
if(H) (*H) = eye(p.dim());
if(H) (*H) = I_6x6;
// manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.localCoordinates(p);
}