Nbuono/sfm/attempt 1

common/geometry/BUILD

@@ -38,4 +38,31 @@ cc_test(
     ":translate_types",
     "@com_google_googletest//:gtest_main"
   ]
-)
+)
+
+
+cc_library(
+  name = "camera",
+  hdrs = ["camera.hh"],
+  srcs = ["camera.cc"],
+  visibility = ["//visibility:public"],
+  deps = [
+    "@eigen",     
+    "@opencv",
+    ":translate_types",
+    "@gtsam",
+    "//common:check"
+  ]
+)
+
+cc_test(
+  name = "camera_test", 
+  srcs = ["camera_test.cc"],
+  visibility = ["//visibility:public"],
+  deps = [
+    ":camera",
+    "@com_google_googletest//:gtest_main",
+    "//visualization/opencv:opencv_viz",
+    ":translate_types"
+  ]
+)

common/geometry/camera.cc

@@ -0,0 +1,63 @@
+#include "common/geometry/camera.hh"
+
+#include <exception>
+#include <optional>
+
+#include "common/check.hh"
+#include "common/geometry/translate_types.hh"
+
+namespace robot::geometry {
+Eigen::Matrix3d get_intrinsic_matrix(const gtsam::Cal3_S2 &intrinsic) {
+    Eigen::Matrix3d K;
+    K << intrinsic.fx(), intrinsic.skew(), intrinsic.px(), 0, intrinsic.fy(), intrinsic.py(), 0, 0,
+        1;
+    return K;
+}
+
+Eigen::Vector3d project(const Eigen::Matrix3d &K, const Eigen::Vector3d &p_cam_point) {
+    return K * p_cam_point;
+}
+
+Eigen::Vector3d deproject(const Eigen::Matrix3d &K, const Eigen::Vector3d &pixel_homog) {
+    return K.inverse() * pixel_homog;
+}
+
+Eigen::Vector3d deproject(const Eigen::Matrix3d &K, const Eigen::Vector2d &pixel_inhomog,
+                          const double depth) {
+    return depth * K.inverse() * Eigen::Vector3d(pixel_inhomog(0), pixel_inhomog(1), 1.);
+}
+
+std::optional<Eigen::Isometry3d> estimate_cam0_from_cam1(const std::vector<cv::KeyPoint> &kpts0,
+                                                         const std::vector<cv::KeyPoint> &kpts1,
+                                                         const std::vector<cv::DMatch> &matches,
+                                                         const cv::Mat &K) {
+    ROBOT_CHECK(matches.size() >= 5 && kpts1.size() >= 5 && kpts0.size() >= 5);
+    Eigen::Isometry3d result;
+    std::vector<cv::Point2f> pts1;
+    std::vector<cv::Point2f> pts2;
+    for (const cv::DMatch &match : matches) {
+        pts1.push_back(kpts0[match.queryIdx].pt);
+        pts2.push_back(kpts1[match.trainIdx].pt);
+    }
+    ROBOT_CHECK(pts1.size() == pts2.size() && pts1.size() >= 5);
+    try {
+        cv::Mat E = cv::findEssentialMat(pts1, pts2, K, cv::RANSAC, 0.999, 1.0);
+        cv::Mat R_c1_c0, t_c1_c0;
+        ROBOT_CHECK(!E.empty(), "Essential matrix is empty.");
+        // TOOD: handle multiple returned candidate E matrices better (they are stacked on top of
+        // each other in E)
+        if (E.rows > 3) {
+            E = E.rowRange(0, 3);
+        }
+        cv::recoverPose(E, pts1, pts2, K, R_c1_c0, t_c1_c0);
+        result.linear() = cv_to_eigen_mat(R_c1_c0);
+        result.translation() = cv_to_eigen_mat(t_c1_c0);
+        result = result.inverse();
+        return result;
+    } catch (const std::exception &e) {
+        std::cerr << "Failed to estimate pose up to scale cam0_from_cam1.\n"
+                  << e.what() << std::endl;
+        return std::nullopt;
+    }
+}
+}  // namespace robot::geometry

common/geometry/camera.hh

