mockingbirdnest · May 27, 2017 · May 28, 2017 · May 28, 2017 · May 30, 2017 · May 30, 2017
diff --git a/geometry/geometry.vcxproj b/geometry/geometry.vcxproj
@@ -292,6 +292,8 @@
     <ClInclude Include="named_quantities.hpp" />
     <ClInclude Include="pair.hpp" />
     <ClInclude Include="pair_body.hpp" />
+    <ClInclude Include="perspective.hpp" />
+    <ClInclude Include="perspective_body.hpp" />
     <ClInclude Include="point.hpp" />
     <ClInclude Include="point_body.hpp" />
     <ClInclude Include="grassmann_body.hpp" />
@@ -322,6 +324,7 @@
     <ClCompile Include="grassmann_test.cpp" />
     <ClCompile Include="identity_test.cpp" />
     <ClCompile Include="pair_test.cpp" />
+    <ClCompile Include="perspective_test.cpp" />
     <ClCompile Include="point_test.cpp" />
     <ClCompile Include="affine_map_test.cpp" />
     <ClCompile Include="orthogonal_map_test.cpp" />

diff --git a/geometry/geometry.vcxproj.filters b/geometry/geometry.vcxproj.filters
@@ -119,6 +119,12 @@
     <ClInclude Include="rp2_element_body.hpp">
       <Filter>Source Files</Filter>
     </ClInclude>
+    <ClInclude Include="perspective.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="perspective_body.hpp">
+      <Filter>Source Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="sign_test.cpp">
@@ -166,5 +172,8 @@
     <ClCompile Include="rp2_element_test.cpp">
       <Filter>Test Files</Filter>
     </ClCompile>
+    <ClCompile Include="perspective_test.cpp">
+      <Filter>Test Files</Filter>
+    </ClCompile>
   </ItemGroup>
 </Project>
