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

Merged in feature/Point3_cleanup (pull request #219) 

Cleanup some Point3/Rot3 uses
release/4.3a0
Frank Dellaert 2016-02-08 20:09:14 -08:00
parent 21bbd631ef 50e8fa9b54
commit 33d84be82b
30 changed files with 114 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SFMExample.cpp
View File

@ -99,7 +99,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */

2
examples/SFMExampleExpressions.cpp
View File

@ -88,7 +88,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j)
initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
cout << "initial error = " << graph.error(initial) << endl;
/* Optimize the graph and print results */

2
examples/SelfCalibrationExample.cpp
View File

@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();

2
examples/VisualISAM2Example.cpp
View File

@ -113,7 +113,7 @@ int main(int argc, char* argv[]) {
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15));
} else {
// Update iSAM with the new factors

2
examples/VisualISAMExample.cpp
View File

@ -120,7 +120,7 @@ int main(int argc, char* argv[]) {
Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) {
// Intentionally initialize the variables off from the ground truth
Point3 initial_lj = points[j].compose(noise);
Point3 initial_lj = points[j] + noise;
initialEstimate.insert(Symbol('l', j), initial_lj);
}

11
gtsam.h
View File

@ -360,17 +360,6 @@ class Point3 {
// Group
static gtsam::Point3 identity();
gtsam::Point3 inverse() const;
gtsam::Point3 compose(const gtsam::Point3& p2) const;
gtsam::Point3 between(const gtsam::Point3& p2) const;
// Manifold
gtsam::Point3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point3& p) const;
// Lie Group
static gtsam::Point3 Expmap(Vector v);
static Vector Logmap(const gtsam::Point3& p);
// Standard Interface
Vector vector() const;

7
gtsam/base/Lie.h
View File

@ -295,19 +295,18 @@ private:
};
/**
* Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>Hausdorff formula
* Three term approximation of the Baker-Campbell-Hausdorff formula
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
* it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
* formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
* http://en.wikipedia.org/wiki/Baker<EFBFBD>Campbell<EFBFBD>Hausdorff_formula
* http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula
*/
/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
template<class T>
T BCH(const T& X, const T& Y) {
static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
T X_Y = bracket(X, Y);
return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
bracket(X, X_Y));
return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
}
/**

3
gtsam/geometry/Point3.cpp
View File

@ -75,6 +75,7 @@ double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
@ -82,13 +83,13 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
return *this - q;
}
#endif
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {

22
gtsam/geometry/Point3.h
View File

@ -107,11 +107,6 @@ namespace gtsam {
double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point3& p2) const {
return (p2 - *this).norm();
}
/** Distance of the point from the origin, with Jacobian */
double norm(OptionalJacobian<1,3> H = boost::none) const;
@ -145,19 +140,12 @@ namespace gtsam {
/// get z
inline double z() const {return z_;}
/** add two points, add(this,q) is same as this + q */
Point3 add (const Point3 &q,
OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
/** subtract two points, sub(this,q) is same as this - q */
Point3 sub (const Point3 &q,
OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
/// @}
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point3 inverse() const { return -(*this);}
@ -167,7 +155,13 @@ namespace gtsam {
Point3 retract(const Vector3& v) const { return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) { return p.vector();}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
/// @}
inline double dist(const Point3& p2) const { return (p2 - *this).norm(); }
Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// @}
#endif
private:

8
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -30,12 +30,8 @@ using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
// Camera situated at 0.5 meters high, looking down
static const Pose3 pose1((Matrix3() <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0,0,0.5));
static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
Point3(0, 0, 0.5));
static const CalibratedCamera camera(pose1);

2
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -34,7 +34,7 @@ typedef PinholeCamera<Cal3_S2> Camera;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Camera camera(pose, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));

2
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -35,7 +35,7 @@ typedef PinholePose<Cal3_S2> Camera;
static const Cal3_S2::shared_ptr K = boost::make_shared<Cal3_S2>(625, 625, 0, 0, 0);
static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Camera camera(pose, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));

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

@ -61,12 +61,12 @@ TEST(Point3, Lie) {
/* ************************************************************************* */
TEST( Point3, arithmetic) {
CHECK(P * 3 == 3 * P);
CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
CHECK(assert_equal<Point3>(Point3(-1, -5, -6), -Point3(1, 5, 6)));
CHECK(assert_equal<Point3>(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
CHECK(assert_equal<Point3>(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
CHECK(assert_equal<Point3>(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
CHECK(assert_equal<Point3>(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
CHECK(assert_equal<Point3>(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
}
/* ************************************************************************* */

25
gtsam/geometry/tests/testRot3.cpp
View File

@ -97,10 +97,10 @@ TEST( Rot3, equals)
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_Rodrigues(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
Matrix3 J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return Rot3(R);
}
/* ************************************************************************* */
@ -274,14 +274,13 @@ TEST(Rot3, manifold_expmap)
}
/* ************************************************************************* */
class AngularVelocity: public Point3 {
public:
AngularVelocity(const Point3& p) :
Point3(p) {
}
AngularVelocity(double wx, double wy, double wz) :
Point3(wx, wy, wz) {
}
class AngularVelocity : public Vector3 {
public:
template <typename Derived>
inline AngularVelocity(const Eigen::MatrixBase<Derived>& v)
: Vector3(v) {}
AngularVelocity(double wx, double wy, double wz) : Vector3(wx, wy, wz) {}
};
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
@ -294,10 +293,10 @@ TEST(Rot3, BCH)
// Approximate exmap by BCH formula
AngularVelocity w1(0.2, -0.1, 0.1);
AngularVelocity w2(0.01, 0.02, -0.03);
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
Rot3 R1 = Rot3::Expmap (w1), R2 = Rot3::Expmap (w2);
Rot3 R3 = R1 * R2;
Vector expected = Rot3::Logmap(R3);
Vector actual = BCH(w1, w2).vector();
Vector actual = BCH(w1, w2);
CHECK(assert_equal(expected, actual,1e-5));
}

8
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -28,12 +28,8 @@ using namespace gtsam;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1((Matrix3() <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0,0,0.5));
static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
Point3(0, 0, 0.5));
static const SimpleCamera camera(pose1, K);

4
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -74,11 +74,11 @@ TEST( StereoCamera, project)
/* ************************************************************************* */
static Pose3 camPose((Matrix)(Matrix(3,3) <<
static Pose3 camPose(Rot3((Matrix3() <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
).finished()),
Point3(0,0,6.25));
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));

2
gtsam/navigation/GPSFactor.cpp
View File

@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p,
H->block < 3, 3 > (0, 0) << zeros(3, 3);
H->block < 3, 3 > (0, 3) << p.rotation().matrix();
}
return (p.translation() -nT_).vector();
return p.translation().vector() -nT_.vector();
}
//***************************************************************************

2
gtsam/navigation/ScenarioRunner.h
View File

@ -73,7 +73,7 @@ class ScenarioRunner {
// An accelerometer measures acceleration in body, but not gravity
Vector3 actualSpecificForce(double t) const {
Rot3 bRn = scenario_->rotation(t).transpose();
Rot3 bRn(scenario_->rotation(t).transpose());
return scenario_->acceleration_b(t) - bRn * gravity_n();
}

2
gtsam/navigation/tests/testNavState.cpp
View File

@ -81,7 +81,7 @@ TEST( NavState, Velocity) {
TEST( NavState, BodyVelocity) {
Matrix39 aH, eH;
Velocity3 actual = kState1.bodyVelocity(aH);
EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity)));
EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>(
boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1);
EXPECT(assert_equal((Matrix )eH, aH));

2
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
Cal3Bundler0(1, 0, 0));
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
Vector9 P = Camera().localCoordinates(camera);
Vector3 X = Point3::Logmap(point);
Vector3 X = point.vector();
Vector2 expectedMeasurement(1.2431567, 1.2525694);
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);

8
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -105,16 +105,16 @@ static vector<Point3> genPoint3() {
static vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> X;
X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.))));
X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.))));
X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.))));
X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.))));
return X;
}
static vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3_S2 K(640, 480, 0.1, 320, 240);
vector<GeneralCamera> X;
X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K));
X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K));
X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
return X;
}