@@ -0,0 +1,30 @@
+#pragma once
+#include <optional>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "gtsam/geometry/Cal3_S2.h"
+#include "opencv2/opencv.hpp"
+
+namespace robot::geometry {
+Eigen::Matrix3d get_intrinsic_matrix(const gtsam::Cal3_S2 &intrinsic);
+
+/// @return Homogeneous pixel projection coordinates
+Eigen::Vector3d project(const Eigen::Matrix3d &K, const Eigen::Vector3d &p_cam_point);
+/**
+ * @param pixel_homog homogeneous pixel cordinates 3x1
+ * @return 3d coordinate in camera frame
+ */
+Eigen::Vector3d deproject(const Eigen::Matrix3d &K, const Eigen::Vector3d &pixel_homog);
+/**
+ * @param pixel_inhomog inhomogeneous pixel cordinates 2x1
+ * @return 3d coordinate in camera frame
+ */
+Eigen::Vector3d deproject(const Eigen::Matrix3d &K, const Eigen::Vector2d &pixel_inhomog,
+                          const double depth);
+
+std::optional<Eigen::Isometry3d> estimate_cam0_from_cam1(const std::vector<cv::KeyPoint> &kpts0,
+                                                         const std::vector<cv::KeyPoint> &kpts1,
+                                                         const std::vector<cv::DMatch> &matches,
+                                                         const cv::Mat &K);
+}  // namespace robot::geometry

common/geometry/camera_test.cc

@@ -0,0 +1,129 @@
+#include "common/geometry/camera.hh"
+
+#include <optional>
+
+#include "common/geometry/translate_types.hh"
+#include "gtest/gtest.h"
+#include "visualization/opencv/opencv_viz.hh"
+
+class CameraTestHelper {
+   public:
+    static bool pixel_in_range(Eigen::Vector2d pixel, size_t img_width, size_t img_height) {
+        return pixel[0] > 0 && pixel[0] < img_width && pixel[1] > 0 && pixel[1] < img_height;
+    }
+};
+
+namespace robot::geometry {
+TEST(CameraTest, test_get_intrinsics) {
+    const double fx = 1., fy = 1., s = 0, px = 0.5, py = 0.5;
+    const gtsam::Cal3_S2 K(fx, fy, s, px, py);
+    const Eigen::Matrix3d intrinsic_mat = get_intrinsic_matrix(K);
+
+    EXPECT_FLOAT_EQ(fx, intrinsic_mat(0, 0));
+    EXPECT_FLOAT_EQ(fy, intrinsic_mat(1, 1));
+    EXPECT_FLOAT_EQ(s, intrinsic_mat(0, 1));
+    EXPECT_FLOAT_EQ(px, intrinsic_mat(0, 2));
+    EXPECT_FLOAT_EQ(py, intrinsic_mat(1, 2));
+}
+
+TEST(CameraTest, test_projection) {
+    const double fx = 1., fy = 1., s = 0, px = 0.5, py = 0.5;
+    const gtsam::Cal3_S2 K(fx, fy, s, px, py);
+    const Eigen::Matrix3d intrinsic_mat = get_intrinsic_matrix(K);
+
+    const Eigen::Vector3d p_cam_lmk1(0.1, 0.15, 1.2);
+    const Eigen::Vector3d pxl_homog_cam_lmk1 = project(intrinsic_mat, p_cam_lmk1);
+    const Eigen::Vector2d pxl_inhomog_cam_lmk1 =
+        pxl_homog_cam_lmk1.head<2>() / pxl_homog_cam_lmk1(2);
+    const Eigen::Vector3d p_cam_lmk1_deproject = deproject(intrinsic_mat, pxl_homog_cam_lmk1);
+    const Eigen::Vector3d p_cam_lmk1_deproject_inhomog =
+        deproject(intrinsic_mat, pxl_inhomog_cam_lmk1, p_cam_lmk1(2));
+
+    EXPECT_TRUE(p_cam_lmk1.isApprox(p_cam_lmk1_deproject));
+    EXPECT_TRUE(p_cam_lmk1.isApprox(p_cam_lmk1_deproject_inhomog));
+}
+
+TEST(CameraTest, test_estimate_pose) {
+    std::vector<Eigen::Vector3d> p_W_cube;
+    float cube_size = 1.0f;
+    p_W_cube.push_back(Eigen::Vector3d(0, 0, 0));
+    p_W_cube.push_back(Eigen::Vector3d(cube_size, 0, 0));
+    p_W_cube.push_back(Eigen::Vector3d(cube_size, cube_size, 0));
+    p_W_cube.push_back(Eigen::Vector3d(0, cube_size, 0));
+    p_W_cube.push_back(Eigen::Vector3d(0, 0, cube_size));
+    p_W_cube.push_back(Eigen::Vector3d(cube_size, 0, cube_size));
+    p_W_cube.push_back(Eigen::Vector3d(cube_size, cube_size, cube_size));
+    p_W_cube.push_back(Eigen::Vector3d(0, cube_size, cube_size));
+
+    Eigen::Matrix3d R_new_points(
+        Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() *
+        Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(1, 0, 0)).toRotationMatrix());
+
+    // const int initial_size = p_W_cube.size();
+    const Eigen::Vector3d p_world_cube_center(cube_size / 2, cube_size / 2, cube_size / 2);
+    for (const Eigen::Vector3d &point_W_cube : p_W_cube) {
+        p_W_cube.emplace_back(R_new_points * (point_W_cube - p_world_cube_center) +
+                              p_world_cube_center);
+    }
+
+    const size_t img_width = 640;
+    const size_t img_height = 480;
+    const double fx = 500.0;
+    const double fy = fx;
+    const double cx = img_width / 2.0;
+    const double cy = img_height / 2.0;
+
+    cv::Mat K = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
+
+    Eigen::Matrix3d K_eig = cv_to_eigen_mat(K);
+
+    Eigen::Matrix3d R_world_from_cam0(
+        Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix() *
+        Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d(1, 0, 0)).toRotationMatrix());
+    Eigen::Isometry3d world_from_cam0 = Eigen::Isometry3d::Identity();
+    world_from_cam0.linear() = R_world_from_cam0;
+    world_from_cam0.translation() = Eigen::Vector3d(4, cube_size / 2, cube_size / 2);
+
+    Eigen::Matrix3d R_world_45deg(
+        Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix());
+    Eigen::Isometry3d world_from_cam1;
+    world_from_cam1.linear() = R_world_45deg * world_from_cam0.linear();
+    world_from_cam1.translation() =
+        R_world_45deg * (world_from_cam0.translation() - p_world_cube_center) + p_world_cube_center;
+
+    std::vector<cv::KeyPoint> kpts0;
+    std::vector<cv::KeyPoint> kpts1;
+    std::vector<cv::DMatch> matches;
+
+    for (const Eigen::Vector3d &point_W_cube : p_W_cube) {
+        Eigen::Vector3d p_cube_points_in_cam0 =
+            (world_from_cam0.inverse() *
+             Eigen::Vector4d(point_W_cube(0), point_W_cube(1), point_W_cube(2), 1.))
+                .head<3>();
+        Eigen::Vector3d p_cube_points_in_cam1 =
+            (world_from_cam1.inverse() *
+             Eigen::Vector4d(point_W_cube(0), point_W_cube(1), point_W_cube(2), 1.))
+                .head<3>();
+        Eigen::Vector3d pxl_c0_pcube_homog = K_eig * p_cube_points_in_cam0;
+        Eigen::Vector3d pxl_c1_pcube_homog = K_eig * p_cube_points_in_cam1;
+        Eigen::Vector2d pxl_c0_pcube = pxl_c0_pcube_homog.head<2>() / pxl_c0_pcube_homog(2);
+        Eigen::Vector2d pxl_c1_pcube = pxl_c1_pcube_homog.head<2>() / pxl_c1_pcube_homog(2);
+        if (CameraTestHelper::pixel_in_range(pxl_c0_pcube, img_width, img_height) &&
+            CameraTestHelper::pixel_in_range(pxl_c1_pcube, img_width, img_height)) {
+            kpts0.emplace_back(pxl_c0_pcube[0], pxl_c0_pcube[1], 3);
+            kpts1.emplace_back(pxl_c1_pcube[0], pxl_c1_pcube[1], 3);
+            matches.emplace_back(kpts0.size() - 1, kpts1.size() - 1, 0);
+        }
+    }
+
+    std::optional<Eigen::Isometry3d> cam0_from_cam1_estimate =
+        estimate_cam0_from_cam1(kpts0, kpts1, matches, K);
+    if (cam0_from_cam1_estimate) {
+        Eigen::Isometry3d cam0_from_cam1 = world_from_cam0.inverse() * world_from_cam1;
+        cam0_from_cam1.translation() /= cam0_from_cam1.translation().norm();
+        EXPECT_TRUE(cam0_from_cam1_estimate->translation().isApprox(cam0_from_cam1.translation(),
+                                                                    0.000001));
+        EXPECT_TRUE(cam0_from_cam1_estimate->linear().isApprox(cam0_from_cam1.linear(), 0.001));
+    }
+}
+}  // namespace robot::geometry

