From 616d441cb607a26f14945b61e3a40669adfe541a Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Fri, 18 Aug 2023 17:43:25 -0700 Subject: [PATCH 1/4] copied fork of sophus to cpp from https://github.com/strasdat/Sophus/commits/sophus2 sha: 27056f4e8aca3d36ffe6796149df5388c28767fc --- cpp/sophus2/.clang-format | 116 + cpp/sophus2/.clang-tidy | 84 + .../.github/ISSUE_TEMPLATE/bug_report.md | 27 + .../.github/ISSUE_TEMPLATE/feature_request.md | 21 + cpp/sophus2/.github/workflows/build.yml | 181 + cpp/sophus2/.github/workflows/docs.yml | 34 + cpp/sophus2/.github/workflows/format.yml | 16 + .../.github/workflows/pypi-release.yml | 21 + cpp/sophus2/.github/workflows/sympy.yml | 24 + cpp/sophus2/.gitignore | 10 + cpp/sophus2/.pre-commit-config.yaml | 62 + cpp/sophus2/.vscode/extensions.json | 7 + cpp/sophus2/CMakeLists.txt | 57 + cpp/sophus2/LICENSE.txt | 23 + cpp/sophus2/README.md | 198 + cpp/sophus2/Sophus.code-workspace | 117 + cpp/sophus2/cpp/CMakeLists.txt | 25 + .../test_install_targets/CMakeLists.txt | 61 + .../compile_all_proto.cpp | 28 + .../test_install_targets/hello_se3.cpp | 34 + .../test_install_targets/hello_so3.cpp | 47 + .../test_install_targets/image_sensor.cpp | 30 + cpp/sophus2/cpp/run_format.sh | 1 + cpp/sophus2/cpp/sophus/CMakeLists.txt | 10 + cpp/sophus2/cpp/sophus/SophusConfig.cmake.in | 0 .../cpp/sophus/calculus/CMakeLists.txt | 21 + cpp/sophus2/cpp/sophus/calculus/num_diff.h | 96 + cpp/sophus2/cpp/sophus/calculus/region.h | 371 + .../cpp/sophus/calculus/region_test.cpp | 82 + cpp/sophus2/cpp/sophus/ceres/CMakeLists.txt | 35 + cpp/sophus2/cpp/sophus/ceres/ceres_test.cpp | 1116 + cpp/sophus2/cpp/sophus/ceres/jet_helpers.h | 33 + cpp/sophus2/cpp/sophus/ceres/manifold.h | 84 + .../cpp/sophus/ceres/manifold_test.cpp | 359 + cpp/sophus2/cpp/sophus/ceres/typetraits.h | 67 + cpp/sophus2/cpp/sophus/common/CMakeLists.txt | 25 + cpp/sophus2/cpp/sophus/common/common.h | 112 + cpp/sophus2/cpp/sophus/common/common_test.cpp | 32 + cpp/sophus2/cpp/sophus/common/enum.h | 15 + .../cpp/sophus/concepts/CMakeLists.txt | 35 + .../cpp/sophus/concepts/division_ring.h | 98 + .../concepts/division_ring_prop_tests.h | 162 + .../cpp/sophus/concepts/group_accessors.h | 196 + .../sophus/concepts/group_accessors_test.cpp | 15 + .../concepts/group_accessors_unit_tests.h | 360 + cpp/sophus2/cpp/sophus/concepts/image.h | 58 + .../cpp/sophus/concepts/image_test.cpp | 15 + cpp/sophus2/cpp/sophus/concepts/lie_group.h | 278 + .../sophus/concepts/lie_group_prop_tests.h | 649 + .../cpp/sophus/concepts/lie_group_test.cpp | 15 + cpp/sophus2/cpp/sophus/concepts/manifold.h | 39 + .../cpp/sophus/concepts/manifold_prop_tests.h | 57 + .../cpp/sophus/concepts/manifold_test.cpp | 15 + cpp/sophus2/cpp/sophus/concepts/params.h | 118 + .../cpp/sophus/concepts/params_test.cpp | 15 + cpp/sophus2/cpp/sophus/concepts/point.h | 84 + .../cpp/sophus/concepts/point_test.cpp | 15 + cpp/sophus2/cpp/sophus/concepts/utils.h | 48 + .../cpp/sophus/concepts/utils_test.cpp | 15 + .../cpp/sophus/geometry/CMakeLists.txt | 36 + cpp/sophus2/cpp/sophus/geometry/fit_plane.h | 41 + .../cpp/sophus/geometry/fit_plane_test.cpp | 16 + .../cpp/sophus/geometry/inverse_depth.h | 117 + .../sophus/geometry/inverse_depth_test.cpp | 24 + cpp/sophus2/cpp/sophus/geometry/plane_conv.h | 211 + .../cpp/sophus/geometry/plane_conv_test.cpp | 153 + .../cpp/sophus/geometry/point_transform.h | 221 + .../sophus/geometry/point_transform_test.cpp | 196 + .../cpp/sophus/geometry/proto/CMakeLists.txt | 22 + .../cpp/sophus/geometry/proto/conv.cpp | 39 + cpp/sophus2/cpp/sophus/geometry/proto/conv.h | 23 + .../cpp/sophus/geometry/proto/conv_test.cpp | 13 + cpp/sophus2/cpp/sophus/geometry/ray.h | 206 + cpp/sophus2/cpp/sophus/geometry/ray_test.cpp | 36 + cpp/sophus2/cpp/sophus/image/CMakeLists.txt | 55 + cpp/sophus2/cpp/sophus/image/dyn_image.cpp | 11 + cpp/sophus2/cpp/sophus/image/dyn_image.h | 163 + .../cpp/sophus/image/dyn_image_test.cpp | 280 + .../cpp/sophus/image/dyn_image_types.cpp | 11 + .../cpp/sophus/image/dyn_image_types.h | 130 + .../cpp/sophus/image/dyn_image_types_test.cpp | 13 + .../cpp/sophus/image/dyn_image_view.cpp | 11 + cpp/sophus2/cpp/sophus/image/dyn_image_view.h | 150 + .../cpp/sophus/image/dyn_image_view_test.cpp | 120 + cpp/sophus2/cpp/sophus/image/image.cpp | 11 + cpp/sophus2/cpp/sophus/image/image.h | 113 + cpp/sophus2/cpp/sophus/image/image_size.cpp | 52 + cpp/sophus2/cpp/sophus/image/image_size.h | 86 + .../cpp/sophus/image/image_size_test.cpp | 15 + cpp/sophus2/cpp/sophus/image/image_test.cpp | 54 + cpp/sophus2/cpp/sophus/image/image_types.cpp | 74 + cpp/sophus2/cpp/sophus/image/image_types.h | 237 + .../cpp/sophus/image/image_types_test.cpp | 59 + cpp/sophus2/cpp/sophus/image/image_view.cpp | 11 + cpp/sophus2/cpp/sophus/image/image_view.h | 301 + .../cpp/sophus/image/image_view_test.cpp | 73 + .../cpp/sophus/image/interpolation.cpp | 11 + cpp/sophus2/cpp/sophus/image/interpolation.h | 47 + .../cpp/sophus/image/interpolation_test.cpp | 60 + cpp/sophus2/cpp/sophus/image/layout.cpp | 28 + cpp/sophus2/cpp/sophus/image/layout.h | 83 + cpp/sophus2/cpp/sophus/image/layout_test.cpp | 15 + .../cpp/sophus/image/mut_dyn_image.cpp | 11 + cpp/sophus2/cpp/sophus/image/mut_dyn_image.h | 157 + .../cpp/sophus/image/mut_dyn_image_test.cpp | 51 + .../cpp/sophus/image/mut_dyn_image_view.cpp | 11 + .../cpp/sophus/image/mut_dyn_image_view.h | 116 + .../sophus/image/mut_dyn_image_view_test.cpp | 23 + cpp/sophus2/cpp/sophus/image/mut_image.cpp | 11 + cpp/sophus2/cpp/sophus/image/mut_image.h | 220 + .../cpp/sophus/image/mut_image_test.cpp | 103 + .../cpp/sophus/image/mut_image_view.cpp | 47 + cpp/sophus2/cpp/sophus/image/mut_image_view.h | 270 + .../cpp/sophus/image/mut_image_view_test.cpp | 87 + cpp/sophus2/cpp/sophus/image/pixel_format.cpp | 39 + cpp/sophus2/cpp/sophus/image/pixel_format.h | 49 + .../cpp/sophus/image/pixel_format_test.cpp | 13 + .../cpp/sophus/image/proto/CMakeLists.txt | 24 + cpp/sophus2/cpp/sophus/image/proto/conv.cpp | 72 + cpp/sophus2/cpp/sophus/image/proto/conv.h | 41 + .../cpp/sophus/image/proto/conv_test.cpp | 90 + cpp/sophus2/cpp/sophus/lie/CMakeLists.txt | 55 + cpp/sophus2/cpp/sophus/lie/group_manifold.h | 92 + .../cpp/sophus/lie/group_manifold_test.cpp | 45 + cpp/sophus2/cpp/sophus/lie/identity.h | 42 + cpp/sophus2/cpp/sophus/lie/identity_test.cpp | 22 + cpp/sophus2/cpp/sophus/lie/impl/identity.h | 217 + cpp/sophus2/cpp/sophus/lie/impl/rotation2.h | 293 + cpp/sophus2/cpp/sophus/lie/impl/rotation3.h | 488 + cpp/sophus2/cpp/sophus/lie/impl/scaling.h | 321 + cpp/sophus2/cpp/sophus/lie/impl/sim_mat_w.h | 226 + .../cpp/sophus/lie/impl/spiral_similarity2.h | 329 + .../cpp/sophus/lie/impl/spiral_similarity3.h | 399 + .../impl/translation_factor_group_product.h | 387 + .../cpp/sophus/lie/interp/CMakeLists.txt | 24 + cpp/sophus2/cpp/sophus/lie/interp/average.h | 352 + .../cpp/sophus/lie/interp/interpolate.h | 50 + .../sophus/lie/interp/interpolate_test.cpp | 268 + cpp/sophus2/cpp/sophus/lie/interp/spline.h | 530 + .../cpp/sophus/lie/interp/spline_test.cpp | 173 + cpp/sophus2/cpp/sophus/lie/isometry2.h | 143 + cpp/sophus2/cpp/sophus/lie/isometry2_test.cpp | 30 + cpp/sophus2/cpp/sophus/lie/isometry3.h | 192 + cpp/sophus2/cpp/sophus/lie/isometry3_test.cpp | 28 + cpp/sophus2/cpp/sophus/lie/lie_group.h | 269 + cpp/sophus2/cpp/sophus/lie/lie_group_test.cpp | 15 + .../cpp/sophus/lie/proto/CMakeLists.txt | 24 + cpp/sophus2/cpp/sophus/lie/proto/conv.cpp | 82 + cpp/sophus2/cpp/sophus/lie/proto/conv.h | 34 + .../cpp/sophus/lie/proto/conv_test.cpp | 13 + cpp/sophus2/cpp/sophus/lie/rotation2.h | 95 + cpp/sophus2/cpp/sophus/lie/rotation2_test.cpp | 28 + cpp/sophus2/cpp/sophus/lie/rotation3.h | 167 + cpp/sophus2/cpp/sophus/lie/rotation3_test.cpp | 27 + cpp/sophus2/cpp/sophus/lie/scaling.h | 59 + cpp/sophus2/cpp/sophus/lie/scaling_test.cpp | 23 + .../cpp/sophus/lie/scaling_translation.h | 90 + .../sophus/lie/scaling_translation_test.cpp | 28 + cpp/sophus2/cpp/sophus/lie/se3.h | 11 + cpp/sophus2/cpp/sophus/lie/similarity2.h | 178 + .../cpp/sophus/lie/similarity2_test.cpp | 28 + cpp/sophus2/cpp/sophus/lie/similarity3.h | 193 + .../cpp/sophus/lie/similarity3_test.cpp | 28 + cpp/sophus2/cpp/sophus/lie/so3.h | 11 + .../cpp/sophus/lie/spiral_similarity2.h | 105 + .../sophus/lie/spiral_similarity2_test.cpp | 29 + .../cpp/sophus/lie/spiral_similarity3.h | 114 + .../sophus/lie/spiral_similarity3_test.cpp | 29 + cpp/sophus2/cpp/sophus/lie/translation.h | 85 + .../cpp/sophus/lie/translation_test.cpp | 23 + cpp/sophus2/cpp/sophus/linalg/CMakeLists.txt | 45 + cpp/sophus2/cpp/sophus/linalg/cast.h | 87 + cpp/sophus2/cpp/sophus/linalg/cast_test.cpp | 15 + cpp/sophus2/cpp/sophus/linalg/homogeneous.h | 42 + .../cpp/sophus/linalg/homogeneous_test.cpp | 32 + cpp/sophus2/cpp/sophus/linalg/orthogonal.h | 93 + .../cpp/sophus/linalg/orthogonal_test.cpp | 15 + cpp/sophus2/cpp/sophus/linalg/point_traits.h | 75 + .../cpp/sophus/linalg/point_traits_test.cpp | 15 + .../cpp/sophus/linalg/proto/CMakeLists.txt | 23 + cpp/sophus2/cpp/sophus/linalg/proto/conv.cpp | 138 + cpp/sophus2/cpp/sophus/linalg/proto/conv.h | 46 + .../cpp/sophus/linalg/proto/conv_test.cpp | 13 + cpp/sophus2/cpp/sophus/linalg/reduce.h | 95 + cpp/sophus2/cpp/sophus/linalg/reduce_test.cpp | 15 + cpp/sophus2/cpp/sophus/linalg/vector_space.h | 311 + .../cpp/sophus/linalg/vector_space_test.cpp | 15 + .../cpp/sophus/linalg/vector_space_traits.h | 77 + .../linalg/vector_space_traits_test.cpp | 15 + .../cpp/sophus/manifold/CMakeLists.txt | 37 + cpp/sophus2/cpp/sophus/manifold/complex.h | 172 + .../cpp/sophus/manifold/complex_test.cpp | 22 + .../cpp/sophus/manifold/product_manifold.h | 263 + .../sophus/manifold/product_manifold_test.cpp | 21 + cpp/sophus2/cpp/sophus/manifold/quaternion.h | 179 + .../cpp/sophus/manifold/quaternion_test.cpp | 23 + cpp/sophus2/cpp/sophus/manifold/unit_vector.h | 223 + .../cpp/sophus/manifold/unit_vector_test.cpp | 90 + .../cpp/sophus/manifold/vector_manifold.h | 70 + cpp/sophus2/cpp/sophus/sensor/CMakeLists.txt | 50 + .../sophus/sensor/camera_distortion/affine.h | 93 + .../sensor/camera_distortion/brown_conrady.h | 280 + .../sensor/camera_distortion/kannala_brandt.h | 232 + .../cpp/sophus/sensor/camera_model.cpp | 270 + cpp/sophus2/cpp/sophus/sensor/camera_model.h | 507 + .../cpp/sophus/sensor/camera_model_test.cpp | 460 + .../camera_projection/projection_ortho.h | 57 + .../sensor/camera_projection/projection_z1.h | 67 + cpp/sophus2/cpp/sophus/sensor/camera_rig.cpp | 11 + cpp/sophus2/cpp/sophus/sensor/camera_rig.h | 47 + .../cpp/sophus/sensor/camera_rig_test.cpp | 17 + .../cpp/sophus/sensor/clipping_planes.cpp | 11 + .../cpp/sophus/sensor/clipping_planes.h | 17 + .../sophus/sensor/clipping_planes_test.cpp | 13 + cpp/sophus2/cpp/sophus/sensor/imu_model.cpp | 93 + cpp/sophus2/cpp/sophus/sensor/imu_model.h | 179 + .../cpp/sophus/sensor/imu_model_test.cpp | 49 + .../cpp/sophus/sensor/orthographic.cpp | 7 + cpp/sophus2/cpp/sophus/sensor/orthographic.h | 66 + .../cpp/sophus/sensor/orthographic_test.cpp | 13 + .../cpp/sophus/sensor/proto/CMakeLists.txt | 23 + cpp/sophus2/cpp/sophus/sensor/proto/conv.cpp | 61 + cpp/sophus2/cpp/sophus/sensor/proto/conv.h | 22 + .../cpp/sophus/sensor/proto/conv_test.cpp | 13 + cpp/sophus2/docs/.gitignore | 20 + cpp/sophus2/docs/README.md | 41 + cpp/sophus2/docs/babel.config.js | 3 + .../docs/docs/Developer Guide/_category_.json | 8 + .../docs/docs/Developer Guide/style-guide.md | 60 + .../docs/docs/User Guide/_category_.json | 8 + .../docs/docs/User Guide/image-classes.md | 5 + .../docs/docs/User Guide/lie-groups.md | 20 + .../docs/docs/User Guide/more-geometry.md | 5 + cpp/sophus2/docs/docs/User Guide/overview.md | 5 + .../docs/docs/User Guide/sensor-models.md | 5 + .../docs/docs/User Guide/serialization.md | 5 + cpp/sophus2/docs/docs/User Guide/sympy.md | 5 + cpp/sophus2/docs/docs/intro.md | 22 + cpp/sophus2/docs/docusaurus.config.js | 134 + cpp/sophus2/docs/package-lock.json | 22434 ++++++++++++++++ cpp/sophus2/docs/package.json | 45 + cpp/sophus2/docs/sidebars.js | 33 + .../src/components/HomepageFeatures/index.tsx | 66 + .../HomepageFeatures/styles.module.css | 11 + cpp/sophus2/docs/src/css/custom.css | 30 + cpp/sophus2/docs/src/pages/index.module.css | 23 + cpp/sophus2/docs/src/pages/index.tsx | 41 + cpp/sophus2/docs/src/pages/markdown-page.md | 7 + cpp/sophus2/docs/static/.nojekyll | 0 .../docs/static/img/Farm-ng_Logo_Black.svg | 1 + .../static/img/docusaurus-social-card.jpg | Bin 0 -> 55746 bytes cpp/sophus2/docs/static/img/docusaurus.png | Bin 0 -> 5142 bytes .../docs/static/img/farm-ng_favicon.png | Bin 0 -> 1221 bytes cpp/sophus2/docs/static/img/favicon.ico | Bin 0 -> 3626 bytes cpp/sophus2/docs/static/img/logo.svg | 1 + .../static/img/undraw_docusaurus_mountain.svg | 171 + .../static/img/undraw_docusaurus_react.svg | 170 + .../static/img/undraw_docusaurus_tree.svg | 40 + cpp/sophus2/docs/tsconfig.json | 7 + cpp/sophus2/infra/bootstrap_venv.sh | 7 + cpp/sophus2/infra/scripts/compile_clang.sh | 9 + cpp/sophus2/infra/scripts/compile_gcc.sh | 9 + .../infra/scripts/compile_gcc_coverage.sh | 13 + .../scripts/install_cmake_farm_ng_core.sh | 27 + .../infra/scripts/install_deps_ceres.sh | 19 + cpp/sophus2/infra/scripts/install_deps_mac.sh | 9 + .../infra/scripts/install_deps_ubuntu.sh | 21 + .../install_deps_without_ceres_proto.sh | 52 + cpp/sophus2/infra/scripts/venv_activate.sh | 24 + .../infra/scripts/venv_export_env_vars.sh | 12 + .../infra/scripts/venv_update_prefix_deps.sh | 35 + cpp/sophus2/license_header.txt | 7 + cpp/sophus2/package.xml | 22 + cpp/sophus2/protos/CMakeLists.txt | 33 + cpp/sophus2/protos/sophus/geometry.proto | 22 + cpp/sophus2/protos/sophus/image.proto | 36 + cpp/sophus2/protos/sophus/lie.proto | 37 + cpp/sophus2/protos/sophus/linalg.proto | 67 + cpp/sophus2/protos/sophus/sensor.proto | 60 + cpp/sophus2/py/.gitignore | 6 + cpp/sophus2/py/sophus/__init__.py | 0 .../py/sympy/cpp_gencode/Se2_Dx_exp_x.cpp | 22 + .../py/sympy/cpp_gencode/Se2_Dx_log_this.cpp | 28 + .../Se2_Dx_this_mul_exp_x_at_0.cpp | 13 + .../py/sympy/cpp_gencode/Se3_Dx_exp_x.cpp | 138 + .../py/sympy/cpp_gencode/Se3_Dx_log_this.cpp | 252 + .../Se3_Dx_this_mul_exp_x_at_0.cpp | 64 + .../py/sympy/cpp_gencode/So2_Dx_exp_x.cpp | 2 + .../So2_Dx_log_exp_x_times_this_at_0.cpp | 1 + .../py/sympy/cpp_gencode/So2_Dx_log_this.cpp | 1 + .../So2_Dx_this_mul_exp_x_at_0.cpp | 2 + .../py/sympy/cpp_gencode/So3_Dx_exp_x.cpp | 29 + .../So3_Dx_log_exp_x_times_this_at_0.cpp | 50 + .../py/sympy/cpp_gencode/So3_Dx_log_this.cpp | 25 + .../So3_Dx_this_mul_exp_x_at_0.cpp | 19 + .../affine/dx_pixel_from_z1_plane_x.cpp | 4 + .../dx_normalized_from_z1_plane_x.cpp | 25 + .../dx_pixel_from_z1_plane_x.cpp | 27 + .../dx_pixel_from_z1_plane_x.cpp | 26 + cpp/sophus2/py/sympy/run_tests.sh | 18 + cpp/sophus2/py/sympy/sophus/__init__.py | 0 cpp/sophus2/py/sympy/sophus/affine_camera.py | 135 + .../py/sympy/sophus/brown_conrady_camera.py | 214 + cpp/sophus2/py/sympy/sophus/complex.py | 113 + cpp/sophus2/py/sympy/sophus/cse_codegen.py | 20 + .../py/sympy/sophus/dual_quaternion.py | 94 + cpp/sophus2/py/sympy/sophus/inverse_depth.py | 181 + .../py/sympy/sophus/kannala_brandt_camera.py | 166 + cpp/sophus2/py/sympy/sophus/matrix.py | 130 + cpp/sophus2/py/sympy/sophus/quaternion.py | 141 + cpp/sophus2/py/sympy/sophus/se2.py | 282 + cpp/sophus2/py/sympy/sophus/se3.py | 347 + cpp/sophus2/py/sympy/sophus/so2.py | 246 + cpp/sophus2/py/sympy/sophus/so3.py | 362 + cpp/sophus2/pyproject.toml | 6 + cpp/sophus2/run_clang_tidy.sh | 12 + cpp/sophus2/setup.cfg | 58 + cpp/sophus2/setup.py | 31 + 318 files changed, 49688 insertions(+) create mode 100644 cpp/sophus2/.clang-format create mode 100644 cpp/sophus2/.clang-tidy create mode 100644 cpp/sophus2/.github/ISSUE_TEMPLATE/bug_report.md create mode 100644 cpp/sophus2/.github/ISSUE_TEMPLATE/feature_request.md create mode 100644 cpp/sophus2/.github/workflows/build.yml create mode 100644 cpp/sophus2/.github/workflows/docs.yml create mode 100644 cpp/sophus2/.github/workflows/format.yml create mode 100644 cpp/sophus2/.github/workflows/pypi-release.yml create mode 100644 cpp/sophus2/.github/workflows/sympy.yml create mode 100644 cpp/sophus2/.gitignore create mode 100644 cpp/sophus2/.pre-commit-config.yaml create mode 100644 cpp/sophus2/.vscode/extensions.json create mode 100644 cpp/sophus2/CMakeLists.txt create mode 100644 cpp/sophus2/LICENSE.txt create mode 100644 cpp/sophus2/README.md create mode 100644 cpp/sophus2/Sophus.code-workspace create mode 100644 cpp/sophus2/cpp/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/examples/test_install_targets/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/examples/test_install_targets/compile_all_proto.cpp create mode 100644 cpp/sophus2/cpp/examples/test_install_targets/hello_se3.cpp create mode 100644 cpp/sophus2/cpp/examples/test_install_targets/hello_so3.cpp create mode 100644 cpp/sophus2/cpp/examples/test_install_targets/image_sensor.cpp create mode 100755 cpp/sophus2/cpp/run_format.sh create mode 100644 cpp/sophus2/cpp/sophus/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/SophusConfig.cmake.in create mode 100644 cpp/sophus2/cpp/sophus/calculus/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/calculus/num_diff.h create mode 100644 cpp/sophus2/cpp/sophus/calculus/region.h create mode 100644 cpp/sophus2/cpp/sophus/calculus/region_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/ceres/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/ceres/ceres_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/ceres/jet_helpers.h create mode 100644 cpp/sophus2/cpp/sophus/ceres/manifold.h create mode 100644 cpp/sophus2/cpp/sophus/ceres/manifold_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/ceres/typetraits.h create mode 100644 cpp/sophus2/cpp/sophus/common/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/common/common.h create mode 100644 cpp/sophus2/cpp/sophus/common/common_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/common/enum.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/concepts/division_ring.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/division_ring_prop_tests.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/group_accessors.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/group_accessors_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/concepts/group_accessors_unit_tests.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/image.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/image_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/concepts/lie_group.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/lie_group_prop_tests.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/lie_group_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/concepts/manifold.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/manifold_prop_tests.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/manifold_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/concepts/params.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/params_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/concepts/point.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/point_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/concepts/utils.h create mode 100644 cpp/sophus2/cpp/sophus/concepts/utils_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/geometry/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/geometry/fit_plane.h create mode 100644 cpp/sophus2/cpp/sophus/geometry/fit_plane_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/geometry/inverse_depth.h create mode 100644 cpp/sophus2/cpp/sophus/geometry/inverse_depth_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/geometry/plane_conv.h create mode 100644 cpp/sophus2/cpp/sophus/geometry/plane_conv_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/geometry/point_transform.h create mode 100644 cpp/sophus2/cpp/sophus/geometry/point_transform_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/geometry/proto/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/geometry/proto/conv.cpp create mode 100644 cpp/sophus2/cpp/sophus/geometry/proto/conv.h create mode 100644 cpp/sophus2/cpp/sophus/geometry/proto/conv_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/geometry/ray.h create mode 100644 cpp/sophus2/cpp/sophus/geometry/ray_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image.h create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image_types.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image_types.h create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image_types_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image_view.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image_view.h create mode 100644 cpp/sophus2/cpp/sophus/image/dyn_image_view_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/image.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/image.h create mode 100644 cpp/sophus2/cpp/sophus/image/image_size.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/image_size.h create mode 100644 cpp/sophus2/cpp/sophus/image/image_size_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/image_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/image_types.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/image_types.h create mode 100644 cpp/sophus2/cpp/sophus/image/image_types_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/image_view.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/image_view.h create mode 100644 cpp/sophus2/cpp/sophus/image/image_view_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/interpolation.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/interpolation.h create mode 100644 cpp/sophus2/cpp/sophus/image/interpolation_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/layout.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/layout.h create mode 100644 cpp/sophus2/cpp/sophus/image/layout_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/mut_dyn_image.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/mut_dyn_image.h create mode 100644 cpp/sophus2/cpp/sophus/image/mut_dyn_image_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/mut_dyn_image_view.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/mut_dyn_image_view.h create mode 100644 cpp/sophus2/cpp/sophus/image/mut_dyn_image_view_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/mut_image.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/mut_image.h create mode 100644 cpp/sophus2/cpp/sophus/image/mut_image_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/mut_image_view.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/mut_image_view.h create mode 100644 cpp/sophus2/cpp/sophus/image/mut_image_view_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/pixel_format.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/pixel_format.h create mode 100644 cpp/sophus2/cpp/sophus/image/pixel_format_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/proto/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/image/proto/conv.cpp create mode 100644 cpp/sophus2/cpp/sophus/image/proto/conv.h create mode 100644 cpp/sophus2/cpp/sophus/image/proto/conv_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/lie/group_manifold.h create mode 100644 cpp/sophus2/cpp/sophus/lie/group_manifold_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/identity.h create mode 100644 cpp/sophus2/cpp/sophus/lie/identity_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/impl/identity.h create mode 100644 cpp/sophus2/cpp/sophus/lie/impl/rotation2.h create mode 100644 cpp/sophus2/cpp/sophus/lie/impl/rotation3.h create mode 100644 cpp/sophus2/cpp/sophus/lie/impl/scaling.h create mode 100644 cpp/sophus2/cpp/sophus/lie/impl/sim_mat_w.h create mode 100644 cpp/sophus2/cpp/sophus/lie/impl/spiral_similarity2.h create mode 100644 cpp/sophus2/cpp/sophus/lie/impl/spiral_similarity3.h create mode 100644 cpp/sophus2/cpp/sophus/lie/impl/translation_factor_group_product.h create mode 100644 cpp/sophus2/cpp/sophus/lie/interp/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/lie/interp/average.h create mode 100644 cpp/sophus2/cpp/sophus/lie/interp/interpolate.h create mode 100644 cpp/sophus2/cpp/sophus/lie/interp/interpolate_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/interp/spline.h create mode 100644 cpp/sophus2/cpp/sophus/lie/interp/spline_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/isometry2.h create mode 100644 cpp/sophus2/cpp/sophus/lie/isometry2_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/isometry3.h create mode 100644 cpp/sophus2/cpp/sophus/lie/isometry3_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/lie_group.h create mode 100644 cpp/sophus2/cpp/sophus/lie/lie_group_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/proto/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/lie/proto/conv.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/proto/conv.h create mode 100644 cpp/sophus2/cpp/sophus/lie/proto/conv_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/rotation2.h create mode 100644 cpp/sophus2/cpp/sophus/lie/rotation2_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/rotation3.h create mode 100644 cpp/sophus2/cpp/sophus/lie/rotation3_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/scaling.h create mode 100644 cpp/sophus2/cpp/sophus/lie/scaling_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/scaling_translation.h create mode 100644 cpp/sophus2/cpp/sophus/lie/scaling_translation_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/se3.h create mode 100644 cpp/sophus2/cpp/sophus/lie/similarity2.h create mode 100644 cpp/sophus2/cpp/sophus/lie/similarity2_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/similarity3.h create mode 100644 cpp/sophus2/cpp/sophus/lie/similarity3_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/so3.h create mode 100644 cpp/sophus2/cpp/sophus/lie/spiral_similarity2.h create mode 100644 cpp/sophus2/cpp/sophus/lie/spiral_similarity2_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/spiral_similarity3.h create mode 100644 cpp/sophus2/cpp/sophus/lie/spiral_similarity3_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/lie/translation.h create mode 100644 cpp/sophus2/cpp/sophus/lie/translation_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/linalg/cast.h create mode 100644 cpp/sophus2/cpp/sophus/linalg/cast_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/homogeneous.h create mode 100644 cpp/sophus2/cpp/sophus/linalg/homogeneous_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/orthogonal.h create mode 100644 cpp/sophus2/cpp/sophus/linalg/orthogonal_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/point_traits.h create mode 100644 cpp/sophus2/cpp/sophus/linalg/point_traits_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/proto/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/linalg/proto/conv.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/proto/conv.h create mode 100644 cpp/sophus2/cpp/sophus/linalg/proto/conv_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/reduce.h create mode 100644 cpp/sophus2/cpp/sophus/linalg/reduce_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/vector_space.h create mode 100644 cpp/sophus2/cpp/sophus/linalg/vector_space_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/linalg/vector_space_traits.h create mode 100644 cpp/sophus2/cpp/sophus/linalg/vector_space_traits_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/manifold/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/manifold/complex.h create mode 100644 cpp/sophus2/cpp/sophus/manifold/complex_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/manifold/product_manifold.h create mode 100644 cpp/sophus2/cpp/sophus/manifold/product_manifold_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/manifold/quaternion.h create mode 100644 cpp/sophus2/cpp/sophus/manifold/quaternion_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/manifold/unit_vector.h create mode 100644 cpp/sophus2/cpp/sophus/manifold/unit_vector_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/manifold/vector_manifold.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_distortion/affine.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_distortion/brown_conrady.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_distortion/kannala_brandt.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_model.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_model.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_model_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_projection/projection_ortho.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_projection/projection_z1.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_rig.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_rig.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/camera_rig_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/clipping_planes.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/clipping_planes.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/clipping_planes_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/imu_model.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/imu_model.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/imu_model_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/orthographic.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/orthographic.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/orthographic_test.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/proto/CMakeLists.txt create mode 100644 cpp/sophus2/cpp/sophus/sensor/proto/conv.cpp create mode 100644 cpp/sophus2/cpp/sophus/sensor/proto/conv.h create mode 100644 cpp/sophus2/cpp/sophus/sensor/proto/conv_test.cpp create mode 100644 cpp/sophus2/docs/.gitignore create mode 100644 cpp/sophus2/docs/README.md create mode 100644 cpp/sophus2/docs/babel.config.js create mode 100644 cpp/sophus2/docs/docs/Developer Guide/_category_.json create mode 100644 cpp/sophus2/docs/docs/Developer Guide/style-guide.md create mode 100644 cpp/sophus2/docs/docs/User Guide/_category_.json create mode 100644 cpp/sophus2/docs/docs/User Guide/image-classes.md create mode 100644 cpp/sophus2/docs/docs/User Guide/lie-groups.md create mode 100644 cpp/sophus2/docs/docs/User Guide/more-geometry.md create mode 100644 cpp/sophus2/docs/docs/User Guide/overview.md create mode 100644 cpp/sophus2/docs/docs/User Guide/sensor-models.md create mode 100644 cpp/sophus2/docs/docs/User Guide/serialization.md create mode 100644 cpp/sophus2/docs/docs/User Guide/sympy.md create mode 100644 cpp/sophus2/docs/docs/intro.md create mode 100644 cpp/sophus2/docs/docusaurus.config.js create mode 100644 cpp/sophus2/docs/package-lock.json create mode 100644 cpp/sophus2/docs/package.json create mode 100644 cpp/sophus2/docs/sidebars.js create mode 100644 cpp/sophus2/docs/src/components/HomepageFeatures/index.tsx create mode 100644 cpp/sophus2/docs/src/components/HomepageFeatures/styles.module.css create mode 100644 cpp/sophus2/docs/src/css/custom.css create mode 100644 cpp/sophus2/docs/src/pages/index.module.css create mode 100644 cpp/sophus2/docs/src/pages/index.tsx create mode 100644 cpp/sophus2/docs/src/pages/markdown-page.md create mode 100644 cpp/sophus2/docs/static/.nojekyll create mode 100644 cpp/sophus2/docs/static/img/Farm-ng_Logo_Black.svg create mode 100644 cpp/sophus2/docs/static/img/docusaurus-social-card.jpg create mode 100644 cpp/sophus2/docs/static/img/docusaurus.png create mode 100644 cpp/sophus2/docs/static/img/farm-ng_favicon.png create mode 100644 cpp/sophus2/docs/static/img/favicon.ico create mode 100644 cpp/sophus2/docs/static/img/logo.svg create mode 100644 cpp/sophus2/docs/static/img/undraw_docusaurus_mountain.svg create mode 100644 cpp/sophus2/docs/static/img/undraw_docusaurus_react.svg create mode 100644 cpp/sophus2/docs/static/img/undraw_docusaurus_tree.svg create mode 100644 cpp/sophus2/docs/tsconfig.json create mode 100755 cpp/sophus2/infra/bootstrap_venv.sh create mode 100755 cpp/sophus2/infra/scripts/compile_clang.sh create mode 100755 cpp/sophus2/infra/scripts/compile_gcc.sh create mode 100755 cpp/sophus2/infra/scripts/compile_gcc_coverage.sh create mode 100755 cpp/sophus2/infra/scripts/install_cmake_farm_ng_core.sh create mode 100755 cpp/sophus2/infra/scripts/install_deps_ceres.sh create mode 100755 cpp/sophus2/infra/scripts/install_deps_mac.sh create mode 100755 cpp/sophus2/infra/scripts/install_deps_ubuntu.sh create mode 100755 cpp/sophus2/infra/scripts/install_deps_without_ceres_proto.sh create mode 100644 cpp/sophus2/infra/scripts/venv_activate.sh create mode 100644 cpp/sophus2/infra/scripts/venv_export_env_vars.sh create mode 100755 cpp/sophus2/infra/scripts/venv_update_prefix_deps.sh create mode 100644 cpp/sophus2/license_header.txt create mode 100644 cpp/sophus2/package.xml create mode 100644 cpp/sophus2/protos/CMakeLists.txt create mode 100644 cpp/sophus2/protos/sophus/geometry.proto create mode 100644 cpp/sophus2/protos/sophus/image.proto create mode 100644 cpp/sophus2/protos/sophus/lie.proto create mode 100644 cpp/sophus2/protos/sophus/linalg.proto create mode 100644 cpp/sophus2/protos/sophus/sensor.proto create mode 100644 cpp/sophus2/py/.gitignore create mode 100644 cpp/sophus2/py/sophus/__init__.py create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/Se2_Dx_exp_x.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/Se2_Dx_log_this.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/Se3_Dx_exp_x.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/Se3_Dx_log_this.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/So2_Dx_exp_x.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/So2_Dx_log_exp_x_times_this_at_0.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/So2_Dx_log_this.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/So3_Dx_exp_x.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/So3_Dx_log_exp_x_times_this_at_0.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/So3_Dx_log_this.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/affine/dx_pixel_from_z1_plane_x.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/brown_conrady/dx_normalized_from_z1_plane_x.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/brown_conrady/dx_pixel_from_z1_plane_x.cpp create mode 100644 cpp/sophus2/py/sympy/cpp_gencode/kannala_brandt/dx_pixel_from_z1_plane_x.cpp create mode 100755 cpp/sophus2/py/sympy/run_tests.sh create mode 100644 cpp/sophus2/py/sympy/sophus/__init__.py create mode 100644 cpp/sophus2/py/sympy/sophus/affine_camera.py create mode 100644 cpp/sophus2/py/sympy/sophus/brown_conrady_camera.py create mode 100644 cpp/sophus2/py/sympy/sophus/complex.py create mode 100644 cpp/sophus2/py/sympy/sophus/cse_codegen.py create mode 100644 cpp/sophus2/py/sympy/sophus/dual_quaternion.py create mode 100644 cpp/sophus2/py/sympy/sophus/inverse_depth.py create mode 100644 cpp/sophus2/py/sympy/sophus/kannala_brandt_camera.py create mode 100644 cpp/sophus2/py/sympy/sophus/matrix.py create mode 100644 cpp/sophus2/py/sympy/sophus/quaternion.py create mode 100644 cpp/sophus2/py/sympy/sophus/se2.py create mode 100644 cpp/sophus2/py/sympy/sophus/se3.py create mode 100644 cpp/sophus2/py/sympy/sophus/so2.py create mode 100644 cpp/sophus2/py/sympy/sophus/so3.py create mode 100644 cpp/sophus2/pyproject.toml create mode 100755 cpp/sophus2/run_clang_tidy.sh create mode 100644 cpp/sophus2/setup.cfg create mode 100644 cpp/sophus2/setup.py diff --git a/cpp/sophus2/.clang-format b/cpp/sophus2/.clang-format new file mode 100644 index 00000000..6366cdd5 --- /dev/null +++ b/cpp/sophus2/.clang-format @@ -0,0 +1,116 @@ +Language: Cpp +AccessModifierOffset: -1 +AlignAfterOpenBracket: AlwaysBreak +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Regroup +IncludeCategories: + # first-party libraries + - Regex: '\"([A-Za-z0-9.\/-_])+\"' + Priority: 1 + # standard libraries + - Regex: '<([A-Za-z0-_])+>' + Priority: 3 + # third-party libraries + - Regex: '<([A-Za-z0-9.\/-_])+>' + Priority: 2 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +QualifierOrder: ['inline', 'static', 'type', 'constexpr', 'const', 'volatile' ] +QualifierAlignment: Custom +RawStringFormats: + - Language: TextProto + BasedOnStyle: google +ReferenceAlignment: Left +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 8 +UseTab: Never diff --git a/cpp/sophus2/.clang-tidy b/cpp/sophus2/.clang-tidy new file mode 100644 index 00000000..13da5e02 --- /dev/null +++ b/cpp/sophus2/.clang-tidy @@ -0,0 +1,84 @@ +Checks: "-*, + readability-*, + -readability-magic-numbers, + -readability-function-cognitive-complexity, + cppcoreguidelines-*, + -cppcoreguidelines-macro-usage, + -cppcoreguidelines-avoid-*, + -cppcoreguidelines-narrowing-conversions, + -cppcoreguidelines-pro-*, + -cppcoreguidelines-slicing, + -cppcoreguidelines-special-member-functions, + -cppcoreguidelines-owning-memory, + -cppcoreguidelines-non-private-member-variables-in-classes, + bugprone-dynamic-static-initializers, + bugprone-easily-swappable-parameters, + bugprone-integer-division, + bugprone-use-after-move, + performance-noexcept-move-constructor, + performance-inefficient-algorithm, + modernize-make-unique, + modernize-make-shared, + modernize-use-bool-literals, + modernize-use-nodiscard, + modernize-use-using" +HeaderFilterRegex: .*/sophus/.* +CheckOptions: + - { key: readability-uppercase-literal-suffix.NewSuffixes, value: L } + + - { key: readability-identifier-naming.ClassCase, value: CamelCase } + - { key: readability-identifier-naming.StructCase, value: CamelCase } + - { + key: readability-identifier-naming.TemplateParameterCase, + value: CamelCase, + } + - { key: readability-identifier-naming.TemplateParameterPrefix, value: T } + - { + key: readability-identifier-naming.TemplateParameterIgnoredRegexp, + value: "expr-type", + } + - { key: readability-identifier-naming.TypedefCase, value: CamelCase } + + - { key: readability-identifier-naming.FunctionCase, value: camelBack } + - { key: readability-identifier-naming.MethodCase, value: camelBack } + + - { key: readability-identifier-naming.EnumConstantCase, value: lower_case } + - { key: readability-identifier-naming.MemberCase, value: lower_case } + - { key: readability-identifier-naming.NamespaceCase, value: lower_case } + - { key: readability-identifier-naming.ParameterCase, value: lower_case } + - { key: readability-identifier-naming.ParameterPackCase, value: lower_case } + - { + key: readability-identifier-naming.ScopedEnumConstantCase, + value: lower_case, + } + - { key: readability-identifier-naming.VariableCase, value: lower_case } + - { key: readability-identifier-naming.ConstantCase, value: lower_case } + + - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } + - { key: readability-identifier-naming.ProtectedMemberSuffix, value: _ } + + - { key: readability-identifier-naming.GlobalConstantCase, value: CamelCase } + - { key: readability-identifier-naming.GlobalConstantPrefix, value: k } + - { key: readability-identifier-naming.StaticConstantCase, value: CamelCase } + - { key: readability-identifier-naming.StaticConstantPrefix, value: k } + - { + key: readability-identifier-naming.ValueTemplateParameterCase, + value: CamelCase, + } + - { + key: readability-identifier-naming.ValueTemplateParameterPrefix, + value: k, + } + + - { key: readability-identifier-naming.GlobalVariablePrefix, value: Static_ } + - { + key: readability-identifier-naming.GlobalVariableCase, + value: Camel_Snake_Case, + } + - { key: readability-identifier-naming.StaticVariablePrefix, value: Static_ } + - { + key: readability-identifier-naming.StaticVariableCase, + value: Camel_Snake_Case, + } + + - { key: cppcoreguidelines-init-variables.MathHeader, value: } diff --git a/cpp/sophus2/.github/ISSUE_TEMPLATE/bug_report.md b/cpp/sophus2/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 00000000..867a07e6 --- /dev/null +++ b/cpp/sophus2/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,27 @@ +--- +name: Bug report +about: 'Report a specific bug. For more general help and questions use discussions: + https://github.com/strasdat/Sophus/discussions/categories/q-a' +title: '' +labels: bug +assignees: '' + +--- + +**Describe the bug** +[A clear and concise description of what the bug is.] + +**To Reproduce** +[Steps to reproduce the behavior:] +1. +2. +3. + +**Expected behavior** +[A clear and concise description of what you expected to happen.] + +**Sophus version: commit hash** + +[Commit hash on main branch which has the bug. Please test on HEAD of main first before submitting a bug.] + +**Additional context** diff --git a/cpp/sophus2/.github/ISSUE_TEMPLATE/feature_request.md b/cpp/sophus2/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 00000000..1fedc76e --- /dev/null +++ b/cpp/sophus2/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,21 @@ +--- +name: Feature request +about: 'Suggest a specific feature for Sophus. For more general ideas, please use + discussions: https://github.com/strasdat/Sophus/discussions/categories/ideas' +title: '' +labels: feature request +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +[A clear and concise description of what the problem is.] + +**Describe the solution you'd like** +[A clear and concise description of what you want to happen.] + +**Describe alternatives you've considered** +[A clear and concise description of any alternative solutions or features you've considered.] + +**Additional context**[ +Add any other context or screenshots about the feature request here.] diff --git a/cpp/sophus2/.github/workflows/build.yml b/cpp/sophus2/.github/workflows/build.yml new file mode 100644 index 00000000..b6035967 --- /dev/null +++ b/cpp/sophus2/.github/workflows/build.yml @@ -0,0 +1,181 @@ +on: + push: + branches: [main, sophus2] + pull_request: + + workflow_dispatch: + +jobs: + build: + runs-on: ${{ matrix.os }} + strategy: + matrix: + include: + - os: ubuntu-20.04 + compiler: gcc + optional_features: minimal + tests: unit_test + access: col_major + build_type: Release + - os: ubuntu-20.04 + compiler: gcc + optional_features: ceres_proto + tests: unit_test + access: col_major + build_type: Release + - os: ubuntu-20.04 + compiler: gcc + optional_features: minimal + tests: unit_test + access: row_major + build_type: Release + - os: ubuntu-20.04 + compiler: clang + optional_features: minimal + tests: clang_tidy + access: col_major + build_type: Release + - os: ubuntu-20.04 + compiler: gcc + optional_features: minimal + tests: coverage + access: col_major + build_type: Debug + - os: macos-11 + compiler: clang + optional_features: minimal + tests: unit_test + access: col_major + build_type: Release + - os: macos-12 + compiler: clang + optional_features: minimal # todo: ceres_proto here + tests: unit_test + access: col_major + build_type: Release + + fail-fast: false + env: + BUILD_TYPE: ${{ matrix.build_type }} + + steps: + - uses: actions/checkout@v3 + with: + submodules: "recursive" + + - uses: hendrikmuhs/ccache-action@v1.2 + with: + key: ${{ github.job }}-${{ matrix.os }}-${{ matrix.compiler }}-${{ matrix.optional_features }}-${{ matrix.tests }}-${{ matrix.access }}-${{ matrix.build_type }} + + - name: Install dependencies (Ubuntu) + working-directory: ./infra/scripts + run: | + ./install_deps_ubuntu.sh + if: matrix.os == 'ubuntu-20.04' + + - name: Install dependencies (Mac) + working-directory: ./infra/scripts + run: | + ./install_deps_mac.sh + if: matrix.os == 'macos-11' || matrix.os == 'macos-12' + + - name: Install dependencies from source + working-directory: ./infra/scripts + run: | + ./install_deps_without_ceres_proto.sh + if: matrix.optional_features == 'minimal' + + - name: Download dependencies (gcc, ceres+proto) + working-directory: ./infra/ + run: | + source ./bootstrap_venv.sh + cd scripts + ./install_cmake_farm_ng_core.sh + env: + ROW_ACCESS: matrix.access + if: matrix.optional_features == 'ceres_proto' + + - name: Compile (gcc) + working-directory: ./infra/scripts + run: + ./compile_gcc.sh + env: + ROW_ACCESS: matrix.access + if: matrix.tests == 'unit_test' && matrix.compiler == 'gcc' && matrix.optional_features == 'minimal' + + - name: Compile (gcc, ceres_proto) + working-directory: ./infra/scripts + run: | + ls + source ../venv/bin/activate + ./compile_gcc.sh + env: + ROW_ACCESS: matrix.access + SOPHUS_CERES: On + if: matrix.tests == 'unit_test' && matrix.compiler == 'gcc' && matrix.optional_features == 'ceres_proto' + + - name: Compile (clang) + working-directory: ./infra/scripts + run: + ./compile_clang.sh + env: + ROW_ACCESS: matrix.access + if: matrix.tests == 'unit_test' && matrix.compiler == 'clang' + + - name: Compile (clang, ceres_proto) + working-directory: ./infra/scripts + run: | + ls + source ../venv/bin/activate + ./compile_clang.sh + env: + ROW_ACCESS: matrix.access + SOPHUS_CERES: On + BUILD_FARM_NG_PROTOS: On + if: matrix.tests == 'unit_test' && matrix.compiler == 'clang' && matrix.optional_features == 'ceres_proto' + + - name: Run unit tests + working-directory: ./infra/scripts/build + run: | + ctest --output-on-failure + env: + ROW_ACCESS: matrix.access + if: matrix.tests == 'unit_test' && matrix.optional_features == 'minimal' + + - name: Run unit tests (ceres_proto) + working-directory: ./infra/scripts + run: | + source ../venv/bin/activate + cd build + ctest --output-on-failure + env: + ROW_ACCESS: matrix.access + if: matrix.tests == 'unit_test' && matrix.optional_features == 'ceres_proto' + + + # todo: re-enable this + + - name: Run install tests + working-directory: ./infra/scripts/build + run: | + sudo cmake --install . + # cd .. + # mkdir example_build + # cmake -G ../../../cpp/examples/test_install_targets + # ninja + # env: + # ROW_ACCESS: matrix.access + if: matrix.tests == 'unit_test' && matrix.optional_features == 'minimal' + + - name: Compile (clang) & tests coverage + working-directory: ./infra/scripts + run: + ./compile_gcc_coverage.sh + env: + ROW_ACCESS: matrix.access + if: matrix.tests == 'coverage' + + - name: clang-tidy + run: | + ./run_clang_tidy.sh + if: matrix.tests == 'clang_tidy' diff --git a/cpp/sophus2/.github/workflows/docs.yml b/cpp/sophus2/.github/workflows/docs.yml new file mode 100644 index 00000000..6b191263 --- /dev/null +++ b/cpp/sophus2/.github/workflows/docs.yml @@ -0,0 +1,34 @@ +on: + push: + branches: [ main, sophus2-docs, sophus2 ] + + workflow_dispatch: + +jobs: + build: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-20.04] + fail-fast: false + + + steps: + - uses: actions/checkout@v3 + with: + submodules: "recursive" + + - name: Install docusaurus dependencies + working-directory: docs + run: yarn install --frozen-lockfile + - name: Test build website + working-directory: docs + run: yarn build + + - name: Deploy + uses: peaceiris/actions-gh-pages@v3 + with: + github_token: ${{ secrets.GITHUB_TOKEN }} + publish_dir: docs/build + publish_branch: docs + destination_dir: latest diff --git a/cpp/sophus2/.github/workflows/format.yml b/cpp/sophus2/.github/workflows/format.yml new file mode 100644 index 00000000..95cb0783 --- /dev/null +++ b/cpp/sophus2/.github/workflows/format.yml @@ -0,0 +1,16 @@ +on: + push: + branches: [ main, sophus2 ] + pull_request: + + workflow_dispatch: + +jobs: + pre-commit-check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v3 + - uses: pre-commit/action@v3.0.0 + - uses: pre-commit-ci/lite-action@v1.0.0 + if: always() diff --git a/cpp/sophus2/.github/workflows/pypi-release.yml b/cpp/sophus2/.github/workflows/pypi-release.yml new file mode 100644 index 00000000..dbab1bc4 --- /dev/null +++ b/cpp/sophus2/.github/workflows/pypi-release.yml @@ -0,0 +1,21 @@ +on: + release: + types: [published] + +jobs: + pypi: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + - uses: actions/setup-python@v2 + - name: Install dependencies + run: python3 -m pip install --upgrade setuptools wheel + - name: Compile project + run: python3 -m pip install -e . + - name: Build distribution package + run: python3 setup.py sdist bdist_wheel + - name: Publish distribution 📦 to PyPI + uses: pypa/gh-action-pypi-publish@release/v1 + with: + user: __token__ + password: ${{ secrets.pypi_password_sophus }} diff --git a/cpp/sophus2/.github/workflows/sympy.yml b/cpp/sophus2/.github/workflows/sympy.yml new file mode 100644 index 00000000..0964db85 --- /dev/null +++ b/cpp/sophus2/.github/workflows/sympy.yml @@ -0,0 +1,24 @@ +on: + push: + branches: [ main, sophus2 ] + pull_request: + + workflow_dispatch: + +jobs: + build: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-20.04] + fail-fast: false + + + steps: + - uses: actions/checkout@v2 + + - name: Run sympy tests + run: | + pip3 install sympy==1.8 + cd py/sympy + ./run_tests.sh diff --git a/cpp/sophus2/.gitignore b/cpp/sophus2/.gitignore new file mode 100644 index 00000000..2d01f326 --- /dev/null +++ b/cpp/sophus2/.gitignore @@ -0,0 +1,10 @@ +build +CMakeLists.txt.user +*.pyc +.vscode/settings.json +clang-tidy-build/ +.eggs +*~ +html-dir +docs/c++-api/html +docs/c++-api/doxygen-awesome-css diff --git a/cpp/sophus2/.pre-commit-config.yaml b/cpp/sophus2/.pre-commit-config.yaml new file mode 100644 index 00000000..209e7c32 --- /dev/null +++ b/cpp/sophus2/.pre-commit-config.yaml @@ -0,0 +1,62 @@ +repos: + - repo: https://github.com/psf/black + rev: 22.3.0 + hooks: + - id: black + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: trailing-whitespace + - id: end-of-file-fixer + exclude: thirdparty/.*$ + - id: check-yaml + - id: check-json + exclude: docs/ + - repo: https://github.com/Lucas-C/pre-commit-hooks + rev: v1.1.13 + hooks: + - id: insert-license + files: \.(h|hpp|cpp|proto)$ + exclude: (py|thirdparty|py/sympy/cpp_gencode)/.*$ + args: + - --comment-style + - // + - --license-filepath + - license_header.txt + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.0 + hooks: + - id: clang-format + files: \.(h|hpp|cpp)$ + exclude: (py|thirdparty)/.*$ + - repo: https://github.com/compilerla/conventional-pre-commit + rev: b367d44fc93cb994af9d18b9578861478755ad0e + hooks: + - id: conventional-pre-commit + stages: [commit-msg] + args: [feat, fix, docs, style, refactor, perf, test, ci, build, chore, revert, bot] + - repo: local + hooks: + - id: pragma-once + name: pragma once check + entry: "#pragma once" + language: pygrep + files: \.(h|hpp)$ + args: [--negate] + exclude: (py|thirdparty)/.*$ + - id: no-c-std-lib + name: c standard library check + entry: | + (?x)<( + assert|complex|ctype|errno|fenv|float|inttypes|iso646|limits|locale|math|setjmp|signal| + stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|tgmath| + threads|time|uchar|wchar|wctype + ).h> + language: pygrep + files: \.(h|hpp|cpp)$ + exclude: (py|thirdparty/.*$) +ci: + autofix_commit_msg: | + bot: auto fixes from pre-commit.com hooks + + for more information, see https://pre-commit.ci diff --git a/cpp/sophus2/.vscode/extensions.json b/cpp/sophus2/.vscode/extensions.json new file mode 100644 index 00000000..5761a40b --- /dev/null +++ b/cpp/sophus2/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + "recommendations": [ + "twxs.cmake", + "ms-vscode.cpptools", + "xaver.clang-format" + ] +} diff --git a/cpp/sophus2/CMakeLists.txt b/cpp/sophus2/CMakeLists.txt new file mode 100644 index 00000000..8bf3cefc --- /dev/null +++ b/cpp/sophus2/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.16) +project(Sophus VERSION 2.0.0) + +include(CMakePackageConfigHelpers) +include(GNUInstallDirs) + +find_package(farm_ng_cmake 0.1.0 REQUIRED) +farm_ng_module() + +find_package(Eigen3 3.4.0 REQUIRED) +find_package(farm_ng_core 0.1.0 REQUIRED) + + +# Determine if sophus is built as a subproject (using add_subdirectory) +# or if it is the master project. +if (NOT DEFINED SOPHUS_MASTER_PROJECT) + set(SOPHUS_MASTER_PROJECT OFF) + if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) + set(SOPHUS_MASTER_PROJECT ON) + message(STATUS "CMake version: ${CMAKE_VERSION}") + endif () +endif () + +if (${COVERAGE}) + set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} --coverage") + add_definitions("-DSOPHUS_CODE_COVERAGE") +endif() + +option(SOPHUS_INSTALL "Generate the install target." ${SOPHUS_MASTER_PROJECT}) +option(SOPHUS_USE_BASIC_LOGGING "Use basic logging (in ensure and test macros)" OFF) + +option(SOPHUS_CERES "Enable sophus ceres" ON) + +option(BUILD_FARM_NG_PROTOS "Build tests." Off) + + +option(BUILD_SOPHUS_TESTS "Build tests." ON) +if(${BUILD_SOPHUS_TESTS}) + farm_ng_enable_testing() +endif() + + +if(${BUILD_FARM_NG_PROTOS}) + find_package(Protobuf REQUIRED) + include_directories(${Protobuf_INCLUDE_DIRS}) +endif() + +if(${BUILD_FARM_NG_PROTOS}) + add_subdirectory(protos) +endif() + + +add_subdirectory(cpp) + +farm_ng_export_module( + NAME Sophus + REQUIRED_DEPS farm_ng_core fmt) diff --git a/cpp/sophus2/LICENSE.txt b/cpp/sophus2/LICENSE.txt new file mode 100644 index 00000000..0bed9d59 --- /dev/null +++ b/cpp/sophus2/LICENSE.txt @@ -0,0 +1,23 @@ +MIT License + +Copyright (c) 2011, Hauke Strasdat. +Copyright (c) 2012, Steven Lovegrove. +Copyright (c) 2021, farm-ng, inc. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to +deal in the Software without restriction, including without limitation the +rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +sell copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +IN THE SOFTWARE. diff --git a/cpp/sophus2/README.md b/cpp/sophus2/README.md new file mode 100644 index 00000000..df7d0841 --- /dev/null +++ b/cpp/sophus2/README.md @@ -0,0 +1,198 @@ +

Sophus

+ +

+ A collection of c++ types for 2d and 3d geometric problems. +

+ +
+ + + Build CI Badge + + + Format CI Badge + + + Docs CI Badge + + +Sympy CI Badge + + + License Badge + + +
+ +
+

Manifolds, Image classes, Camera models and more. +

+
+ +Sophus started as a c++ implementaion of Lie Groups / Manifolds. It evolved to a +collection of types and functions commonly used or 2d and 3d geometric problems +especially in the domain of `robotics`, `computer vision` annd `graphics`. + +- [Lie groups / Manifold](#lie-groups) such as `SO(2)` and + `SO(3)` to present rotations in 2d and 3, +- [Other geometric conecpts](#other-geometric-concepts) such as unit vector, plane + fitting and inverse depth points, +- [Image classes](#image-classes) +- [Sensor models](#sensor-models) such `camera models` (as pinhole, orthographic + and lens distortion model) and `IMU models`. +- [Sympy](#sympy) representations of selected types - for + instance o symbolically derive Jacobians and autogenerate c++ code. +- [Serialization](#serialization) using proto (and soon json). + +## 👟Getting Started + +[Working in progress.] + +Check out the docs for now: + +(Which are also work in progress...) + +## 🌐Lie groups + +### tldr: rotations, translations and scaling in 2d and 3d + +`Lie groups` are generalizations of the Euclidean vector spaces R^N. A little +more formally, a Manifold which is also an [abstract group](https://en.wikipedia.org/wiki/Group_theory#Abstract_groups). + +*Okay, and what is a Manifold?* + +`Manifold` are generalizations of the Euclidean vector spaces R^N. In +particular, they behave locally like a Euclidean vector space, but globally +can have a very different structures. In particular there can be wrap-around. +The circle group SO(2) is the simplest example for such wrap around. Assume, +we have a dial pointing North. If you turn the dial 90 degree to the left, it +points West. If you turn it another 90 degrees it turns South. Now, if you turn +it again 90 degrees is points East. And you turn it left again for 90 degrees it +points North again. It wrapped around: `90 "+" 90 "+" 90 "+" 90 = 0`. + +*Do I need to understand the concept of Manifold, Group Theory and Lie Groups +in order to use Sophus?* + +Not at all! If you aim to solve geometric problems in 2d and 3d, it is best to +directly into concrete examples. By far the most commonly used Lie groups of +Sophus are the group of 3D rotations, also called Special Orthogonal Group, +short SO(3), as well as the group of rotation and translation in 3D, short +SE(3). + +### 3d rotation example using the SO(3) type + +```c++ + // The following demonstrates the group multiplication of rotation matrices + + // Create rotation matrices from rotations around the x and y and z axes: + double const kPi = sophus::kPi; + sophus::Rotation3F64 R1 = sophus::Rotation3F64::fromRx(kPi / 4); + sophus::Rotation3F64 R2 = sophus::Rotation3F64::fromRy(kPi / 6); + sophus::Rotation3F64 R3 = sophus::Rotation3F64::fromRz(-kPi / 3); + + std::cout << "The rotation matrices are" << std::endl; + std::cout << "R1:\n" << R1.matrix() << std::endl; + std::cout << "R2:\n" << R2.matrix() << std::endl; + std::cout << "R3:\n" << R3.matrix() << std::endl; + std::cout << "Their product R1*R2*R3:\n" + << (R1 * R2 * R3).matrix() << std::endl; + std::cout << std::endl; + + // Rotation matrices can act on vectors + Eigen::Vector3d x; + x << 0.0, 0.0, 1.0; + std::cout << "Rotation matrices can act on 3-vectors" << std::endl; + std::cout << "x\n" << x << std::endl; + std::cout << "R2*x\n" << R2 * x << std::endl; + std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl; + std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl; + std::cout << std::endl; + + // SO(3) are internally represented as unit quaternions. + std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl; + std::cout << "R1 in unit quaternion form:\n" + << R1.unitQuaternion().coeffs() << std::endl; + // Note that the order of coefficients of Eigen's quaternion class is + // (imag0, imag1, imag2, real) + std::cout << std::endl; +``` + +[hello_so3.cpp](cpp/examples/test_install_targets/hello_so3.cpp) + +### 3d rotation + translation example using the SE(3) type + +```c++ + // Example of create a rigid transformation from an SO(3) = 3D rotation and a + // translation 3-vector: + + // Let use assume there is a camera in the world. First we describe its + // orientation in the world reference frame. + sophus::Rotation3F64 world_from_camera_rotation = + sophus::Rotation3F64::fromRx(sophus::kPi / 4); + // Then the position of the camera in the world. + Eigen::Vector3d camera_in_world(0.0, 0.0, 1.0); + + // The pose (position and orientation) of the camera in the world is + // constructed by its orientation ``world_from_camera_rotation`` as well as + // its position ``camera_in_world``. + sophus::Isometry3F64 world_anchored_camera_pose( + world_from_camera_rotation, camera_in_world); + + // SE(3) naturally representation is a 4x4 matrix which can be accessed using + // the .matrix() method: + std::cout << "world_anchored_camera_pose:\n" + << world_anchored_camera_pose.matrix() << std::endl; +``` + +[hello_se3](cpp/examples/test_install_targets/hello_se3.cpp) + +### Tabel of Lie Groups + +The following table gives an overview of all Lie Groups in Sophus. + +| c++ type | Lie group name | Description | +| ----------------------------------------------|------------------------------------------------------| ---------------------------------------------------------------------------------------------------------- | +| [`Rotation2`](cpp/sophus/lie/so2.h) | Special Orthogonal Group in 2D, SO(2) | rotations in 2d, also called Circle Group, or just "angle" | +| [`Rotation3`](cpp/sophus/lie/so3.h) | Special Orthogonal Group in 3D, SO(3) | rotations in 3d, 3D orientations | +| [`Isometry2`](cpp/sophus/lie/se2.h) | Special Euclidean Group in 2D, SE(3) | rotations and translations in 2D, also called 2D rigid body transformations, 2d poses, plane isometries | +| [`Isometry3`](cpp/sophus/lie/se3.h) | Special Euclidean Group in 3D, SE(3) | rotations and translations in 3D, also called rigid body transformations,6 DoF poses, Euclidean isometries | +| [`RxSo2`](cpp/sophus/lie/rxso2.h) | Direct product of SO(3) and scalar matrix, R x SO(2) | scaling and rotations in 2D | +| [`RxSo3`](cpp/sophus/lie/rxso3.h) | Direct product of SO(3) and scalar matrix R x SO(3) | scaling and rotations in 3D | +| [`Similarity2`](cpp/sophus/lie/sim2.h) | Similarity Group in 2D, Sim(2) | scaling, rotations and translation in 2D | +| [`Similarity3`](cpp/sophus/lie/sim3.h) | Similarity Group in 3D, Sim(3) | scaling, rotations and translation in 3D | +| [`Cartesian2`](cpp/sophus/lie/cartesian.h) | 2D Euclidean Vector Space, R^2 | all vector spaces are trivial Lie groups, also called 2d translation group, the translation part of SE(2) | +| [`Cartesian3`](cpp/sophus/lie/cartesian.h) | 3D Euclidean Vector Space, R^3 | all vector spaces are trivial Lie groups, also called 3d translation group, the translation part of SE(2) | +| ----------------------------------------------|------------------------------------------------------| ---------------------------------------------------------------------------------------------------------- | + +Supported advanced features on Lie groups: + +- ✅ (linear) interpolation +- ✅ Spline interpolation +- ✅ Averaging (of more than two elements) + +## 📐More Geometry + +## 🌁Image classes + +## 📷Sensor Models + +## 📜Sympy + +## 🫙Serialization + +## 🧑🏽‍🏭Contribute + +Contributions are welcome! + +## 📝License + +This project is licensed under the [MIT](https://opensource.org/licenses/MIT) license. + +## 👩‍🚀Show your support + +Give a ⭐️ if this project helped you! diff --git a/cpp/sophus2/Sophus.code-workspace b/cpp/sophus2/Sophus.code-workspace new file mode 100644 index 00000000..68cef520 --- /dev/null +++ b/cpp/sophus2/Sophus.code-workspace @@ -0,0 +1,117 @@ +{ + "folders": [ + { + "path": "." + } + ], + "settings": { + "files.associations": { + "__bit_reference": "cpp", + "__bits": "cpp", + "__config": "cpp", + "__debug": "cpp", + "__errc": "cpp", + "__hash_table": "cpp", + "__locale": "cpp", + "__mutex_base": "cpp", + "__node_handle": "cpp", + "__split_buffer": "cpp", + "__threading_support": "cpp", + "__tree": "cpp", + "__tuple": "cpp", + "__verbose_abort": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "bitset": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "complex": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "exception": "cpp", + "initializer_list": "cpp", + "ios": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "locale": "cpp", + "map": "cpp", + "memory": "cpp", + "mutex": "cpp", + "new": "cpp", + "optional": "cpp", + "ostream": "cpp", + "queue": "cpp", + "ratio": "cpp", + "set": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "thread": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "typeinfo": "cpp", + "unordered_map": "cpp", + "variant": "cpp", + "vector": "cpp", + "concepts": "cpp", + "core": "cpp", + "*.ipp": "cpp", + "valarray": "cpp", + "__nullptr": "cpp", + "__string": "cpp", + "any": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "condition_variable": "cpp", + "forward_list": "cpp", + "fstream": "cpp", + "iomanip": "cpp", + "list": "cpp", + "numbers": "cpp", + "numeric": "cpp", + "random": "cpp", + "ranges": "cpp", + "semaphore": "cpp", + "stack": "cpp", + "unordered_set": "cpp", + "algorithm": "cpp", + "*.tcc": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "source_location": "cpp", + "utility": "cpp", + "stop_token": "cpp", + "cinttypes": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "csetjmp": "cpp", + "csignal": "cpp", + "coroutine": "cpp", + "future": "cpp", + "regex": "cpp", + "scoped_allocator": "cpp", + "span": "cpp", + "strstream": "cpp", + "typeindex": "cpp", + "__std_stream": "cpp", + "filesystem": "cpp" + } + } +} diff --git a/cpp/sophus2/cpp/CMakeLists.txt b/cpp/sophus2/cpp/CMakeLists.txt new file mode 100644 index 00000000..b092d07d --- /dev/null +++ b/cpp/sophus2/cpp/CMakeLists.txt @@ -0,0 +1,25 @@ +# Create examples make targets using ctest +option(BUILD_SOPHUS_EXAMPLES "Build examples." ON) +add_subdirectory(sophus) + +add_library(sophus INTERFACE) +target_link_libraries(sophus INTERFACE + sophus_common + sophus_calculus + sophus_linalg + sophus_lie + sophus_lie_interp + sophus_geometry +) +add_library (Sophus::sophus ALIAS sophus) +# Associate target with include directory +target_include_directories(sophus INTERFACE + "$" + "$" +) + +install(TARGETS sophus + EXPORT SophusTargets + LIBRARY DESTINATION lib + COMPONENT Libs + ) diff --git a/cpp/sophus2/cpp/examples/test_install_targets/CMakeLists.txt b/cpp/sophus2/cpp/examples/test_install_targets/CMakeLists.txt new file mode 100644 index 00000000..b26ac265 --- /dev/null +++ b/cpp/sophus2/cpp/examples/test_install_targets/CMakeLists.txt @@ -0,0 +1,61 @@ +project(SophusInstallTargetTests) + +cmake_minimum_required(VERSION 3.16) + +include(CMakePackageConfigHelpers) +include(GNUInstallDirs) + +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O2 -g") + +set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH ON) +set(BUILD_SHARED_LIBS ON) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Coarsely, the c++ standard used is c++17 plus concepts from c++20. +if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang") + # For clang, c++20 is enabled, but only concepts shall be used from the features newly added. + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra -Wsign-compare -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wreorder-ctor -Wreorder-init-list") +elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fconcepts") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wall -Wextra -Wno-missing-field-initializers -Wno-unused-parameter -Wno-unused-but-set-variable -Wno-unused-variable -Wno-unused-function -Wno-maybe-uninitialized") +endif() + +find_package(Sophus 23.0.1 REQUIRED) +find_package(fmt REQUIRED) +find_package(Eigen3 3.4.0 REQUIRED) + +set(example_source hello_so3 hello_se3) + +foreach(example_src ${example_source}) + add_executable( ${example_src} ${example_src}.cpp) + target_link_libraries(${example_src} + Eigen3::Eigen + fmt::fmt + pthread + Sophus::sophus +) +endforeach() + +set(example_source2 image_sensor) + +foreach(example_src ${example_source2}) + add_executable( ${example_src} ${example_src}.cpp) + target_link_libraries(${example_src} Eigen3::Eigen fmt::fmt pthread Sophus::sophus_image Sophus::sophus_sensor +) +endforeach() + +if(${BUILD_FARM_NG_PROTOS}) + find_package(Protobuf REQUIRED) + add_executable(compile_all_proto compile_all_proto.cpp) + target_link_libraries(compile_all_proto PUBLIC + fmt::fmt + Eigen3::Eigen + protobuf::libprotobuf + farm_ng_core::farm_ng_core_prototools + farm_ng_core::farm_ng_core_misc_proto_conv + Sophus::sophus_sensor_proto_conv + Sophus::sophus_image_proto_conv + ) +endif() diff --git a/cpp/sophus2/cpp/examples/test_install_targets/compile_all_proto.cpp b/cpp/sophus2/cpp/examples/test_install_targets/compile_all_proto.cpp new file mode 100644 index 00000000..32ff47f6 --- /dev/null +++ b/cpp/sophus2/cpp/examples/test_install_targets/compile_all_proto.cpp @@ -0,0 +1,28 @@ + +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include "sophus/geometry/proto/conv.h" +#include "sophus/image/proto/conv.h" +#include "sophus/lie/proto/conv.h" +#include "sophus/linalg/proto/conv.h" +#include "sophus/sensor/proto/conv.h" + +#include +#include + +using namespace farm_ng; + +int main() { + Uri uri; + uri.path = "foo"; + + auto proto = toProto(uri); + + writeProtobufToJsonFile("/tmp/foo.json", proto); +} diff --git a/cpp/sophus2/cpp/examples/test_install_targets/hello_se3.cpp b/cpp/sophus2/cpp/examples/test_install_targets/hello_se3.cpp new file mode 100644 index 00000000..77c16710 --- /dev/null +++ b/cpp/sophus2/cpp/examples/test_install_targets/hello_se3.cpp @@ -0,0 +1,34 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +#include + +int main() { + // Example of create a rigid transformation from an SO(3) = 3D rotation and a + // translation 3-vector: + + // Let use assume there is a camera in the world. First we describe its + // orientation in the world reference frame. + sophus::Rotation3F64 world_from_camera_rotation = + sophus::Rotation3F64::fromRx(sophus::kPi / 4); + // Then the position of the camera in the world. + Eigen::Vector3d camera_in_world(0.0, 0.0, 1.0); + + // The pose (position and orientation) of the camera in the world is + // constructed by its orientation ``world_from_camera_rotation`` as well as + // its position ``camera_in_world``. + sophus::Isometry3F64 world_anchored_camera_pose( + world_from_camera_rotation, camera_in_world); + + // SE(3) naturally representation is a 4x4 matrix which can be accessed using + // the .matrix() method: + std::cout << "world_anchored_camera_pose:\n" + << world_anchored_camera_pose.matrix() << std::endl; +} diff --git a/cpp/sophus2/cpp/examples/test_install_targets/hello_so3.cpp b/cpp/sophus2/cpp/examples/test_install_targets/hello_so3.cpp new file mode 100644 index 00000000..be0b7692 --- /dev/null +++ b/cpp/sophus2/cpp/examples/test_install_targets/hello_so3.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +#include + +int main() { + // The following demonstrates the group multiplication of rotation matrices + + // Create rotation matrices from rotations around the x and y and z axes: + double const kPi = sophus::kPi; + sophus::Rotation3F64 rotation1 = sophus::Rotation3F64::fromRx(kPi / 4); + sophus::Rotation3F64 rotation2 = sophus::Rotation3F64::fromRy(kPi / 6); + sophus::Rotation3F64 rotation3 = sophus::Rotation3F64::fromRz(-kPi / 3); + + std::cout << "The rotation matrices are" << std::endl; + std::cout << "R1:\n" << rotation1.matrix() << std::endl; + std::cout << "R2:\n" << rotation2.matrix() << std::endl; + std::cout << "R3:\n" << rotation3.matrix() << std::endl; + std::cout << "Their product R1*R2*R3:\n" + << (rotation1 * rotation2 * rotation3).matrix() << std::endl; + std::cout << std::endl; + + // Rotation matrices can act on vectors + Eigen::Vector3d x; + x << 0.0, 0.0, 1.0; + std::cout << "Rotation matrices can act on 3-vectors" << std::endl; + std::cout << "x\n" << x << std::endl; + std::cout << "R2*x\n" << rotation2 * x << std::endl; + std::cout << "R1*(R2*x)\n" << rotation1 * (rotation2 * x) << std::endl; + std::cout << "(R1*R2)*x\n" << (rotation1 * rotation2) * x << std::endl; + std::cout << std::endl; + + // SO(3) are internally represented as unit quaternions. + std::cout << "R1 in matrix form:\n" << rotation1.matrix() << std::endl; + std::cout << "R1 in unit quaternion form:\n" + << rotation1.unitQuaternion().coeffs() << std::endl; + // Note that the order of coefficients of Eigen's quaternion class is + // (imag0, imag1, imag2, real) + std::cout << std::endl; +} diff --git a/cpp/sophus2/cpp/examples/test_install_targets/image_sensor.cpp b/cpp/sophus2/cpp/examples/test_install_targets/image_sensor.cpp new file mode 100644 index 00000000..862b17ef --- /dev/null +++ b/cpp/sophus2/cpp/examples/test_install_targets/image_sensor.cpp @@ -0,0 +1,30 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include +#include + +using namespace sophus; + +int main() { + const ImageSize size64{6, 4}; + MutImage mut_image(size64); + mut_image.fill(0.5f); + Image image(std::move(mut_image)); + AnyImage<> any_image(image); + + std::vector camera_models; + CameraModel pinhole = CameraModel::createDefaultPinholeModel({640, 480}); + Eigen::VectorXd get_params(8); + get_params << 1000, 1000, 320, 280, 0.1, 0.01, 0.001, 0.0001; + CameraModel kb3 = CameraModel( + {640, 480}, CameraDistortionType::kannala_brandt_k3, get_params); + + camera_models.push_back(pinhole); + camera_models.push_back(kb3); +} diff --git a/cpp/sophus2/cpp/run_format.sh b/cpp/sophus2/cpp/run_format.sh new file mode 100755 index 00000000..ef6b7403 --- /dev/null +++ b/cpp/sophus2/cpp/run_format.sh @@ -0,0 +1 @@ +find . -type d \( -path ./sympy -o -path ./doxyrest_b -o -path "./*/CMakeFiles/*" \) -prune -o \( -iname "*.hpp" -o -iname "*.cpp" \) -print | xargs clang-format -i diff --git a/cpp/sophus2/cpp/sophus/CMakeLists.txt b/cpp/sophus2/cpp/sophus/CMakeLists.txt new file mode 100644 index 00000000..016c7d66 --- /dev/null +++ b/cpp/sophus2/cpp/sophus/CMakeLists.txt @@ -0,0 +1,10 @@ +add_subdirectory(common) +add_subdirectory(concepts) +add_subdirectory(linalg) +add_subdirectory(calculus) +add_subdirectory(lie) +add_subdirectory(geometry) +add_subdirectory(image) +add_subdirectory(sensor) +add_subdirectory(manifold) +add_subdirectory(ceres) diff --git a/cpp/sophus2/cpp/sophus/SophusConfig.cmake.in b/cpp/sophus2/cpp/sophus/SophusConfig.cmake.in new file mode 100644 index 00000000..e69de29b diff --git a/cpp/sophus2/cpp/sophus/calculus/CMakeLists.txt b/cpp/sophus2/cpp/sophus/calculus/CMakeLists.txt new file mode 100644 index 00000000..3923b6c6 --- /dev/null +++ b/cpp/sophus2/cpp/sophus/calculus/CMakeLists.txt @@ -0,0 +1,21 @@ +#[[ +sophus_calculus + + + +]] + +farm_ng_add_library(sophus_calculus + NAMESPACE Sophus + INCLUDE_DIR ../../ + HEADERS + num_diff.h + region.h +) +target_link_libraries(sophus_calculus INTERFACE sophus_common) + +if(${BUILD_SOPHUS_TESTS}) + farm_ng_add_test(region + PARENT_LIBRARY sophus_calculus + LABELS small) +endif() diff --git a/cpp/sophus2/cpp/sophus/calculus/num_diff.h b/cpp/sophus2/cpp/sophus/calculus/num_diff.h new file mode 100644 index 00000000..d8a4ceba --- /dev/null +++ b/cpp/sophus2/cpp/sophus/calculus/num_diff.h @@ -0,0 +1,96 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +/// @file +/// Numerical differentiation using finite differences + +#pragma once + +#include "sophus/common/common.h" + +#include +#include +#include + +namespace sophus { + +namespace details { +template +class Curve { + public: + template + static auto numDiff(TFn curve, TScalar t, TScalar h) -> decltype(curve(t)) { + static_assert( + std::is_floating_point::value, + "Scalar must be a floating point type."); + + return (curve(t + h) - curve(t - h)) / (TScalar(2) * h); + } +}; + +template +class VectorField { + public: + static auto numDiff( + std::function( + Eigen::Vector)> vector_field, + Eigen::Vector const& a, + TScalar eps) -> Eigen::Matrix { + static_assert( + std::is_floating_point::value, + "Scalar must be a floating point type."); + Eigen::Matrix j; + Eigen::Vector h; + h.setZero(); + for (int i = 0; i < kM; ++i) { + h[i] = eps; + Eigen::Vector vfp = vector_field(a + h); + Eigen::Vector vfm = vector_field(a - h); + + j.col(i) = (vfp - vfm) / (TScalar(2) * eps); + h[i] = TScalar(0); + } + + return j; + } +}; + +} // namespace details + +/// Calculates the derivative of a curve at a point ``t``. +/// +/// Here, a curve is a function from a Scalar to a Euclidean space. Thus, it +/// returns either a Scalar, a vector or a matrix. +/// +template +auto curveNumDiff(TFn curve, TScalar t, TScalar h = kEpsilonSqrt) + -> decltype(details::Curve::numDiff(std::move(curve), t, h)) { + return details::Curve::numDiff(std::move(curve), t, h); +} + +/// Calculates the derivative of a vector field at a point ``a``. +/// +/// Here, a vector field is a function from a vector space to another vector +/// space. +/// +template < + class TScalar, + int kMatrixDim, + int kM, + class TScalarOrVector, + class TFn> +auto vectorFieldNumDiff( + TFn vector_field, + TScalarOrVector const& a, + TScalar eps = kEpsilonSqrt) + -> Eigen::Matrix { + return details::VectorField::numDiff( + vector_field, a, eps); +} + +} // namespace sophus diff --git a/cpp/sophus2/cpp/sophus/calculus/region.h b/cpp/sophus2/cpp/sophus/calculus/region.h new file mode 100644 index 00000000..a306a2a1 --- /dev/null +++ b/cpp/sophus2/cpp/sophus/calculus/region.h @@ -0,0 +1,371 @@ + +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#pragma once + +#include "sophus/common/common.h" +#include "sophus/linalg/vector_space.h" +#include "sophus/linalg/vector_space_traits.h" + +namespace sophus { + +template +class Region; + +using RegionI = Region; +using RegionF32 = Region; +using RegionF64 = Region; + +template +using Region2 = Region>; +using Region2I = Region2; +using Region2F32 = Region2; +using Region2F64 = Region2; + +template +using Region3 = Region>; +using Region3I = Region3; +using Region3F32 = Region3; +using Region3F64 = Region3; + +template +using Region4 = Region>; +using Region4I = Region4; +using Region4F32 = Region4; +using Region4F64 = Region4; + +/// A region is a closed interval [a, b] with a being the lower bound (=min) and +/// b being the upper bound (=max). +/// +/// Here, the bounds a, b sre either boths scalars (e.g. floats, doubles) +/// or fixed length vectors/arrays (such as Eigen::Vector3f or Eigen::Array2d). +/// +/// Special case for integer numbers: +/// +/// An integer number X is considered being equivalent to a real region +/// [X-0.5, X+0.5]. +template +class Region { + public: + static bool constexpr kIsInteger = PointTraits::kIsInteger; + using Point = TPoint; + using Scalar = typename PointTraits::Scalar; + + static int constexpr kRows = PointTraits::kRows; + static int constexpr kCols = PointTraits::kCols; + static_assert(kRows >= 1); + static_assert(kCols >= 1); + static int constexpr kDim = kRows * kCols; + + /// Creates an uninitialized region. + static auto uninitialized() noexcept -> Region { + return Region(UninitTag{}); + } + + /// Creates and empty region. + static auto empty() noexcept -> Region { + auto region = Region::uninitialized(); + region.min_max_ = { + PointTraits::max(), PointTraits::lowest()}; + return region; + } + + /// Creates unbounded region. + /// + /// If TPoint is floating point, the region is [-inf, +inf]. + static auto unbounded() noexcept -> Region { + auto region = Region::uninitialized(); + region.min_max_ = { + PointTraits::lowest(), PointTraits::max()}; + return region; + } + + /// Creates a region from a given point. + /// + /// If TPoint is a floating point then the region is considered degenerated. + static auto from(TPoint const& p) noexcept -> Region { + auto region = Region::empty(); + SOPHUS_ASSERT(!isNan(p)); + region.min_max_[0] = p; + region.min_max_[1] = p; + return region; + } + + /// Creates Region from two points, min and max. + /// + /// The points min, max need not to be ordered. After construction it will be + /// true that this->min() <= this->max(). + static auto fromMinMax(TPoint const& min, TPoint const& max) noexcept + -> Region { + auto region = Region::empty(); + SOPHUS_ASSERT(!isNan(min)); + SOPHUS_ASSERT(!isNan(max)); + region.extend(min); + region.extend(max); + return region; + } + + /// Convenient constructor to create a Segment from two points p1 and p2. + /// + /// The points need not to be ordered. After construction it will be true + /// that this->min() <= this->max(). + /// + /// Note: This constructor is only available for scalar regions. For + /// multi-dim regions, use the fromMinMax() factory instead., + template + requires std::is_same_v Region( + TScalar const& p1, TScalar const& p2) + noexcept : min_max_{p1, p1} { + SOPHUS_ASSERT(!isNan(p1)); + SOPHUS_ASSERT(!isNan(p2)); + this->extend(p2); + } + + template + requires(kDim == 2) static auto createPerAxis( + Region const& segment_x, + Region const& segment_y) noexcept -> Region2 { + auto region = Region::uninitialized(); + region.setElem(segment_x, 0); + region.setElem(segment_y, 1); + return region; + } + + template + requires(kDim == 3) static auto createPerAxis( + Region const& segment_x, + Region const& segment_y, + Region const& segment_z) noexcept -> Region2 { + auto region = Region::uninitialized(); + region.setElem(segment_x, 0); + region.setElem(segment_y, 1); + region.setElem(segment_z, 2); + return region; + } + + template + requires(kDim == 4) static auto createPerAxis( + Region const& segment_x, + Region const& segment_y, + Region const& segment_z, + Region const& segment_w) noexcept -> Region2 { + auto region = Region::uninitialized(); + region.setElem(segment_x, 0); + region.setElem(segment_y, 1); + region.setElem(segment_z, 2); + region.setElem(segment_w, 3); + return region; + } + + /// Precondition: !this->isEmptpy() + [[nodiscard]] auto getElem(size_t row) const -> Region const& { + return { + SOPHUS_UNWRAP(tryGetElem(min(), row)), + SOPHUS_UNWRAP(tryGetElem(max(), row))}; + } + + /// Precondition: !this->isEmptpy() + void setElem(size_t row, Region const& s) { + SOPHUS_UNWRAP(trySetElem(min(), s, row)); + SOPHUS_UNWRAP(trySetElem(max(), s, row)); + } + + /// Returns the lower bound of the region. + /// + /// Precondition: !this->isEmptpy() + [[nodiscard]] auto min() const noexcept -> TPoint const& { + SOPHUS_ASSERT(!this->isEmpty()); + return min_max_[0]; + } + + /// Returns the upper bound of the region. + /// + /// Precondition: !this->isEmptpy() + [[nodiscard]] auto max() const noexcept -> TPoint const& { + SOPHUS_ASSERT(!this->isEmpty()); + return min_max_[1]; + } + + /// Returns the lower bound of the region if it exit and nullopt otherwise. + [[nodiscard]] auto tryMin() const noexcept -> std::optional { + if (isEmpty()) { + return std::nullopt; + } + return min_max_[0]; + } + + /// Returns the lower bound of the region if it exists and nullopt otherwise. + [[nodiscard]] auto tryMax() const noexcept -> std::optional { + if (isEmpty()) { + return std::nullopt; + } + return min_max_[1]; + } + + /// Returns the clamped version of the given point. + /// + /// Precondition: !this->isEmptpy() + [[nodiscard]] auto clamp(TPoint const& point) const noexcept -> TPoint { + return sophus::clamp(point, min(), max()); + } + + /// Returns true if the region contains the given point. + [[nodiscard]] auto contains(TPoint const& point) const noexcept -> bool { + if (isEmpty()) { + return false; + } + return isLessEqual(min(), point) && isLessEqual(point, max()); + } + + /// Returns the range of the region. + /// + /// It is zero if the region is not proper. + [[nodiscard]] auto range() const noexcept -> TPoint { + if (isEmpty()) { + return zero(); + } + if constexpr (kIsInteger) { + // For integers, we consider e.g. {2} == [1.5, 2.5] hence range of + // Region(2, 3) == [1.5, 3.5] is 2. + return plus(eval(max() - min()), 1); + } + return eval(max() - min()); + } + + /// Returns the mid point. + /// + /// Note: If TPoint is an integer point then the result will be rounded to + /// the closed integer. + /// + /// Precondition: !this->isEmptpy() + [[nodiscard]] auto mid() const noexcept -> TPoint { + return eval(min() + range() / 2); + } + + /// Extends this by other region. + auto extend(Region const& other) noexcept -> Region& { + if (other.isEmpty()) { + return *this; + } + if (this->isEmpty()) { + *this = other; + return *this; + } + min_max_[0] = sophus::min(min(), other.min()); + min_max_[1] = sophus::max(max(), other.max()); + return *this; + } + + /// Extends this by given point. + auto extend(TPoint const& point) noexcept -> Region& { + if (this->isEmpty()) { + *this = from(point); + } else { + min_max_[0] = sophus::min(min(), point); + min_max_[1] = sophus::max(max(), point); + } + return *this; + } + + /// Returns translated region. + [[nodiscard]] auto translated(TPoint const& p) const noexcept + -> Region { + return Region::fromMinMax(min_max_[0] + p, min_max_[1] + p); + } + + template + auto cast() const noexcept -> Region { + if (isEmpty()) { + return Region::empty(); + } + if constexpr (kIsInteger == Region::kIsInteger) { + // case 1: floating => floating and integer => integer is trivial + return Region( + sophus::cast(min()), sophus::cast(max())); + } + if constexpr (kIsInteger && !Region::kIsInteger) { + // case 2: integer to floating. + // + // example: [2, 5] -> [1.5, 5.5] + return Region( + plus(sophus::cast(min()), -0.5), + plus(sophus::cast(max()), 0.5)); + } + // case 3: floating to integer. + static_assert( + kIsInteger || !Region::kIsInteger, + "For floating to integer: call encloseCast() or roundCast() " + "instead."); + } + + /// Returns the smallest integer region which contains this. + /// + /// example: [1.2, 1.3] -> [1, 2] + template + auto encloseCast() const noexcept -> Region { + static_assert(!kIsInteger && Region::kIsInteger); + if (isEmpty()) { + return Region::empty(); + } + return Region( + sophus::cast(sophus::floor(min())), + sophus::cast(sophus::ceil(max()))); + } + + /// Rounds given region bounds and returns resulting integer region. + /// + /// example: [1.2, 2.3] -> [1, 2] + /// example: [1.1, 2.7] -> [1, 3] + template + auto roundCast() const noexcept -> Region { + if (isEmpty()) { + return Region::empty(); + } + static_assert(!kIsInteger && Region::kIsInteger); + return Region( + sophus::cast(sophus::round(min())), + sophus::cast(sophus::round(max()))); + } + + /// Returns true if region is empty. + [[nodiscard]] auto isEmpty() const noexcept -> bool { + return allTrue(min_max_[0] == PointTraits::max()) && + allTrue(min_max_[1] == PointTraits::lowest()); + } + + /// Returns true if region contains a single floating point number. + [[nodiscard]] auto isDegenerated() const noexcept -> bool { + return !kIsInteger && allTrue(min_max_[0] == min_max_[1]); + } + + /// Returns true if region is neither empty nor degenerated. + /// Hence it contains a range of values. + [[nodiscard]] auto isProper() const noexcept -> bool { + return !this->isEmpty() && !this->isDegenerated(); + } + + /// Returns true if region has no bounds. + [[nodiscard]] auto isUnbounded() const noexcept -> bool { + return allTrue(min_max_[0] == PointTraits::lowest()) && + allTrue(min_max_[1] == PointTraits::max()); + } + + private: + explicit Region(UninitTag /*unused*/) {} + + // invariant: this->isEmpty() or min_max[0] <= min_max[1] + std::array min_max_; +}; + +template +auto operator==(Region const& lhs, Region const& rhs) -> bool { + return lhs.min() == rhs.min() && lhs.max() == rhs.max(); +} + +} // namespace sophus diff --git a/cpp/sophus2/cpp/sophus/calculus/region_test.cpp b/cpp/sophus2/cpp/sophus/calculus/region_test.cpp new file mode 100644 index 00000000..197e9429 --- /dev/null +++ b/cpp/sophus2/cpp/sophus/calculus/region_test.cpp @@ -0,0 +1,82 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include "sophus/calculus/region.h" + +#include + +using namespace sophus; + +template +void regionTests(std::vector const& points) { + // TODO: test fromMinMax, createPerAxis, cast, encloseCast, roundCast, clamp, + // extend + + using RegionP = Region; + std::vector regions; + + // constr + RegionP empty_region = RegionP::empty(); + regions.push_back(empty_region); + EXPECT_TRUE(empty_region.isEmpty()); + EXPECT_FALSE(empty_region.isDegenerated()); + EXPECT_FALSE(empty_region.isProper()); + EXPECT_FALSE(empty_region.isUnbounded()); + for (TPoint p : points) { + EXPECT_FALSE(empty_region.contains(p)); + } + + RegionP unbounded = RegionP::unbounded(); + regions.push_back(unbounded); + + EXPECT_FALSE(unbounded.isEmpty()); + EXPECT_FALSE(unbounded.isDegenerated()); + EXPECT_TRUE(unbounded.isProper()); + EXPECT_TRUE(unbounded.isUnbounded()); + for (TPoint p : points) { + EXPECT_TRUE(unbounded.contains(p)); + } + + for (TPoint p : points) { + auto x = RegionP::from(p); + regions.push_back(x); + EXPECT_FALSE(x.isEmpty()); + EXPECT_TRUE(x.contains(p)); + if (RegionP::kIsInteger) { + EXPECT_TRUE(x.isProper()); + EXPECT_FALSE(x.isDegenerated()); + } else { + EXPECT_FALSE(x.isProper()); + EXPECT_TRUE(x.isDegenerated()); + } + EXPECT_FALSE(x.isUnbounded()); + } + + for (int i = 0; i < RegionP::kDim; ++i) { + for (auto x : regions) { + if (x.isEmpty()) { + EXPECT_FALSE(x.tryMin()); + EXPECT_FALSE(x.tryMax()); + EXPECT_EQ(x.range(), zero()); + } else { + auto maybe_min = x.tryMin(); + auto maybe_max = x.tryMax(); + EXPECT_EQ(SOPHUS_UNWRAP(maybe_min), x.min()); + EXPECT_EQ(SOPHUS_UNWRAP(maybe_max), x.max()); + } + } + } +} + +TEST(ScalarRegion, init) { + regionTests({0.0, 1.0, -5.0}); + regionTests({0u, 1u, 128u, 255u}); + regionTests(pointExamples()); + regionTests(pointExamples()); + regionTests(pointExamples()); +} diff --git a/cpp/sophus2/cpp/sophus/ceres/CMakeLists.txt b/cpp/sophus2/cpp/sophus/ceres/CMakeLists.txt new file mode 100644 index 00000000..5712c992 --- /dev/null +++ b/cpp/sophus2/cpp/sophus/ceres/CMakeLists.txt @@ -0,0 +1,35 @@ +#[[ +sophus_ceres + + + +]] +farm_ng_add_library(sophus_ceres + NAMESPACE Sophus + INCLUDE_DIR ../../ + HEADERS + jet_helpers.h + manifold.h + typetraits.h +) +target_link_libraries(sophus_ceres INTERFACE sophus_lie) + + +if(${BUILD_SOPHUS_TESTS}) + if(${SOPHUS_CERES}) + find_package(Ceres 2.1.0 REQUIRED) + foreach(test_basename + ceres + manifold + ) + farm_ng_add_test(${test_basename} + PARENT_LIBRARY sophus_ceres + LINK_LIBRARIES + Ceres::ceres sophus_ceres + farm_ng_core::farm_ng_core_pipeline + sophus_sensor + LABELS large) + target_compile_definitions(${test_basename}_test_sophus_ceres PRIVATE ${ARGN}) + endforeach() + endif() +endif() diff --git a/cpp/sophus2/cpp/sophus/ceres/ceres_test.cpp b/cpp/sophus2/cpp/sophus/ceres/ceres_test.cpp new file mode 100644 index 00000000..1420f824 --- /dev/null +++ b/cpp/sophus2/cpp/sophus/ceres/ceres_test.cpp @@ -0,0 +1,1116 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include "sophus/ceres/manifold.h" +#include "sophus/common/common.h" +#include "sophus/lie/identity.h" +#include "sophus/lie/isometry2.h" +#include "sophus/lie/isometry3.h" +#include "sophus/lie/scaling.h" +#include "sophus/lie/scaling_translation.h" +#include "sophus/lie/similarity2.h" +#include "sophus/lie/similarity3.h" +#include "sophus/lie/spiral_similarity2.h" +#include "sophus/lie/spiral_similarity3.h" +#include "sophus/lie/translation.h" +#include "sophus/manifold/complex.h" +#include "sophus/manifold/quaternion.h" +#include "sophus/sensor/camera_rig.h" + +#include +#include +#include + +#include + +namespace sophus::test { + +struct Cost { + double median() const { + SOPHUS_ASSERT(!cost_terms.empty()); + std::vector sorted = cost_terms; + std::sort(sorted.begin(), sorted.end()); + return sorted[sorted.size() / 2]; + } + + double mean() const { + SOPHUS_ASSERT(!cost_terms.empty()); + double sum = 0.0; + for (double cost : cost_terms) { + sum += cost; + } + return sum / cost_terms.size(); + } + + std::vector cost_terms; +}; + +template