diff --git a/geometry/perspective.hpp b/geometry/perspective.hpp
@@ -0,0 +1,43 @@
+
+#pragma once
+
+#include "geometry/affine_map.hpp"
+#include "geometry/grassmann.hpp"
+#include "geometry/point.hpp"
+#include "geometry/rp2_element.hpp"
+
+namespace principia {
+namespace geometry {
+namespace internal_perspective {
+
+// A perspective using the pinhole camera model.  It project a point of
+// |FromFrame| to an element of ℝP².  |ToFrame| is the frame of the camera.  In
+// that frame the camera is located at the origin and looking at the positive
+// z-axis.  The x- and y- axis of the camera correspond to those of ℝP².
+template<typename FromFrame, typename ToFrame, typename Scalar,
+         template<typename, typename> class LinearMap>
+class Perspective final {
+ public:
+  Perspective(
+      AffineMap<FromFrame, ToFrame, Scalar, LinearMap> const& to_camera,
+      Scalar const& focal);
+  Perspective(
+      AffineMap<ToFrame, FromFrame, Scalar, LinearMap> const& from_camera,
+      Scalar const& focal);
+
+  RP2Element<Scalar> operator()(
+      Point<Vector<Scalar, FromFrame>> const& point) const;
+
+ private:
+  AffineMap<FromFrame, ToFrame, Scalar, LinearMap> to_camera_;
+  Scalar focal_;
+};
+
+}  // namespace internal_perspective
+
+using internal_perspective::Perspective;
+
+}  // namespace geometry
+}  // namespace principia
+
+#include "geometry/perspective_body.hpp"
diff --git a/geometry/perspective_body.hpp b/geometry/perspective_body.hpp
@@ -0,0 +1,45 @@
+
+#pragma once
+
+#include "geometry/perspective.hpp"
+
+namespace principia {
+namespace geometry {
+namespace internal_perspective {
+
+template<typename FromFrame,
+         typename ToFrame,
+         typename Scalar,
+         template<typename, typename> class LinearMap>
+Perspective<FromFrame, ToFrame, Scalar, LinearMap>::Perspective(
+    AffineMap<FromFrame, ToFrame, Scalar, LinearMap> const& to_camera,
+    Scalar const& focal)
+    : to_camera_(to_camera),
+      focal_(focal) {}
+
+template<typename FromFrame, typename ToFrame, typename Scalar,
+         template<typename, typename> class LinearMap>
+Perspective<FromFrame, ToFrame, Scalar, LinearMap>::Perspective(
+    AffineMap<ToFrame, FromFrame, Scalar, LinearMap> const& from_camera,
+    Scalar const& focal)
+    : to_camera_(from_camera.Inverse()),
+      focal_(focal) {}
+
+template<typename FromFrame, typename ToFrame, typename Scalar,
+         template<typename, typename> class LinearMap>
+RP2Element<Scalar> Perspective<FromFrame, ToFrame, Scalar, LinearMap>::
+operator()(Point<Vector<Scalar, FromFrame>> const& point) const {
+  Point<Vector<Scalar, ToFrame>> const point_in_camera = to_camera_(point);
+  Vector<Scalar, ToFrame> const displacement_in_camera =
+      point_in_camera - ToFrame::origin;
+  R3Element<Scalar> const coordinates_in_camera =
+      displacement_in_camera.coordinates();
+  // This is the actual pinhole camera projection.
+  return RP2Element<Scalar>(coordinates_in_camera.x,
+                            coordinates_in_camera.y,
+                            coordinates_in_camera.z / focal_);
+}
+
+}  // namespace internal_perspective
+}  // namespace geometry
+}  // namespace principia
diff --git a/geometry/perspective_test.cpp b/geometry/perspective_test.cpp
@@ -0,0 +1,115 @@
+
+#pragma once
+
+#include "geometry/affine_map.hpp"
+#include "geometry/frame.hpp"
+#include "geometry/orthogonal_map.hpp"
+#include "geometry/named_quantities.hpp"
+#include "geometry/perspective.hpp"
+#include "geometry/rotation.hpp"
+#include "geometry/rp2_element.hpp"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+#include "quantities/elementary_functions.hpp"
+#include "quantities/quantities.hpp"
+#include "quantities/si.hpp"
+#include "testing_utilities/almost_equals.hpp"
+#include "testing_utilities/componentwise.hpp"
+#include "testing_utilities/vanishes_before.hpp"
+
+namespace principia {
+namespace geometry {
+namespace internal_perspective {
+
+using quantities::Length;
+using quantities::Sqrt;
+using quantities::si::Metre;
+using quantities::si::Radian;
+using testing_utilities::AlmostEquals;
+using testing_utilities::Componentwise;
+using testing_utilities::VanishesBefore;
+using ::testing::Eq;
+
+class PerspectiveTest : public ::testing::Test {
+ protected:
+  using World = Frame<serialization::Frame::TestTag,
+                      serialization::Frame::TEST1, false>;
+  using Camera = Frame<serialization::Frame::TestTag,
+                       serialization::Frame::TEST2, false>;
+};
+
+TEST_F(PerspectiveTest, Basic) {
+  Point<Displacement<World>> const camera_origin =
+      World::origin + Displacement<World>({1 * Metre, 2 * Metre, -3 * Metre});
+  Rotation<World, Camera> const world_to_camera_rotation(
+      π / 6 * Radian,
+      π / 4 * Radian,
+      π / 3 * Radian,
+      CardanoAngles::ZYX,
+      DefinesFrame<Camera>());
+  AffineMap<World, Camera, Length, OrthogonalMap> const world_to_camera_affine(
+      camera_origin,
+      Camera::origin,
+      world_to_camera_rotation.Forget());
+  Perspective<World, Camera, Length, OrthogonalMap> perspective(
+      world_to_camera_affine,
+      /*focal=*/10 * Metre);
+
+  // Check that points in the camera z axis get projected to the origin of ℝP².
+  Displacement<World> const camera_z_axis = world_to_camera_rotation.Inverse()(
+      Displacement<Camera>({0 * Metre, 0 * Metre, 1 * Metre}));
+  Point<Displacement<World>> const p0 = camera_origin;
+  Point<Displacement<World>> const p1 = camera_origin + 1 * camera_z_axis;
+  Point<Displacement<World>> const p2 = camera_origin + 10 * camera_z_axis;
+  EXPECT_TRUE(perspective(p0).is_at_infinity());
+  EXPECT_THAT(perspective(p1),
+              Componentwise(VanishesBefore(1 * Metre, 10),
+                            VanishesBefore(1 * Metre, 0)));
+  EXPECT_THAT(perspective(p2),
+              Componentwise(VanishesBefore(1 * Metre, 8),
+                            VanishesBefore(1 * Metre, 4)));
+
+  // Check that points on the camera x axis get projected on the x axis of ℝP².
+  Displacement<World> const camera_x_axis = world_to_camera_rotation.Inverse()(
+      Displacement<Camera>({1 * Metre, 0 * Metre, 0 * Metre}));
+  Point<Displacement<World>> const p3 = p1 + 5 * camera_x_axis;
+  Point<Displacement<World>> const p4 = p1 + 7 * camera_x_axis;
+  EXPECT_THAT(perspective(p3).y(), VanishesBefore(1 * Metre, 20));
+  EXPECT_THAT(perspective(p4).y(), VanishesBefore(1 * Metre, 10));
+
+  // Check that points on the camera y axis get projected on the y axis of ℝP².
+  Displacement<World> const camera_y_axis = world_to_camera_rotation.Inverse()(
+      Displacement<Camera>({0 * Metre, 1 * Metre, 0 * Metre}));
+  Point<Displacement<World>> const p5 = p1 - 11 * camera_y_axis;
+  Point<Displacement<World>> const p6 = p1 + 13 * camera_y_axis;
+  EXPECT_THAT(perspective(p5).x(), VanishesBefore(1 * Metre, 120));
+  EXPECT_THAT(perspective(p6).x(), VanishesBefore(1 * Metre, 0));
+
+  // Check that aligned points are aligned in ℝP².
+  Point<Displacement<World>> const p7 =
+      camera_origin +
+      Displacement<World>({17 * Metre, -23 * Metre, 29 * Metre});
+  Point<Displacement<World>> const p8 =
+      camera_origin +
+      Displacement<World>({18 * Metre, -21 * Metre, 24 * Metre});
+  Point<Displacement<World>> const p9 =
+      camera_origin +
+      Displacement<World>({19 * Metre, -19 * Metre, 19 * Metre});
+  RP2Element<Length> const q7 = perspective(p7);
+  RP2Element<Length> const q8 = perspective(p8);
+  RP2Element<Length> const q9 = perspective(p9);
+  EXPECT_THAT((q8.x() - q7.x()) * (q9.y() - q7.y()) -
+                  (q9.x() - q7.x()) * (q8.y() - q7.y()),
+              VanishesBefore(1 * Metre * Metre, 6));
+
+  // Check that the focal works as expected.
+  Point<Displacement<World>> const p10 =
+      camera_origin + 1 * camera_x_axis + 2 * camera_y_axis + 3 * camera_z_axis;
+  EXPECT_THAT(perspective(p10),
+              Componentwise(AlmostEquals(1.0 / 0.3 * Metre, 3),
+                            AlmostEquals(2.0 / 0.3 * Metre, 2)));
+}
+
+}  // namespace internal_perspective
+}  // namespace geometry
+}  // namespace principia
diff --git a/geometry/rp2_element.hpp b/geometry/rp2_element.hpp
@@ -58,6 +58,9 @@ std::ostream& operator<<(std::ostream& os,
                          RP2Element<Scalar> const& rp2_element);
 
 }  // namespace internal_rp2_element
