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

Cleaned up a number of Point3/Rot3 related uses

release/4.3a0
Frank 2016-02-08 17:30:44 -08:00
parent 4319bece1e
commit a19aa793d7
14 changed files with 48 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,11 +30,7 @@ using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
// Camera situated at 0.5 meters high, looking down // Camera situated at 0.5 meters high, looking down
static const Pose3 pose1((Matrix3() << static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0, 0, 0.5)); Point3(0, 0, 0.5));
static const CalibratedCamera camera(pose1); 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 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 Camera camera(pose, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); 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 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 Camera camera(pose, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); 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) { TEST( Point3, arithmetic) {
CHECK(P * 3 == 3 * P); CHECK(P * 3 == 3 * P);
CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); CHECK(assert_equal<Point3>(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>(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>(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>(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>(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, 2, 3), Point3(2, 4, 6) / 2));
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

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

@ -28,11 +28,7 @@ using namespace gtsam;
static const Cal3_S2 K(625, 625, 0, 0, 0); static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1((Matrix3() << static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0, 0, 0.5)); Point3(0, 0, 0.5));
static const SimpleCamera camera(pose1, K); 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., 1., 0., 0.,
0.,-1., 0., 0.,-1., 0.,
0., 0.,-1. 0., 0.,-1.
).finished(), ).finished()),
Point3(0,0,6.25)); Point3(0,0,6.25));
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); 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, 0) << zeros(3, 3);
H->block < 3, 3 > (0, 3) << p.rotation().matrix(); 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 // An accelerometer measures acceleration in body, but not gravity
Vector3 actualSpecificForce(double t) const { 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(); 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) { TEST( NavState, BodyVelocity) {
Matrix39 aH, eH; Matrix39 aH, eH;
Velocity3 actual = kState1.bodyVelocity(aH); Velocity3 actual = kState1.bodyVelocity(aH);
EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); EXPECT(assert_equal<Velocity3>(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>( eH = numericalDerivative11<Velocity3, NavState>(
boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1);
EXPECT(assert_equal((Matrix )eH, aH)); EXPECT(assert_equal((Matrix )eH, aH));

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

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

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

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

2
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -115,7 +115,7 @@ private:
case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break;
default: assert(false); break; default: assert(false); break;
} }
return (p2 - hx).vector(); return p2.vector() - hx.vector();
} }
}; };

4
tests/simulated3D.h
View File

@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
*/ */
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H = Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) const { 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, Vector evaluateError(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const { 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();
} }
}; };