add opengv

WORKSPACE

@@ -438,3 +438,13 @@ load("@rules_gtsam//bzl:init_deps.bzl", "gtsam_init_deps")
 gtsam_init_deps()
 load("@rules_gtsam//bzl:repositories.bzl", "gtsam_repositories")
 gtsam_repositories()
+
+http_archive(
+  name = "opengv",
+  urls = ["https://github.com/laurentkneip/opengv/archive/91f4b19c73450833a40e463ad3648aae80b3a7f3.zip"],
+  strip_prefix="opengv-91f4b19c73450833a40e463ad3648aae80b3a7f3",
+  build_file="//third_party:BUILD.opengv",
+  patch_args = ["-p1"],
+  patches = ["//third_party:opengv_0001-prefix-unsupported-eigen-include-paths.patch"],
+  integrity = "sha256-gIK3IvE6rGDpxvN3BA+EPFKPvZ7Zi7Ix33IIcm2RULA="
+)

third_party/BUILD.eigen

@@ -35,6 +35,8 @@ cc_library(
     "Eigen/UmfPackSupport",
     "unsupported/Eigen/AutoDiff",
     "unsupported/Eigen/Polynomials",
+    "unsupported/Eigen/NonLinearOptimization",
+    "unsupported/Eigen/NumericalDiff",
   ],
   srcs = glob(["Eigen/src/**/*"]) +
   [
@@ -43,6 +45,18 @@ cc_library(
     "unsupported/Eigen/src/Polynomials/Companion.h",
     "unsupported/Eigen/src/Polynomials/PolynomialUtils.h",
     "unsupported/Eigen/src/Polynomials/PolynomialSolver.h",
+    "unsupported/Eigen/src/NonLinearOptimization/chkder.h",
+    "unsupported/Eigen/src/NonLinearOptimization/covar.h",
+    "unsupported/Eigen/src/NonLinearOptimization/dogleg.h",
+    "unsupported/Eigen/src/NonLinearOptimization/fdjac1.h",
+    "unsupported/Eigen/src/NonLinearOptimization/lmpar.h",
+    "unsupported/Eigen/src/NonLinearOptimization/qrsolv.h",
+    "unsupported/Eigen/src/NonLinearOptimization/rwupdt.h",
+    "unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h",
+    "unsupported/Eigen/src/NonLinearOptimization/r1updt.h",
+    "unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h",
+    "unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h",
+    "unsupported/Eigen/src/NumericalDiff/NumericalDiff.h",
   ],
   # Other third party dependencies often pull in Eigen using angle brackets
   # This makes it so that Eigen is available at both "Eigen/Core" and <Eigen/Core>

third_party/BUILD.opengv

@@ -0,0 +1,45 @@
+
+cc_library(
+    name = "opengv",
+    hdrs = glob(["include/**/*.hpp", "include/*.hpp"]),
+    srcs = glob(["src/**/*.cpp"]),
+    strip_include_prefix="include",
+    deps = ["@eigen"],
+    visibility=["//visibility:public"],
+)
+
+cc_library(
+    name = "random_generators",
+    hdrs = [
+        "test/random_generators.hpp",
+        "test/experiment_helpers.hpp",
+        "test/time_measurement.hpp",
+    ],
+    srcs = [
+        "test/random_generators.cpp",
+        "test/experiment_helpers.cpp",
+        "test/time_measurement.cpp",
+    ],
+    strip_include_prefix="test",
+    deps = [":opengv"],
+)
+
+cc_test(
+    name = "test_absolute_pose",
+    srcs = ["test/test_absolute_pose.cpp"],
+    deps = [
+        ":opengv",
+        ":random_generators",
+    ],
+)
+
+cc_test(
+    name = "test_absolute_pose_sac",
+    srcs = ["test/test_absolute_pose_sac.cpp"],
+    deps = [
+        ":opengv",
+        ":random_generators",
+    ],
+)
+
+# TODO: additional tests and python bindings

third_party/opengv_0001-prefix-unsupported-eigen-include-paths.patch

@@ -0,0 +1,165 @@
+diff --git a/src/absolute_pose/methods.cpp b/src/absolute_pose/methods.cpp
+index b1f0889..84c3ef4 100644
+--- a/src/absolute_pose/methods.cpp
++++ b/src/absolute_pose/methods.cpp
+@@ -32,8 +32,8 @@
+ #include <opengv/absolute_pose/methods.hpp>
+ #include <opengv/Indices.hpp>
+
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ #include <opengv/absolute_pose/modules/main.hpp>
+ #include <opengv/absolute_pose/modules/Epnp.hpp>
+diff --git a/src/absolute_pose/modules/main.cpp b/src/absolute_pose/modules/main.cpp
+index ed0c271..a179aaf 100644
+--- a/src/absolute_pose/modules/main.cpp
++++ b/src/absolute_pose/modules/main.cpp
+@@ -29,8 +29,8 @@
+  ******************************************************************************/
+
+
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ #include <opengv/absolute_pose/modules/main.hpp>
+ #include <opengv/absolute_pose/modules/gp3p/modules.hpp>
+diff --git a/src/point_cloud/methods.cpp b/src/point_cloud/methods.cpp
+index 5409eeb..ed69488 100644
+--- a/src/point_cloud/methods.cpp
++++ b/src/point_cloud/methods.cpp
+@@ -32,8 +32,8 @@
+ #include <opengv/point_cloud/methods.hpp>
+ #include <opengv/Indices.hpp>
+
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ #include <opengv/OptimizationFunctor.hpp>
+ #include <opengv/math/arun.hpp>
+diff --git a/src/relative_pose/methods.cpp b/src/relative_pose/methods.cpp
+index 0027dae..939ad6e 100644
+--- a/src/relative_pose/methods.cpp
++++ b/src/relative_pose/methods.cpp
+@@ -32,8 +32,8 @@
+ #include <opengv/relative_pose/methods.hpp>
+ #include <opengv/Indices.hpp>
+
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ #include <opengv/OptimizationFunctor.hpp>
+ #include <opengv/math/arun.hpp>
+diff --git a/src/relative_pose/modules/fivept_nister/modules.cpp b/src/relative_pose/modules/fivept_nister/modules.cpp
+index 4b134c5..b4f8d7d 100644
+--- a/src/relative_pose/modules/fivept_nister/modules.cpp
++++ b/src/relative_pose/modules/fivept_nister/modules.cpp
+@@ -30,8 +30,8 @@
+
+
+ #include <opengv/relative_pose/modules/fivept_nister/modules.hpp>
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ #include <opengv/OptimizationFunctor.hpp>
+
+diff --git a/src/relative_pose/modules/main.cpp b/src/relative_pose/modules/main.cpp
+index 674c36b..a8c6ed4 100644
+--- a/src/relative_pose/modules/main.cpp
++++ b/src/relative_pose/modules/main.cpp
+@@ -31,8 +31,8 @@
+
+ #include <math.h>
+ #include <vector>
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ #include <opengv/relative_pose/modules/main.hpp>
+ #include <opengv/relative_pose/modules/fivept_nister/modules.hpp>
+diff --git a/src/sac_problems/relative_pose/CentralRelativePoseSacProblem.cpp b/src/sac_problems/relative_pose/CentralRelativePoseSacProblem.cpp
+index cc48dfb..2c231eb 100644
+--- a/src/sac_problems/relative_pose/CentralRelativePoseSacProblem.cpp
++++ b/src/sac_problems/relative_pose/CentralRelativePoseSacProblem.cpp
+@@ -32,8 +32,8 @@
+ #include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
+ #include <opengv/relative_pose/methods.hpp>
+ #include <opengv/triangulation/methods.hpp>
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ bool
+ opengv::sac_problems::
+diff --git a/src/sac_problems/relative_pose/MultiCentralRelativePoseSacProblem.cpp b/src/sac_problems/relative_pose/MultiCentralRelativePoseSacProblem.cpp
+index 25f446a..7a44d30 100644
+--- a/src/sac_problems/relative_pose/MultiCentralRelativePoseSacProblem.cpp
++++ b/src/sac_problems/relative_pose/MultiCentralRelativePoseSacProblem.cpp
+@@ -33,8 +33,8 @@
+
+ #include <opengv/relative_pose/methods.hpp>
+ #include <opengv/triangulation/methods.hpp>
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ bool
+ opengv::sac_problems::
+diff --git a/src/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.cpp b/src/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.cpp
+index cd9596e..0c35cc9 100644
+--- a/src/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.cpp
++++ b/src/sac_problems/relative_pose/MultiNoncentralRelativePoseSacProblem.cpp
+@@ -33,8 +33,8 @@
+
+ #include <opengv/relative_pose/methods.hpp>
+ #include <opengv/triangulation/methods.hpp>
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ #include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
+
+diff --git a/src/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.cpp b/src/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.cpp
+index df58181..6ad47af 100644
+--- a/src/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.cpp
++++ b/src/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.cpp
+@@ -32,8 +32,8 @@
+ #include <opengv/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.hpp>
+ #include <opengv/relative_pose/methods.hpp>
+ #include <opengv/triangulation/methods.hpp>
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ #include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
+
+diff --git a/src/sac_problems/relative_pose/RotationOnlySacProblem.cpp b/src/sac_problems/relative_pose/RotationOnlySacProblem.cpp
+index af04c3c..c11cf10 100644
+--- a/src/sac_problems/relative_pose/RotationOnlySacProblem.cpp
++++ b/src/sac_problems/relative_pose/RotationOnlySacProblem.cpp
+@@ -32,8 +32,8 @@
+ #include <opengv/sac_problems/relative_pose/RotationOnlySacProblem.hpp>
+ #include <opengv/relative_pose/methods.hpp>
+ #include <opengv/triangulation/methods.hpp>
+-#include <Eigen/NonLinearOptimization>
+-#include <Eigen/NumericalDiff>
++#include <unsupported/Eigen/NonLinearOptimization>
++#include <unsupported/Eigen/NumericalDiff>
+
+ bool
+ opengv::sac_problems::