common/gps/BUILD

@@ -19,6 +19,29 @@ cc_test(
   ]
 )
 
+cc_library(
+  name = "frame_translation",
+  srcs = ["frame_translation.cc"],
+  hdrs = ["frame_translation.hh"],
+  visibility = ["//visibility:public"],
+  deps = [
+    "@com_google_googletest//:gtest_main",
+    "@geographiclib//:geographiclib",
+    "@eigen"
+  ]
+)
+
+cc_test(
+  name = "frame_translation_test",
+  srcs = ["frame_translation_test.cc"],
+  deps = [
+    "@com_google_googletest//:gtest_main",
+    ":frame_translation",
+    "@eigen",
+    "//common:check"
+  ]
+)
+
 py_library(
   name = "web_mercator",
   srcs = ["web_mercator.py"],

common/gps/frame_translation.cc

@@ -0,0 +1,32 @@
+#include "common/gps/frame_translation.hh"
+
+#include "GeographicLib/Constants.hpp"
+#include "GeographicLib/Geocentric.hpp"
+#include "GeographicLib/Geodesic.hpp"
+#include "GeographicLib/LocalCartesian.hpp"
+
+namespace robot::gps {
+Eigen::Vector3d lla_from_ecef(const Eigen::Vector3d& t_place_from_ECEF) {
+    static const GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
+
+    double x = t_place_from_ECEF.x(), y = t_place_from_ECEF.y(), z = t_place_from_ECEF.z();
+
+    double lat_deg, lon_deg, alt_m;
+    earth.Reverse(x, y, z, lat_deg, lon_deg, alt_m);
+
+    return Eigen::Vector3d(lat_deg, lon_deg, alt_m);
+}
+
+Eigen::Vector3d ecef_from_lla(const Eigen::Vector3d& gcs_coordinate) {
+    double lat_deg = gcs_coordinate.x();
+    double lon_deg = gcs_coordinate.y();
+    double alt_m = gcs_coordinate.z();
+
+    double x, y, z;
+
+    static const GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
+    earth.Forward(lat_deg, lon_deg, alt_m, x, y, z);
+
+    return Eigen::Vector3d(x, y, z);
+}
+}  // namespace robot::gps

common/gps/frame_translation.hh

@@ -0,0 +1,8 @@
+#pragma once
+
+#include "Eigen/Core"
+
+namespace robot::gps {
+Eigen::Vector3d lla_from_ecef(const Eigen::Vector3d& t_place_from_ECEF);
+Eigen::Vector3d ecef_from_lla(const Eigen::Vector3d& gcs_coordinate);
+}  // namespace robot::gps

common/gps/frame_translation_test.cc

@@ -0,0 +1,29 @@
+#include "common/gps/frame_translation.hh"
+
+#include "Eigen/Core"
+#include "common/check.hh"
+#include "gtest/gtest.h"
+
+namespace robot::gps {
+TEST(frame_translation_test, lla_from_ecef) {
+    const Eigen::Vector3d lla_easthampton_ma(42.2608, -72.6634, 51.82);  //
+    const Eigen::Vector3d ecef_easthampton_ma = ecef_from_lla(lla_easthampton_ma);
+
+    const Eigen::Vector3d ecef_expected_easthampton_ma(1408753.87, -4512832.60, 4267122.34);
+
+    constexpr double tol = 1e-9;
+    ROBOT_CHECK(ecef_easthampton_ma.isApprox(ecef_expected_easthampton_ma, tol),
+                ecef_easthampton_ma, ecef_expected_easthampton_ma);
+}
+
+TEST(frame_translation_test, ecef_from_lla) {
+    const Eigen::Vector3d ecef_palace_versailles(4205987.87, 155694.68, 4776399.15);
+    const Eigen::Vector3d lla_palace_versailles = lla_from_ecef(ecef_palace_versailles);
+
+    const Eigen::Vector3d expected_lla_palace_versailles(48.8049440, 2.1199720, 131.985);
+
+    constexpr double tol = 1e-4;
+    ROBOT_CHECK(lla_palace_versailles.isApprox(expected_lla_palace_versailles, tol),
+                lla_palace_versailles, expected_lla_palace_versailles);
+}
+}  // namespace robot::gps

common/gps/geographiclib_test.cc

@@ -8,7 +8,7 @@
 #include "GeographicLib/LocalCartesian.hpp"
 #include "gtest/gtest.h"
 
-namespace robot::experimental::learn_descriptors {
+namespace robot::gps {
 TEST(GeographiclibTest, airport_dist) {
     const GeographicLib::Geodesic& geod = GeographicLib::Geodesic::WGS84();
     // Distance from JFK to LHR
@@ -58,4 +58,4 @@ TEST(GeographiclibTest, local_cartestian) {
         EXPECT_NEAR(calais_height_meters, 264.915, 1e-3);
     }
 }
-}  // namespace robot::experimental::learn_descriptors
+}  // namespace robot::gps

common/gps/nmea_test.cc

@@ -2,7 +2,7 @@
 #include "nmea/message/gga.hpp"
 #include "nmea/sentence.hpp"
 
-namespace robot::experimental::learn_descriptors {
+namespace robot::gps {
 TEST(NmeaTest, parse) {
     // Read an NMEA string from your serial port
     std::string nmea_string =
@@ -44,4 +44,4 @@ TEST(NmeaTest, generate) {
     EXPECT_EQ(nmea_sentence.get_field(1), "abc");
     EXPECT_EQ(nmea_sentence.get_field(2), "");
 }
-}  // namespace robot::experimental::learn_descriptors
+}  // namespace robot::gps