+
+using internal_rp2_element::RP2Element;
+
 }  // namespace geometry
 }  // namespace principia
 

diff --git a/testing_utilities/componentwise.hpp b/testing_utilities/componentwise.hpp
@@ -9,6 +9,7 @@
 #include "geometry/grassmann.hpp"
 #include "geometry/pair.hpp"
 #include "geometry/r3_element.hpp"
+#include "geometry/rp2_element.hpp"
 #include "gmock/gmock.h"
 #include "quantities/quantities.hpp"
 
@@ -46,6 +47,9 @@ class ComponentwiseMatcher2 final {
   template<typename T1, typename T2>
   bool MatchAndExplain(geometry::Pair<T1, T2> const& actual,
                        testing::MatchResultListener* listener) const;
+  template<typename Scalar>
+  bool MatchAndExplain(geometry::RP2Element<Scalar> const& actual,
+                       testing::MatchResultListener* listener) const;
 
   void DescribeTo(std::ostream* out) const;
   void DescribeNegationTo(std::ostream* out) const;

diff --git a/testing_utilities/componentwise_body.hpp b/testing_utilities/componentwise_body.hpp
@@ -104,6 +104,24 @@ bool ComponentwiseMatcher2<T1Matcher, T2Matcher>::MatchAndExplain(
   return t1_matches && t2_matches;
 }
 
+template<typename T1Matcher, typename T2Matcher>
+template<typename Scalar>
+bool ComponentwiseMatcher2<T1Matcher, T2Matcher>::MatchAndExplain(
+    geometry::RP2Element<Scalar> const& actual,
+    testing::MatchResultListener* listener) const {
+  bool const x_matches = Matcher<Scalar>(t1_matcher_).MatchAndExplain(
+                              actual.x(), listener);
+  if (!x_matches) {
+    *listener << " in the x coordinate; ";
+  }
+  bool const y_matches = Matcher<Scalar>(t2_matcher_).MatchAndExplain(
+                              actual.y(), listener);
+  if (!y_matches) {
+    *listener << " in the y coordinate; ";
+  }
+  return x_matches && y_matches;
+}
+
 template<typename T1Matcher, typename T2Matcher>
 void ComponentwiseMatcher2<T1Matcher, T2Matcher>::DescribeTo(
     std::ostream* out) const {