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

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -30,13 +30,9 @@ 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);
static const Point3 point1(-0.08,-0.08, 0.0);

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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));

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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));
}
/* ************************************************************************* */

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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);
}
/* ************************************************************************* */
@ -201,7 +201,7 @@ TEST(Rot3, log)
// Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x*PI, y*PI, z*PI).finished();
R = Rot3::Rodrigues(w);
R = Rot3::Rodrigues(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
#else
CHECK_OMEGA(x*PI,y*PI,z*PI)
@ -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));
}

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -28,13 +28,9 @@ 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);
static const Point3 point1(-0.08,-0.08, 0.0);

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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));

4
gtsam/navigation/GPSFactor.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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));

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/VelocityConstraint.h
View File

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

6
tests/simulated3D.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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();
}
};