Cleaned up a number of Point3/Rot3 related uses
							parent
							
								
									4319bece1e
								
							
						
					
					
						commit
						a19aa793d7
					
				|  | @ -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); | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -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)); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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(); | ||||
| } | ||||
| 
 | ||||
| //***************************************************************************
 | ||||
|  |  | |||
|  | @ -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(); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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(); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -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(); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue