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

Fixed a few issues with Point3 typedef path

release/4.3a0
Frank 2016-02-24 12:28:30 -08:00
parent 0372a959ee
commit 872f86698b
3 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/geometry/tests/testPoint3.cpp
View File

@ -93,20 +93,20 @@ TEST( Point3, dot) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<double(const Point3&, const Point3&)> f = boost::bind(&Point3::dot, _1, _2, //
boost::none, boost::none);
boost::function<double(const Point3&, const Point3&)> f =
[](const Point3& p, const Point3& q) { return gtsam::dot(p, q); };
{
p.dot(q, H1, H2);
gtsam::dot(p, q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, q), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, q), H2, 1e-9));
}
{
p.dot(r, H1, H2);
gtsam::dot(p, r, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, r), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, r), H2, 1e-9));
}
{
p.dot(t, H1, H2);
gtsam::dot(p, t, H1, H2);
EXPECT(assert_equal(numericalDerivative21<double,Point3>(f, p, t), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<double,Point3>(f, p, t), H2, 1e-9));
}
@ -134,15 +134,15 @@ TEST( Point3, cross2) {
// Use numerical derivatives to calculate the expected Jacobians
Matrix H1, H2;
boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&Point3::cross, _1, _2, //
boost::function<Point3(const Point3&, const Point3&)> f = boost::bind(&gtsam::cross, _1, _2, //
boost::none, boost::none);
{
p.cross(q, H1, H2);
gtsam::cross(p, q, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, q), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Point3,Point3>(f, p, q), H2, 1e-9));
}
{
p.cross(r, H1, H2);
gtsam::cross(p, r, H1, H2);
EXPECT(assert_equal(numericalDerivative21<Point3,Point3>(f, p, r), H1, 1e-9));
EXPECT(assert_equal(numericalDerivative22<Point3,Point3>(f, p, r), H2, 1e-9));
}

7
gtsam/navigation/GPSFactor.cpp
View File

@ -66,20 +66,21 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
//***************************************************************************
void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
nT_.print(" GPS measurement: ");
cout << " GPS measurement: " << nT_.transpose() << endl;
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol);
return e != NULL && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
}
//***************************************************************************
Vector GPSFactor2::evaluateError(const NavState& p,
boost::optional<Matrix&> H) const {
return p.position(H).vector() -nT_.vector();
return p.position(H) -nT_;
}
//***************************************************************************

6
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -372,7 +372,7 @@ TEST(Expression, TripleSum) {
/* ************************************************************************* */
TEST(Expression, SumOfUnaries) {
const Key key(67);
const double_ norm_(Point3_(key), &Point3::norm);
const double_ norm_(&gtsam::norm, Point3_(key));
const double_ sum_ = norm_ + norm_;
Values values;
@ -391,7 +391,7 @@ TEST(Expression, SumOfUnaries) {
TEST(Expression, UnaryOfSum) {
const Key key1(42), key2(67);
const Point3_ sum_ = Point3_(key1) + Point3_(key2);
const double_ norm_(sum_, &Point3::norm);
const double_ norm_(&gtsam::norm, sum_);
map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
norm_.dims(actual_dims);
@ -455,7 +455,7 @@ TEST(Expression, Subtract) {
/* ************************************************************************* */
TEST(Expression, LinearExpression) {
const Key key(67);
const boost::function<Vector3(Point3)> f = boost::bind(&Point3::vector, _1);
const boost::function<Vector3(Point3)> f = [](const Point3& p) { return (Vector3)p; };
const Matrix3 kIdentity = I_3x3;
const Expression<Vector3> linear_ = linearExpression(f, Point3_(key), kIdentity);