6
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -31,11 +31,7 @@
using namespace std;
using namespace gtsam;
static Pose3 camera1((Matrix) (Matrix(3, 3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()),
Point3(0,0,6.25));
static boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -161,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
// predict point for constraint
// NOTE: uses simple Euler approach for prediction
Point3 pred_t2 = t().retract(v2 * dt);
Point3 pred_t2 = t() + Point3(v2 * dt);
return pred_t2;
}

8
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -110,12 +110,12 @@ private:
const Point3& p1 = x1.t(), p2 = x2.t();
Point3 hx;
switch(mode) {
case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break;
case dynamics::EULER_START: hx = p1.retract(v1 * dt); break;
case dynamics::EULER_END : hx = p1.retract(v2 * dt); break;
case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break;
case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break;
case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break;
default: assert(false); break;
}
return (p2 - hx).vector();
return p2.vector() - hx.vector();
}
};

2
gtsam_unstable/geometry/Event.h
View File

@ -72,7 +72,7 @@ public:
/// Updates a with tangent space delta
inline Event retract(const Vector4& v) const {
return Event(time_ + v[0], location_.retract(v.tail(3)));
return Event(time_ + v[0], location_ + Point3(v.tail<3>()));
}
/// Returns inverse retraction

2
gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -75,7 +75,7 @@ int main() {
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Point3 t2 = t1 + Point3(Vel1*measurement_dt);
Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
Vector3 Vel2 = Vel1 + dv;

11
gtsampy.h
View File

@ -360,17 +360,6 @@ class Point3 {
// Group
static gtsam::Point3 identity();
gtsam::Point3 inverse() const;
gtsam::Point3 compose(const gtsam::Point3& p2) const;
gtsam::Point3 between(const gtsam::Point3& p2) const;
// Manifold
gtsam::Point3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point3& p) const;
// Lie Group
static gtsam::Point3 Expmap(Vector v);
static Vector Logmap(const gtsam::Point3& p);
// Standard Interface
Vector vector() const;

3
python/handwritten/geometry/Point3.cpp
View File

@ -36,16 +36,13 @@ class_<Point3>("Point3")
.def(init<const Vector3 &>())
.def("identity", &Point3::identity)
.staticmethod("identity")
.def("add", &Point3::add)
.def("cross", &Point3::cross)
.def("dist", &Point3::dist)
.def("distance", &Point3::distance)
.def("dot", &Point3::dot)
.def("equals", &Point3::equals, equals_overloads(args("q","tol")))
.def("norm", &Point3::norm)
.def("normalize", &Point3::normalize)
.def("print", &Point3::print, print_overloads(args("s")))
.def("sub", &Point3::sub)
.def("vector", &Point3::vector)
.def("x", &Point3::x)
.def("y", &Point3::y)

4
tests/simulated3D.h
View File

@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
*/
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) const {
return (prior(x, H) - measured_).vector();
return prior(x, H).vector() - measured_.vector();
}
};
@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
*/
Vector evaluateError(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - measured_).vector();
return mea(x1, x2, H1, H2).vector() - measured_.vector();
}
};

8
timing/timeSFMBAL.h
View File

@ -49,8 +49,12 @@ SfM_data preamble(int argc, char* argv[]) {
// Load BAL file
SfM_data db;
string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre");
bool success = readBAL(argc > 1 ? argv[argc - 1] : defaultFilename, db);
string filename;
if (argc > 1)
filename = argv[argc - 1];
else
filename = findExampleDataFile("dubrovnik-16-22106-pre");
bool success = readBAL(argv[argc - 1], db);
if (!success) throw runtime_error("Could not access file!");
return db;
}