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 pose1(Rot3(), Point3(0, 0, 0));
Pose3 pose2(Rot3(), Point3(2, 1, 3)); Pose3 pose2(Rot3(), Point3(2, 1, 3));
Pose3 z(Rot3(), Point3(1, 1, 1)); 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 // Create a configuration corresponding to the ground truth
Values values; Values values;
@ -92,7 +92,7 @@ TEST( AntiFactor, EquivalentBayesNet)
Pose3 pose1(Rot3(), Point3(0, 0, 0)); Pose3 pose1(Rot3(), Point3(0, 0, 0));
Pose3 pose2(Rot3(), Point3(2, 1, 3)); Pose3 pose2(Rot3(), Point3(2, 1, 3));
Pose3 z(Rot3(), Point3(1, 1, 1)); Pose3 z(Rot3(), Point3(1, 1, 1));
SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); SharedNoiseModel sigma(noiseModel::Unit::Create(6));
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(PriorFactor<Pose3>(1, pose1, sigma)); 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&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
assert(false); assert(false);
return zero(x1.dim()); return Vector6::Zero();
} }
private: private:

2
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -90,7 +90,7 @@ public:
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
assert(false); // no corresponding factor here assert(false); // no corresponding factor here
return zero(x1.dim()); return Vector6::Zero();
} }
private: 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((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_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); 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)); //Vector6 v2 = Pose3::Logmap(g1.between(g2));
double mass = 100.0; double mass = 100.0;
@ -52,12 +52,10 @@ Vector testExpmapDeriv(const Vector6& v) {
TEST(Reconstruction, ExpmapInvDeriv) { TEST(Reconstruction, ExpmapInvDeriv) {
Matrix numericalExpmap = numericalDerivative11( Matrix numericalExpmap = numericalDerivative11(
boost::function<Vector(const Vector6&)>( boost::function<Vector(const Vector6&)>(boost::bind(testExpmapDeriv, _1)),
boost::bind(testExpmapDeriv, _1) Vector6(Vector::Zero(6)), 1e-5);
), Pose3 newPose = Pose3::Expmap(h * V1_g1);
Vector6(Vector::Zero(6)), 1e-5 Matrix dExpInv = Pose3::LogmapDerivative(newPose);
);
Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1);
EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); 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)) // manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.localCoordinates(p.compose(*body_P_sensor_, H)); return prior_.localCoordinates(p.compose(*body_P_sensor_, H));
} else { } else {
if(H) (*H) = eye(p.dim()); if(H) (*H) = I_6x6;
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.localCoordinates(p); return prior_.localCoordinates(p);
} }