Merged in feature/geometry_tidbits (pull request #216)
A set of geometry changes from the FixedValues branch, unrelated to that PRrelease/4.3a0
						commit
						11b7d263e4
					
				|  | @ -110,20 +110,16 @@ public: | |||
|     std::vector<Z> z(m); | ||||
| 
 | ||||
|     // Allocate derivatives
 | ||||
|     if (E) | ||||
|       E->resize(ZDim * m, N); | ||||
|     if (Fs) | ||||
|       Fs->resize(m); | ||||
|     if (E) E->resize(ZDim * m, N); | ||||
|     if (Fs) Fs->resize(m); | ||||
| 
 | ||||
|     // Project and fill derivatives
 | ||||
|     for (size_t i = 0; i < m; i++) { | ||||
|       MatrixZD Fi; | ||||
|       Eigen::Matrix<double, ZDim, N> Ei; | ||||
|       z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0); | ||||
|       if (Fs) | ||||
|         (*Fs)[i] = Fi; | ||||
|       if (E) | ||||
|         E->block<ZDim, N>(ZDim * i, 0) = Ei; | ||||
|       if (Fs) (*Fs)[i] = Fi; | ||||
|       if (E) E->block<ZDim, N>(ZDim * i, 0) = Ei; | ||||
|     } | ||||
| 
 | ||||
|     return z; | ||||
|  |  | |||
|  | @ -32,6 +32,14 @@ namespace gtsam { | |||
| template<typename Calibration> | ||||
| class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> { | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Some classes template on either PinholeCamera or StereoCamera, | ||||
|    *  and this typedef informs those classes what "project" returns. | ||||
|    */ | ||||
|   typedef Point2 Measurement; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
 | ||||
|  | @ -98,6 +106,20 @@ public: | |||
|     return PinholeCamera(Base::LookatPose(eye, target, upVector), K); | ||||
|   } | ||||
| 
 | ||||
|   // Create PinholeCamera, with derivatives
 | ||||
|   static PinholeCamera Create(const Pose3& pose, const Calibration &K, | ||||
|       OptionalJacobian<dimension, 6> H1 = boost::none, //
 | ||||
|       OptionalJacobian<dimension, DimK> H2 = boost::none) { | ||||
|     typedef Eigen::Matrix<double, DimK, 6> MatrixK6; | ||||
|     if (H1) | ||||
|       *H1 << I_6x6, MatrixK6::Zero(); | ||||
|     typedef Eigen::Matrix<double, 6, DimK> Matrix6K; | ||||
|     typedef Eigen::Matrix<double, DimK, DimK> MatrixK; | ||||
|     if (H2) | ||||
|       *H2 << Matrix6K::Zero(), MatrixK::Identity(); | ||||
|     return PinholeCamera(pose,K); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Advanced Constructors
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -57,6 +57,21 @@ TEST( PinholeCamera, constructor) | |||
|   EXPECT(assert_equal( pose, camera.pose())); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(PinholeCamera, Create) { | ||||
| 
 | ||||
|   Matrix actualH1, actualH2; | ||||
|   EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2))); | ||||
| 
 | ||||
|   // Check derivative
 | ||||
|   boost::function<Camera(Pose3,Cal3_S2)> f = //
 | ||||
|       boost::bind(Camera::Create,_1,_2,boost::none,boost::none); | ||||
|   Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K); | ||||
|   EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); | ||||
|   Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K); | ||||
|   EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(PinholeCamera, Pose) { | ||||
| 
 | ||||
|  |  | |||
|  | @ -21,9 +21,4 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| const double Event::Speed = 330; | ||||
| const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| } //\ namespace gtsam
 | ||||
|  |  | |||
|  | @ -33,10 +33,6 @@ class Event { | |||
| public: | ||||
|   enum { dimension = 4 }; | ||||
| 
 | ||||
|   /// Speed of sound
 | ||||
|   static const double Speed; | ||||
|   static const Matrix14 JacobianZ; | ||||
| 
 | ||||
|   /// Default Constructor
 | ||||
|   Event() : | ||||
|       time_(0) { | ||||
|  | @ -57,6 +53,7 @@ public: | |||
| 
 | ||||
|   // TODO we really have to think of a better way to do linear arguments
 | ||||
|   double height(OptionalJacobian<1,4> H = boost::none) const { | ||||
|     static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished(); | ||||
|     if (H) *H = JacobianZ; | ||||
|     return location_.z(); | ||||
|   } | ||||
|  | @ -87,6 +84,7 @@ public: | |||
|   double toa(const Point3& microphone, //
 | ||||
|       OptionalJacobian<1, 4> H1 = boost::none, //
 | ||||
|       OptionalJacobian<1, 3> H2 = boost::none) const { | ||||
|     static const double Speed = 330; | ||||
|     Matrix13 D1, D2; | ||||
|     double distance = location_.distance(microphone, D1, D2); | ||||
|     if (H1) | ||||
|  |  | |||
|  | @ -45,13 +45,13 @@ TEST( Event, Constructor ) { | |||
| //*****************************************************************************
 | ||||
| TEST( Event, Toa1 ) { | ||||
|   Event event(0, 1, 0, 0); | ||||
|   double expected = 1 / Event::Speed; | ||||
|   double expected = 1. / 330; | ||||
|   EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); | ||||
| } | ||||
| 
 | ||||
| //*****************************************************************************
 | ||||
| TEST( Event, Toa2 ) { | ||||
|   double expectedTOA = timeOfEvent + 1 / Event::Speed; | ||||
|   double expectedTOA = timeOfEvent + 1. / 330; | ||||
|   EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); | ||||
| } | ||||
| 
 | ||||
|  | @ -79,7 +79,7 @@ TEST( Event, Expression ) { | |||
| 
 | ||||
|   Values values; | ||||
|   values.insert(key, exampleEvent); | ||||
|   double expectedTOA = timeOfEvent + 1 / Event::Speed; | ||||
|   double expectedTOA = timeOfEvent + 1. / 330; | ||||
|   EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue