diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md
index ec2377616..867a07e65 100644
--- a/.github/ISSUE_TEMPLATE/bug_report.md
+++ b/.github/ISSUE_TEMPLATE/bug_report.md
@@ -14,8 +14,8 @@ assignees: ''
 **To Reproduce**
 [Steps to reproduce the behavior:]
 1.
-2. 
-3. 
+2.
+3.
 
 **Expected behavior**
 [A clear and concise description of what you expected to happen.]
diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
new file mode 100644
index 000000000..7af55f7e8
--- /dev/null
+++ b/.github/workflows/format.yml
@@ -0,0 +1,22 @@
+name: format
+
+on:
+  push:
+    branches:
+      - main
+  pull_request:
+    branches:
+      - main
+
+jobs:
+  pre-commit-check:
+      runs-on: ubuntu-latest
+      steps:
+        - name: Checkout workspace
+          uses: actions/checkout@v3
+        - name: Install pre-commit and install
+          run: |
+            pip install pre-commit
+            pre-commit install
+        - name: Run pre-commit checks
+          run: pre-commit run --all-files
diff --git a/.gitignore b/.gitignore
index 0aed9a819..d2df4b884 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
 build
 CMakeLists.txt.user
 *.pyc
-.vscode/settings.json
\ No newline at end of file
+.vscode/settings.json
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index d7355e0d4..8fba72242 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -12,6 +12,7 @@ repos:
     rev: v14.0.0
     hooks:
       - id: clang-format
+        exclude: (sympy/.*)
         types_or: [file]
         files: .*(\.h|\.cpp|\.h\.glsl|\.proto)$
   - repo: https://github.com/pre-commit/mirrors-prettier
@@ -23,4 +24,5 @@ repos:
     rev: v0.6.10
     hooks:
       - id: cmake-format
-      - id: cmake-lint
+      # lint does not pass
+      #- id: cmake-lint
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e9d85bdc2..477b568bf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,44 +4,48 @@ project(Sophus VERSION 1.24.06)
 include(CMakePackageConfigHelpers)
 include(GNUInstallDirs)
 
-# Determine if sophus is built as a subproject (using add_subdirectory)
-# or if it is the master project.
-if (NOT DEFINED SOPHUS_MASTER_PROJECT)
+# 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)
+  if(CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR)
     set(SOPHUS_MASTER_PROJECT ON)
     message(STATUS "CMake version: ${CMAKE_VERSION}")
-  endif ()
-endif ()
+  endif()
+endif()
 
 option(SOPHUS_INSTALL "Generate the install target." ${SOPHUS_MASTER_PROJECT})
 
 if(SOPHUS_MASTER_PROJECT)
-  # Release by default
-  # Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug"
+  # Release by default Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug"
   if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE RelWithDebInfo)
   endif()
 
   set(CMAKE_CXX_STANDARD 17)
 
-  # Set compiler specific settings (FixMe: Should not cmake do this for us automatically?)
-  IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
-    SET(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g")
-    SET(CMAKE_CXX_FLAGS_RELEASE "-O3")
-    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra  -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics")
-  ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
-    SET(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g")
-    SET(CMAKE_CXX_FLAGS_RELEASE "-O3")
-    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++14 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0 -Wno-array-bounds")
-  ENDIF()
+  # Set compiler specific settings (FixMe: Should not cmake do this for us
+  # automatically?)
+  if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+    set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g")
+    set(CMAKE_CXX_FLAGS_RELEASE "-O3")
+    set(CMAKE_CXX_FLAGS
+        "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra  -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics"
+    )
+  elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
+    set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g")
+    set(CMAKE_CXX_FLAGS_RELEASE "-O3")
+    set(CMAKE_CXX_FLAGS
+        "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++14 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0 -Wno-array-bounds"
+    )
+  endif()
 
   # Add local path for finding packages, set the local version first
   list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules")
 endif()
 
-# Find public dependencies if targets are not yet defined. (Targets might be for example
-# defined by a parent project including Sophus via `add_subdirectory`.)
+# Find public dependencies if targets are not yet defined. (Targets might be for
+# example defined by a parent project including Sophus via `add_subdirectory`.)
 
 if(NOT TARGET Eigen3::Eigen)
   find_package(Eigen3 3.3.0 REQUIRED)
@@ -51,73 +55,62 @@ if(NOT TARGET fmt::fmt)
   find_package(fmt)
 endif()
 
-
 # Define interface library target
 add_library(sophus INTERFACE)
-add_library (Sophus::Sophus ALIAS sophus)
+add_library(Sophus::Sophus ALIAS sophus)
 
 set(SOPHUS_HEADER_FILES
-  sophus/average.hpp
-  sophus/cartesian.hpp
-  sophus/ceres_manifold.hpp
-  sophus/ceres_typetraits.hpp
-  sophus/common.hpp
-  sophus/geometry.hpp
-  sophus/interpolate.hpp
-  sophus/interpolate_details.hpp
-  sophus/num_diff.hpp
-  sophus/rotation_matrix.hpp
-  sophus/rxso2.hpp
-  sophus/rxso3.hpp
-  sophus/se2.hpp
-  sophus/se3.hpp
-  sophus/sim2.hpp
-  sophus/sim3.hpp
-  sophus/sim_details.hpp
-  sophus/so2.hpp
-  sophus/so3.hpp
-  sophus/spline.hpp
-  sophus/types.hpp
-)
-
-set(SOPHUS_OTHER_FILES
-  sophus/test_macros.hpp
-  sophus/example_ensure_handler.cpp
-)
+    sophus/average.hpp
+    sophus/cartesian.hpp
+    sophus/ceres_manifold.hpp
+    sophus/ceres_typetraits.hpp
+    sophus/common.hpp
+    sophus/geometry.hpp
+    sophus/interpolate.hpp
+    sophus/interpolate_details.hpp
+    sophus/num_diff.hpp
+    sophus/rotation_matrix.hpp
+    sophus/rxso2.hpp
+    sophus/rxso3.hpp
+    sophus/se2.hpp
+    sophus/se3.hpp
+    sophus/sim2.hpp
+    sophus/sim3.hpp
+    sophus/sim_details.hpp
+    sophus/so2.hpp
+    sophus/so3.hpp
+    sophus/spline.hpp
+    sophus/types.hpp)
+
+set(SOPHUS_OTHER_FILES sophus/test_macros.hpp sophus/example_ensure_handler.cpp)
 
 if(MSVC)
   # Define common math constants if we compile with MSVC
-  target_compile_definitions (sophus INTERFACE _USE_MATH_DEFINES)
-endif (MSVC)
+  target_compile_definitions(sophus INTERFACE _USE_MATH_DEFINES)
+endif(MSVC)
 
 # Add Eigen interface dependency, depending on available cmake info
 if(TARGET Eigen3::Eigen)
   target_link_libraries(sophus INTERFACE Eigen3::Eigen)
   set(Eigen3_DEPENDENCY "find_dependency (Eigen3 ${Eigen3_VERSION})")
 else()
-  target_include_directories (sophus SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR})
+  target_include_directories(sophus SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR})
 endif()
 
-
 target_link_libraries(sophus INTERFACE fmt::fmt)
 set(fmt_DEPENDENCY "find_dependency (fmt ${fmt_VERSION})")
 message(STATUS "Turning basic logging OFF")
 
 # Associate target with include directory
-target_include_directories(sophus INTERFACE
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>"
-    "$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>"
-)
+target_include_directories(
+  sophus INTERFACE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>"
+                   "$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>")
 
 # Declare all used C++14 features
-target_compile_features (sophus INTERFACE
-  cxx_auto_type
-  cxx_decltype
-  cxx_nullptr
-  cxx_right_angle_brackets
-  cxx_variadic_macros
-  cxx_variadic_templates
-)
+target_compile_features(
+  sophus
+  INTERFACE cxx_auto_type cxx_decltype cxx_nullptr cxx_right_angle_brackets
+            cxx_variadic_macros cxx_variadic_templates)
 
 # Add sources as custom target so that they are shown in IDE's
 add_custom_target(other SOURCES ${SOPHUS_OTHER_FILES} ${SOPHUS_HEADER_FILES})
@@ -125,8 +118,8 @@ add_custom_target(other SOURCES ${SOPHUS_OTHER_FILES} ${SOPHUS_HEADER_FILES})
 # Create 'test' make target using ctest
 option(BUILD_SOPHUS_TESTS "Build tests." ON)
 if(BUILD_SOPHUS_TESTS)
-    enable_testing()
-    add_subdirectory(test)
+  enable_testing()
+  add_subdirectory(test)
 endif()
 
 # Build python sophus bindings
@@ -134,14 +127,14 @@ option(BUILD_PYTHON_BINDINGS "Build python sophus bindings." OFF)
 if(BUILD_PYTHON_BINDINGS)
   include(FetchContent)
   FetchContent_Declare(
-      pybind11
-      GIT_REPOSITORY https://github.com/pybind/pybind11.git
-      GIT_TAG master
-  )
+    pybind11
+    GIT_REPOSITORY https://github.com/pybind/pybind11.git
+    GIT_TAG master)
   FetchContent_MakeAvailable(pybind11)
 
   add_subdirectory(${pybind11_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}/pybind)
-  pybind11_add_module(sophus_pybind ${CMAKE_CURRENT_SOURCE_DIR}/sophus_pybind/bindings.cpp)
+  pybind11_add_module(sophus_pybind
+                      ${CMAKE_CURRENT_SOURCE_DIR}/sophus_pybind/bindings.cpp)
   target_link_libraries(sophus_pybind PUBLIC sophus)
 endif(BUILD_PYTHON_BINDINGS)
 
@@ -152,45 +145,41 @@ if(SOPHUS_INSTALL)
   set_target_properties(sophus PROPERTIES EXPORT_NAME Sophus)
 
   install(TARGETS sophus EXPORT SophusTargets)
-  install(EXPORT SophusTargets
+  install(
+    EXPORT SophusTargets
     NAMESPACE Sophus::
-    DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}
-  )
+    DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR})
 
-  export(TARGETS sophus NAMESPACE Sophus:: FILE SophusTargets.cmake)
+  export(
+    TARGETS sophus
+    NAMESPACE Sophus::
+    FILE SophusTargets.cmake)
   export(PACKAGE Sophus)
 
   configure_package_config_file(
-    SophusConfig.cmake.in
-    ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake
+    SophusConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake
     INSTALL_DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}
-    NO_CHECK_REQUIRED_COMPONENTS_MACRO
-  )
+    NO_CHECK_REQUIRED_COMPONENTS_MACRO)
 
   # Remove architecture dependence. Sophus is a header-only library.
   set(TEMP_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P})
   unset(CMAKE_SIZEOF_VOID_P)
 
   # Write version to file
-  write_basic_package_version_file (
+  write_basic_package_version_file(
     SophusConfigVersion.cmake
     VERSION ${PROJECT_VERSION}
-    COMPATIBILITY SameMajorVersion
-  )
+    COMPATIBILITY SameMajorVersion)
 
   # Recover architecture dependence
   set(CMAKE_SIZEOF_VOID_P ${TEMP_SIZEOF_VOID_P})
 
   # Install cmake targets
-  install(
-    FILES ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake
-          ${CMAKE_CURRENT_BINARY_DIR}/SophusConfigVersion.cmake
-    DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}
-  )
+  install(FILES ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake
+                ${CMAKE_CURRENT_BINARY_DIR}/SophusConfigVersion.cmake
+          DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR})
 
   # Install header files
-  install(
-    FILES ${SOPHUS_HEADER_FILES}
-    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sophus
-  )
+  install(FILES ${SOPHUS_HEADER_FILES}
+          DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sophus)
 endif()
diff --git a/LICENSE.txt b/LICENSE.txt
index fb67702de..5e0fd3e3f 100644
--- a/LICENSE.txt
+++ b/LICENSE.txt
@@ -1,4 +1,4 @@
-Copyright 2011-2017 Hauke Strasdat           
+Copyright 2011-2017 Hauke Strasdat
           2012-2017 Steven Lovegrove
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
diff --git a/README.md b/README.md
index 4b0f8b7c2..d272a8927 100644
--- a/README.md
+++ b/README.md
@@ -26,7 +26,7 @@ This is a c++ implementation of Lie groups commonly used for 2d and 3d
 geometric problems (i.e. for Computer Vision or Robotics applications).
 Among others, this package includes the special orthogonal groups SO(2) and
 SO(3) to present rotations in 2d and 3d as well as the special Euclidean group
-SE(2) and SE(3) to represent isometries also known as rigid body transformations 
+SE(2) and SE(3) to represent isometries also known as rigid body transformations
 (i.e. rotations and translations) in 2d and 3d.
 
 ## Status
@@ -40,21 +40,21 @@ However, next incarnations of Sophus are under development:
 
 
  - sophus2 is the next c++ iteration of Sophus and is a complete rewrite.
-   In addition to the Lie groups, it includes a more geometric concepts 
+   In addition to the Lie groups, it includes a more geometric concepts
    such unit vector, splines, image classes, camera models and more.
 
-   It is currently hosted as part of the [farm-ng-core repository](https://github.com/farm-ng/farm-ng-core/tree/cygnet-dev) 
-   and has likely only a few community users. While the code itself is in a good shape, there are 
+   It is currently hosted as part of the [farm-ng-core repository](https://github.com/farm-ng/farm-ng-core/tree/cygnet-dev)
+   and has likely only a few community users. While the code itself is in a good shape, there are
    no good build instructions yet. Hopefully, this will change in the near future.
 
 
- - sophus-rs is a Rust version of Sophus. Similar to sophus2, it includes a more geometric concepts 
+ - sophus-rs is a Rust version of Sophus. Similar to sophus2, it includes a more geometric concepts
    such unit vector, splines, image classes, camera models and more. Also it includes an early and
-   experimental version of non-linear least squares optimization library (similar to Ceres, g2o, 
+   experimental version of non-linear least squares optimization library (similar to Ceres, g2o,
    etc.).
- 
-   sophus-rs has likely only a few community users so far, but should be easy to build and 
-   experiment with - of course being written in Rust. 
+
+   sophus-rs has likely only a few community users so far, but should be easy to build and
+   experiment with - of course being written in Rust.
 
    https://github.com/sophus-vision/sophus-rs
 
@@ -67,9 +67,9 @@ How to build
 
 Sophus requires a C++17 compiler (though older versions build with C++14).
 
-Sophus is tested on Linux and macOS. It also worked on Windows in the past, however there is 
+Sophus is tested on Linux and macOS. It also worked on Windows in the past, however there is
 currently no CI for Windows, so it might require some smaller patches to build on Windows.
 
 There are no comprehensive build instructions but inspecting the install [scripts](scripts/)
-as well as the [main.yml](.github/workflows/main.yml) file should give you a good idea how to 
-build the required dependencies.
\ No newline at end of file
+as well as the [main.yml](.github/workflows/main.yml) file should give you a good idea how to
+build the required dependencies.
diff --git a/Sophus.code-workspace b/Sophus.code-workspace
index 8fef94ca5..b3aa39876 100644
--- a/Sophus.code-workspace
+++ b/Sophus.code-workspace
@@ -67,4 +67,4 @@
 			"typeinfo": "cpp"
 		}
 	}
-}
\ No newline at end of file
+}
diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake
index 0b36805e7..476c7ea28 100644
--- a/cmake_modules/FindEigen3.cmake
+++ b/cmake_modules/FindEigen3.cmake
@@ -1,29 +1,26 @@
-# - Try to find Eigen3 lib
+# * Try to find Eigen3 lib
 #
 # This module supports requiring a minimum version, e.g. you can do
-#   find_package(Eigen3 3.1.2)
-# to require version 3.1.2 or newer of Eigen3.
+# find_package(Eigen3 3.1.2) to require version 3.1.2 or newer of Eigen3.
 #
 # Once done this will define
 #
-#  EIGEN3_FOUND - system has eigen lib with correct version
-#  EIGEN3_INCLUDE_DIR - the eigen include directory
-#  EIGEN3_VERSION - eigen version
+# EIGEN3_FOUND - system has eigen lib with correct version EIGEN3_INCLUDE_DIR -
+# the eigen include directory EIGEN3_VERSION - eigen version
 #
 # and the following imported target:
 #
-#  Eigen3::Eigen - The header-only Eigen library
+# Eigen3::Eigen - The header-only Eigen library
 #
-# This module reads hints about search locations from 
-# the following environment variables:
+# This module reads hints about search locations from the following environment
+# variables:
 #
-# EIGEN3_ROOT
-# EIGEN3_ROOT_DIR
+# EIGEN3_ROOT EIGEN3_ROOT_DIR
 
-# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
-# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
-# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
-# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org> Copyright (c) 2008,
+# 2009 Gael Guennebaud, <g.gael@free.fr> Copyright (c) 2009 Benoit Jacob
+# <jacob.benoit.1@gmail.com> Redistribution and use is allowed according to the
+# terms of the 2-clause BSD license.
 
 if(NOT Eigen3_FIND_VERSION)
   if(NOT Eigen3_FIND_VERSION_MAJOR)
@@ -36,20 +33,27 @@ if(NOT Eigen3_FIND_VERSION)
     set(Eigen3_FIND_VERSION_PATCH 0)
   endif()
 
-  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+  set(Eigen3_FIND_VERSION
+      "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}"
+  )
 endif()
 
 macro(_eigen3_check_version)
-  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h"
+       _eigen3_version_header)
 
-  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)"
+               _eigen3_world_version_match "${_eigen3_version_header}")
   set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)"
+               _eigen3_major_version_match "${_eigen3_version_header}")
   set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)"
+               _eigen3_minor_version_match "${_eigen3_version_header}")
   set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
 
-  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  set(EIGEN3_VERSION
+      ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
   if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
     set(EIGEN3_VERSION_OK FALSE)
   else()
@@ -58,35 +62,33 @@ macro(_eigen3_check_version)
 
   if(NOT EIGEN3_VERSION_OK)
 
-    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
-                   "but at least version ${Eigen3_FIND_VERSION} is required")
+    message(
+      STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+             "but at least version ${Eigen3_FIND_VERSION} is required")
   endif()
 endmacro()
 
-if (EIGEN3_INCLUDE_DIR)
+if(EIGEN3_INCLUDE_DIR)
 
   # in cache already
   _eigen3_check_version()
   set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
   set(Eigen3_FOUND ${EIGEN3_VERSION_OK})
 
-else ()
-  
-  # search first if an Eigen3Config.cmake is available in the system,
-  # if successful this would set EIGEN3_INCLUDE_DIR and the rest of
-  # the script will work as usual
+else()
+
+  # search first if an Eigen3Config.cmake is available in the system, if
+  # successful this would set EIGEN3_INCLUDE_DIR and the rest of the script will
+  # work as usual
   find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET)
 
   if(NOT EIGEN3_INCLUDE_DIR)
-    find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
-        HINTS
-        ENV EIGEN3_ROOT 
-        ENV EIGEN3_ROOT_DIR
-        PATHS
-        ${CMAKE_INSTALL_PREFIX}/include
-        ${KDE4_INCLUDE_DIR}
-        PATH_SUFFIXES eigen3 eigen
-      )
+    find_path(
+      EIGEN3_INCLUDE_DIR
+      NAMES signature_of_eigen3_matrix_library
+      HINTS ENV EIGEN3_ROOT ENV EIGEN3_ROOT_DIR
+      PATHS ${CMAKE_INSTALL_PREFIX}/include ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen)
   endif()
 
   if(EIGEN3_INCLUDE_DIR)
@@ -94,7 +96,8 @@ else ()
   endif()
 
   include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR
+                                    EIGEN3_VERSION_OK)
 
   mark_as_advanced(EIGEN3_INCLUDE_DIR)
 
@@ -102,6 +105,6 @@ endif()
 
 if(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen)
   add_library(Eigen3::Eigen INTERFACE IMPORTED)
-  set_target_properties(Eigen3::Eigen PROPERTIES
-    INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}")
+  set_target_properties(Eigen3::Eigen PROPERTIES INTERFACE_INCLUDE_DIRECTORIES
+                                                 "${EIGEN3_INCLUDE_DIR}")
 endif()
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 5c18f2da9..38ef83f11 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -2,14 +2,12 @@ cmake_minimum_required(VERSION 3.16)
 
 project(SophusExample)
 
-
 find_package(Sophus 1.22.06 REQUIRED)
 
-
 # Tests to run
-SET( EXAMPLE_SOURCES HelloSO3)
+set(EXAMPLE_SOURCES HelloSO3)
 
-FOREACH(example_src ${EXAMPLE_SOURCES})
-  ADD_EXECUTABLE( ${example_src} ${example_src}.cpp)
-  TARGET_LINK_LIBRARIES( ${example_src} Sophus::Sophus)
-ENDFOREACH(example_src)
+foreach(example_src ${EXAMPLE_SOURCES})
+  add_executable(${example_src} ${example_src}.cpp)
+  target_link_libraries(${example_src} Sophus::Sophus)
+endforeach(example_src)
diff --git a/examples/HelloSO3.cpp b/examples/HelloSO3.cpp
index ad998a353..a9ccf13ab 100644
--- a/examples/HelloSO3.cpp
+++ b/examples/HelloSO3.cpp
@@ -35,4 +35,4 @@ int main() {
   // Note that the order of coefficients of Eigen's quaternion class is
   // (imag0, imag1, imag2, real)
   std::cout << std::endl;
-}
\ No newline at end of file
+}
diff --git a/scripts/install_ubuntu_deps_incl_ceres.sh b/scripts/install_ubuntu_deps_incl_ceres.sh
index ff3977444..c1dc08abe 100755
--- a/scripts/install_ubuntu_deps_incl_ceres.sh
+++ b/scripts/install_ubuntu_deps_incl_ceres.sh
@@ -17,4 +17,4 @@ ccache -s
 cmake -DMINIGLOG=On -DCMAKE_CXX_COMPILER_LAUNCHER=ccache ..
 make -j8
 sudo make install
-cd ../..
\ No newline at end of file
+cd ../..
diff --git a/sophus_pybind/README b/sophus_pybind/README
index 0c5a5bbce..e7e5a2e19 100644
--- a/sophus_pybind/README
+++ b/sophus_pybind/README
@@ -1,6 +1,6 @@
-# Python binding for Sophus library: sophus-pybind 
+# Python binding for Sophus library: sophus-pybind
 sophus-pybind implement python binding for sophus that provides access to SO3, SE3, interpolate and iterativeMean features.
-The user interface is inspired by scipy.spacial.transform.Rotation. 
+The user interface is inspired by scipy.spacial.transform.Rotation.
 Here is a specific list of features.
 
 ## Feature list
@@ -19,15 +19,15 @@ Here is a specific list of features.
   * Function vectorization
   * Inverse, copy, print, and len
   * Interpolate between two SE3
-  * Iterative mean of a group of SE3 
+  * Iterative mean of a group of SE3
 
-## Python module (pysophus) installation step 
+## Python module (pysophus) installation step
 ```
 # Create virtual environment
 python3 -m venv ~/sophus_venv
 source ~/sophus_venv/bin/activate
 
-# Install package 
+# Install package
 git clone <sophus-package>
 cd Sophus
 pip install .
@@ -58,10 +58,10 @@ pip install .
 ## Vectorization detail
 In python, we choose to export our Sophus::SO3 as a vector of SO3 objects by binding the cpp object `SO3Group` defined below. This is because numerical code in python tends to work with array of values to get efficient program. This approach is inspired by scipy.spatial.transform.Rotation.
 ```
-class SO3Group : public std::vector<Sophus::SO3<Scalar>> 
-class SE3Group : public std::vector<Sophus::SE3<Scalar>> 
+class SO3Group : public std::vector<Sophus::SO3<Scalar>>
+class SE3Group : public std::vector<Sophus::SE3<Scalar>>
 ```
 
 ### Passing a single SO3/SE3 object to c++ code in python binding
 To allow other python binding c++ code to take in a single SO3/SE3 object, we also build a caster so that, even if we wrap SO3Group/SE3Group in python, those can be implicitly converted to the c++ Sophus::SO3/SE3 object at boundaries between languages.
-This is so we can pass python SO3/SE3 object to c++ function as if they were regular 1-element Sophus::SO3/SE3 object. This simplifies binding the rest of c++ code. The implicit cast fails if the python object is not a 1-element object.
\ No newline at end of file
+This is so we can pass python SO3/SE3 object to c++ function as if they were regular 1-element Sophus::SO3/SE3 object. This simplifies binding the rest of c++ code. The implicit cast fails if the python object is not a 1-element object.
diff --git a/sophus_pybind/SE3PyBind.h b/sophus_pybind/SE3PyBind.h
index ebf8da6f3..cca162d13 100644
--- a/sophus_pybind/SE3PyBind.h
+++ b/sophus_pybind/SE3PyBind.h
@@ -14,8 +14,8 @@
 
 namespace Sophus {
 
-// In python, we choose to export our Sophus::SE3 as a vector of SE3 objects by binding the cpp
-// object `SE3Group` defined below.
+// In python, we choose to export our Sophus::SE3 as a vector of SE3 objects by
+// binding the cpp object `SE3Group` defined below.
 template <typename Scalar>
 class SE3Group : public std::vector<Sophus::SE3<Scalar>> {
  public:
@@ -23,19 +23,17 @@ class SE3Group : public std::vector<Sophus::SE3<Scalar>> {
   // Python always create at least one identity element (like c++ sophus)
   SE3Group() = default;
   // implicit copy conversion from a Sophus::SE3 value
-  SE3Group(const Sophus::SE3<Scalar>& in) {
-    this->push_back(in);
-  }
+  SE3Group(const Sophus::SE3<Scalar>& in) { this->push_back(in); }
 };
-} // namespace Sophus
-
-// The following caster makes so that, even if we wrap SE3Group in python, those can be
-// implicitly converted to the c++ Sophus::SE3 object at boundaries between languages.
-// This is so we can pass python SE3 object to c++ function as if they were regular 1-element
-// Sophus::SE3 object. This simplifies binding the rest of c++ code. This implicit cast fails if the
-// python object is not a 1-element SE3 object.
-// NOTE: this caster makes a copy, so can't not be used for passing a reference of a SE3 element to
-// a c++ function.
+}  // namespace Sophus
+
+// The following caster makes so that, even if we wrap SE3Group in python, those
+// can be implicitly converted to the c++ Sophus::SE3 object at boundaries
+// between languages. This is so we can pass python SE3 object to c++ function
+// as if they were regular 1-element Sophus::SE3 object. This simplifies binding
+// the rest of c++ code. This implicit cast fails if the python object is not a
+// 1-element SE3 object. NOTE: this caster makes a copy, so can't not be used
+// for passing a reference of a SE3 element to a c++ function.
 namespace pybind11 {
 namespace detail {
 template <>
@@ -49,22 +47,25 @@ struct type_caster<Sophus::SE3<double>> {
       Sophus::SE3Group<double>& ref = src.cast<Sophus::SE3Group<double>&>();
       if (ref.size() != 1) {
         throw std::domain_error(fmt::format(
-            "A element of size 1 is required here. Input has {} elements.", ref.size()));
+            "A element of size 1 is required here. Input has {} elements.",
+            ref.size()));
       }
       value = ref[0];
       return true;
     } catch (const pybind11::cast_error&) {
-      return false; // Conversion failed
+      return false;  // Conversion failed
     }
   }
 
   // converting from c++ -> python type
-  static handle cast(Sophus::SE3<double> src, return_value_policy policy, handle parent) {
-    return type_caster_base<Sophus::SE3Group<double>>::cast(Sophus::SE3Group<double>(src), policy, parent);
+  static handle cast(Sophus::SE3<double> src, return_value_policy policy,
+                     handle parent) {
+    return type_caster_base<Sophus::SE3Group<double>>::cast(
+        Sophus::SE3Group<double>(src), policy, parent);
   }
 };
-} // namespace detail
-} // namespace pybind11
+}  // namespace detail
+}  // namespace pybind11
 
 namespace Sophus {
 /*SE3*/
@@ -72,49 +73,58 @@ template <typename Scalar>
 using PybindSE3Type = pybind11::class_<SE3Group<Scalar>>;
 
 template <typename Scalar>
-PybindSE3Type<Scalar> exportSE3Transformation(
-    pybind11::module& module,
-    const std::string& name = "SE3") {
+PybindSE3Type<Scalar> exportSE3Transformation(pybind11::module& module,
+                                              const std::string& name = "SE3") {
   PybindSE3Type<Scalar> type(module, name.c_str());
 
-  type.def(
-      pybind11::init([]() {
-        SE3Group<Scalar> ret;
-        ret.push_back({});
-        return ret;
-      }),
-      " Default Constructor initializing a group containing 1 identity element");
-  type.def(pybind11::init<const Sophus::SE3<Scalar>&>(), "Copy constructor from single element");
-
-  type.def_static("from_matrix", [](const Eigen::Matrix<Scalar, 4, 4>& matrix) -> SE3Group<Scalar> {
-    return SE3Group<Scalar>{Sophus::SE3<Scalar>::fitToSE3(matrix)};
-  });
-  type.def_static("from_matrix", [](const pybind11::array_t<Scalar>& matrices) -> SE3Group<Scalar> {
-    if (matrices.ndim() != 3 || matrices.shape(1) != 4 || matrices.shape(2) != 4) {
-      throw std::runtime_error(
-          fmt::format("The size of the input matrix should be Nx4x4 dimensions."));
-    }
+  type.def(pybind11::init([]() {
+             SE3Group<Scalar> ret;
+             ret.push_back({});
+             return ret;
+           }),
+           " Default Constructor initializing a group containing 1 identity "
+           "element");
+  type.def(pybind11::init<const Sophus::SE3<Scalar>&>(),
+           "Copy constructor from single element");
 
-    SE3Group<Scalar> output;
-    output.reserve(matrices.shape(0));
-    for (size_t i = 0; i < matrices.shape(0); ++i) {
-      Eigen::Map<const Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>> mat(matrices.data(i, 0, 0));
-      output.push_back(Sophus::SE3<Scalar>::fitToSE3(mat));
-    }
-    return output;
-  });
+  type.def_static(
+      "from_matrix",
+      [](const Eigen::Matrix<Scalar, 4, 4>& matrix) -> SE3Group<Scalar> {
+        return SE3Group<Scalar>{Sophus::SE3<Scalar>::fitToSE3(matrix)};
+      });
+  type.def_static(
+      "from_matrix",
+      [](const pybind11::array_t<Scalar>& matrices) -> SE3Group<Scalar> {
+        if (matrices.ndim() != 3 || matrices.shape(1) != 4 ||
+            matrices.shape(2) != 4) {
+          throw std::runtime_error(fmt::format(
+              "The size of the input matrix should be Nx4x4 dimensions."));
+        }
+
+        SE3Group<Scalar> output;
+        output.reserve(matrices.shape(0));
+        for (size_t i = 0; i < matrices.shape(0); ++i) {
+          Eigen::Map<const Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>> mat(
+              matrices.data(i, 0, 0));
+          output.push_back(Sophus::SE3<Scalar>::fitToSE3(mat));
+        }
+        return output;
+      });
 
   type.def_static(
-      "from_matrix3x4", [](const Eigen::Matrix<Scalar, 3, 4>& matrix) -> SE3Group<Scalar> {
+      "from_matrix3x4",
+      [](const Eigen::Matrix<Scalar, 3, 4>& matrix) -> SE3Group<Scalar> {
         return SE3Group<Scalar>{Sophus::SE3<Scalar>(
             Sophus::SO3<Scalar>::fitToSO3(matrix.template block<3, 3>(0, 0)),
             matrix.template block<3, 1>(0, 3))};
       });
   type.def_static(
-      "from_matrix3x4", [](const pybind11::array_t<Scalar>& matrices) -> SE3Group<Scalar> {
-        if (matrices.ndim() != 3 || matrices.shape(1) != 3 || matrices.shape(2) != 4) {
-          throw std::runtime_error(
-              fmt::format("The size of the input matrix should be Nx3x4 dimensions."));
+      "from_matrix3x4",
+      [](const pybind11::array_t<Scalar>& matrices) -> SE3Group<Scalar> {
+        if (matrices.ndim() != 3 || matrices.shape(1) != 3 ||
+            matrices.shape(2) != 4) {
+          throw std::runtime_error(fmt::format(
+              "The size of the input matrix should be Nx3x4 dimensions."));
         }
 
         SE3Group<Scalar> output;
@@ -133,40 +143,43 @@ PybindSE3Type<Scalar> exportSE3Transformation(
       "exp",
       [](const Eigen::Matrix<Scalar, 3, 1>& translational_part,
          const Eigen::Matrix<Scalar, 3, 1>& rotvec) -> SE3Group<Scalar> {
-        auto tangentVec = Eigen::Matrix<Scalar, 6, 1>{
-            translational_part[0],
-            translational_part[1],
-            translational_part[2],
-            rotvec[0],
-            rotvec[1],
-            rotvec[2]};
+        auto tangentVec = Eigen::Matrix<Scalar, 6, 1>{translational_part[0],
+                                                      translational_part[1],
+                                                      translational_part[2],
+                                                      rotvec[0],
+                                                      rotvec[1],
+                                                      rotvec[2]};
         return {Sophus::SE3<Scalar>::exp(tangentVec)};
       },
-      "Create SE3 from a translational_part (3x1) and a rotation vector (3x1) of magnitude in rad. NOTE: translational_part is not translation vector in SE3");
+      "Create SE3 from a translational_part (3x1) and a rotation vector (3x1) "
+      "of magnitude in rad. NOTE: translational_part is not translation vector "
+      "in SE3");
   type.def_static(
       "exp",
       [](const Eigen::Matrix<Scalar, Eigen::Dynamic, 3>& translational_parts,
-         const Eigen::Matrix<Scalar, Eigen::Dynamic, 3>& rotvecs) -> SE3Group<Scalar> {
+         const Eigen::Matrix<Scalar, Eigen::Dynamic, 3>& rotvecs)
+          -> SE3Group<Scalar> {
         SE3Group<Scalar> output;
         output.reserve(rotvecs.rows());
         for (size_t i = 0; i < rotvecs.rows(); ++i) {
-          auto tangentVec = Eigen::Matrix<Scalar, 6, 1>{
-              translational_parts(i, 0),
-              translational_parts(i, 1),
-              translational_parts(i, 2),
-              rotvecs(i, 0),
-              rotvecs(i, 1),
-              rotvecs(i, 2)};
+          auto tangentVec =
+              Eigen::Matrix<Scalar, 6, 1>{translational_parts(i, 0),
+                                          translational_parts(i, 1),
+                                          translational_parts(i, 2),
+                                          rotvecs(i, 0),
+                                          rotvecs(i, 1),
+                                          rotvecs(i, 2)};
           output.emplace_back(Sophus::SE3<Scalar>::exp(tangentVec));
         }
         return output;
       },
-      "Create a set of SE3 from translational_parts (Nx3) and rotation vectors (Nx3) of magnitude in rad. NOTE: translational_part is not translation vector in SE3");
+      "Create a set of SE3 from translational_parts (Nx3) and rotation vectors "
+      "(Nx3) of magnitude in rad. NOTE: translational_part is not translation "
+      "vector in SE3");
 
   type.def(
       "from_quat_and_translation",
-      [](const Scalar& w,
-         const Eigen::Matrix<Scalar, 3, 1>& xyz,
+      [](const Scalar& w, const Eigen::Matrix<Scalar, 3, 1>& xyz,
          const Eigen::Matrix<Scalar, 3, 1>& translation) -> SE3Group<Scalar> {
         Eigen::Quaternion<Scalar> quat(w, xyz[0], xyz[1], xyz[2]);
         quat.normalize();
@@ -179,30 +192,33 @@ PybindSE3Type<Scalar> exportSE3Transformation(
       [](const std::vector<Scalar>& x_vec,
          const Eigen::Matrix<Scalar, -1, 3>& xyz_vec,
          const Eigen::Matrix<Scalar, -1, 3>& translations) -> SE3Group<Scalar> {
-        if (x_vec.size() != xyz_vec.rows() || x_vec.size() != translations.rows()) {
-          throw std::domain_error(fmt::format(
-              "Size of the input variables are not the same: x_vec = {}, xyz_vec = {}, translation = {}",
-              x_vec.size(),
-              xyz_vec.rows(),
-              translations.rows()));
+        if (x_vec.size() != xyz_vec.rows() ||
+            x_vec.size() != translations.rows()) {
+          throw std::domain_error(
+              fmt::format("Size of the input variables are not the same: x_vec "
+                          "= {}, xyz_vec = {}, translation = {}",
+                          x_vec.size(), xyz_vec.rows(), translations.rows()));
         }
         SE3Group<Scalar> output;
         output.reserve(x_vec.size());
         for (size_t i = 0; i < x_vec.size(); ++i) {
-          Eigen::Quaternion<Scalar> quat(x_vec[i], xyz_vec(i, 0), xyz_vec(i, 1), xyz_vec(i, 2));
+          Eigen::Quaternion<Scalar> quat(x_vec[i], xyz_vec(i, 0), xyz_vec(i, 1),
+                                         xyz_vec(i, 2));
           quat.normalize();
           output.push_back(Sophus::SE3<Scalar>(quat, translations.row(i)));
         }
         return output;
       },
-      "Create SE3 from a list of quaternion as w_vec: Nx1, xyz_vec: Nx3, and a list of translation vectors: Nx3");
+      "Create SE3 from a list of quaternion as w_vec: Nx1, xyz_vec: Nx3, and a "
+      "list of translation vectors: Nx3");
 
   type.def(
       "to_matrix3x4",
       [](const SE3Group<Scalar>& transformations) -> pybind11::array_t<Scalar> {
         pybind11::array_t<Scalar> result(
             std::vector<long>{long(transformations.size()), 3, 4},
-            std::vector<long>{12 * sizeof(Scalar), 4 * sizeof(Scalar), sizeof(Scalar)});
+            std::vector<long>{12 * sizeof(Scalar), 4 * sizeof(Scalar),
+                              sizeof(Scalar)});
 
         for (size_t i = 0; i < transformations.size(); i++) {
           Eigen::Map<Eigen::Matrix<Scalar, 3, 4, Eigen::RowMajor>> map(
@@ -211,14 +227,16 @@ PybindSE3Type<Scalar> exportSE3Transformation(
         }
         return result.squeeze();
       },
-      "Convert an array of SE3 into an array of transformation matrices of size 3x4");
+      "Convert an array of SE3 into an array of transformation matrices of "
+      "size 3x4");
 
   type.def(
       "to_matrix",
       [](const SE3Group<Scalar>& transformations) -> pybind11::array_t<Scalar> {
         pybind11::array_t<Scalar> result(
             std::vector<long>{long(transformations.size()), 4, 4},
-            std::vector<long>{16 * sizeof(Scalar), 4 * sizeof(Scalar), sizeof(Scalar)});
+            std::vector<long>{16 * sizeof(Scalar), 4 * sizeof(Scalar),
+                              sizeof(Scalar)});
 
         for (size_t i = 0; i < transformations.size(); i++) {
           Eigen::Map<Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>> map(
@@ -227,12 +245,15 @@ PybindSE3Type<Scalar> exportSE3Transformation(
         }
         return result.squeeze();
       },
-      "Convert an array of SE3 into an array of transformation matrices of size 4x4");
+      "Convert an array of SE3 into an array of transformation matrices of "
+      "size 4x4");
 
   type.def(
       "to_quat_and_translation",
-      [](const SE3Group<Scalar>& transformations) -> Eigen::Matrix<Scalar, Eigen::Dynamic, 7> {
-        auto output = Eigen::Matrix<Scalar, Eigen::Dynamic, 7>(transformations.size(), 7);
+      [](const SE3Group<Scalar>& transformations)
+          -> Eigen::Matrix<Scalar, Eigen::Dynamic, 7> {
+        auto output =
+            Eigen::Matrix<Scalar, Eigen::Dynamic, 7>(transformations.size(), 7);
         for (size_t i = 0; i < transformations.size(); ++i) {
           output.row(i) = Eigen::Matrix<Scalar, 1, 7>{
               transformations[i].so3().unit_quaternion().w(),
@@ -245,18 +266,22 @@ PybindSE3Type<Scalar> exportSE3Transformation(
         }
         return output;
       },
-      "Return quaternion and translation as Nx7 vectors of [quat (w, x, y, z), translation]");
+      "Return quaternion and translation as Nx7 vectors of [quat (w, x, y, z), "
+      "translation]");
 
   type.def(
       "log",
-      [](const SE3Group<Scalar>& transformations) -> Eigen::Matrix<Scalar, Eigen::Dynamic, 6> {
-        auto output = Eigen::Matrix<Scalar, Eigen::Dynamic, 6>(transformations.size(), 6);
+      [](const SE3Group<Scalar>& transformations)
+          -> Eigen::Matrix<Scalar, Eigen::Dynamic, 6> {
+        auto output =
+            Eigen::Matrix<Scalar, Eigen::Dynamic, 6>(transformations.size(), 6);
         for (size_t i = 0; i < transformations.size(); ++i) {
           output.row(i) = transformations[i].log();
         }
         return output;
       },
-      "Return the log of SE3 as [translational_part, rotation_vector] of dimension Nx6.");
+      "Return the log of SE3 as [translational_part, rotation_vector] of "
+      "dimension Nx6.");
 
   type.def(
       "inverse",
@@ -283,8 +308,10 @@ PybindSE3Type<Scalar> exportSE3Transformation(
       "Get the rotation component of the transformation.");
   type.def(
       "translation",
-      [](const SE3Group<Scalar>& transformations) -> Eigen::Matrix<Scalar, Eigen::Dynamic, 3> {
-        auto translations = Eigen::Matrix<Scalar, Eigen::Dynamic, 3>(transformations.size(), 3);
+      [](const SE3Group<Scalar>& transformations)
+          -> Eigen::Matrix<Scalar, Eigen::Dynamic, 3> {
+        auto translations =
+            Eigen::Matrix<Scalar, Eigen::Dynamic, 3>(transformations.size(), 3);
         for (size_t i = 0; i < transformations.size(); ++i) {
           translations.row(i) = transformations[i].translation();
         }
@@ -292,62 +319,66 @@ PybindSE3Type<Scalar> exportSE3Transformation(
       },
       "Get the translation component of the transformation.");
 
-  type.def("__copy__", [](const SE3Group<Scalar>& transformations) -> SE3Group<Scalar> {
-    return transformations; // copy is done with the std::vector copy constructor
+  type.def("__copy__",
+           [](const SE3Group<Scalar>& transformations) -> SE3Group<Scalar> {
+             return transformations;  // copy is done with the std::vector copy
+                                      // constructor
+           });
+  type.def("__repr__",
+           [](const SE3Group<Scalar>& transformation) -> std::string {
+             std::stringstream stream;
+             stream << fmt::format(
+                 "SE3 (quaternion(w,x,y,z), translation (x,y,z)) (x{})\n[",
+                 transformation.size());
+             for (const auto& se3 : transformation) {
+               stream << fmt::format(
+                   "[{}, {}, {}, {}, {}, {}, {}],\n", se3.unit_quaternion().w(),
+                   se3.unit_quaternion().x(), se3.unit_quaternion().y(),
+                   se3.unit_quaternion().z(), se3.translation().x(),
+                   se3.translation().y(), se3.translation().z());
+             }
+             // replace last to previous characters
+             stream.seekp(-2, stream.cur);
+             stream << "]";
+             return stream.str();
+           });
+  type.def("__len__", [](const SE3Group<Scalar>& transformations) {
+    return transformations.size();
   });
-  type.def("__repr__", [](const SE3Group<Scalar>& transformation) -> std::string {
-    std::stringstream stream;
-    stream << fmt::format(
-        "SE3 (quaternion(w,x,y,z), translation (x,y,z)) (x{})\n[", transformation.size());
-    for (const auto& se3 : transformation) {
-      stream << fmt::format(
-          "[{}, {}, {}, {}, {}, {}, {}],\n",
-          se3.unit_quaternion().w(),
-          se3.unit_quaternion().x(),
-          se3.unit_quaternion().y(),
-          se3.unit_quaternion().z(),
-          se3.translation().x(),
-          se3.translation().y(),
-          se3.translation().z());
-    }
-    // replace last to previous characters
-    stream.seekp(-2, stream.cur);
-    stream << "]";
-    return stream.str();
-  });
-  type.def(
-      "__len__", [](const SE3Group<Scalar>& transformations) { return transformations.size(); });
-
-  type.def("__str__", [](const SE3Group<Scalar>& transformations) -> std::string {
-    return fmt::format("Sophus.SE3 (x{})", transformations.size());
-  });
-
-  type.def(
-      "__matmul__",
-      [](const SE3Group<Scalar>& transformations,
-         const SE3Group<Scalar>& other) -> SE3Group<Scalar> {
-        if (other.size() == 0 || transformations.size() == 0) {
-          throw std::domain_error("Both operand should have size greater than 0");
-        }
-        SE3Group<Scalar> result;
-        if (other.size() == 1) {
-          result.reserve(transformations.size());
-          for (size_t i = 0; i < transformations.size(); ++i) {
-            result.push_back(transformations[i] * other[0]);
-          }
-        } else if (transformations.size() == 1) {
-          result.reserve(other.size());
-          for (size_t i = 0; i < other.size(); ++i) {
-            result.push_back(transformations[0] * other[i]);
-          }
-        } else {
-          throw std::domain_error(
-              "Only allows transformations of size 1 to N (or N to 1) multiplication.");
-        }
-        return result;
-      });
 
-  type.def("__imatmul__", [](SE3Group<Scalar>& transformations, const SE3Group<Scalar>& other) {
+  type.def("__str__",
+           [](const SE3Group<Scalar>& transformations) -> std::string {
+             return fmt::format("Sophus.SE3 (x{})", transformations.size());
+           });
+
+  type.def("__matmul__",
+           [](const SE3Group<Scalar>& transformations,
+              const SE3Group<Scalar>& other) -> SE3Group<Scalar> {
+             if (other.size() == 0 || transformations.size() == 0) {
+               throw std::domain_error(
+                   "Both operand should have size greater than 0");
+             }
+             SE3Group<Scalar> result;
+             if (other.size() == 1) {
+               result.reserve(transformations.size());
+               for (size_t i = 0; i < transformations.size(); ++i) {
+                 result.push_back(transformations[i] * other[0]);
+               }
+             } else if (transformations.size() == 1) {
+               result.reserve(other.size());
+               for (size_t i = 0; i < other.size(); ++i) {
+                 result.push_back(transformations[0] * other[i]);
+               }
+             } else {
+               throw std::domain_error(
+                   "Only allows transformations of size 1 to N (or N to 1) "
+                   "multiplication.");
+             }
+             return result;
+           });
+
+  type.def("__imatmul__", [](SE3Group<Scalar>& transformations,
+                             const SE3Group<Scalar>& other) {
     if (transformations.size() == 0 || other.size() == 0) {
       throw std::domain_error("Both operand should have size greater than 0");
     }
@@ -362,47 +393,52 @@ PybindSE3Type<Scalar> exportSE3Transformation(
       }
     } else {
       throw std::domain_error(
-          "Only allows transformations of size 1 to N (or N to 1) multiplication.");
+          "Only allows transformations of size 1 to N (or N to 1) "
+          "multiplication.");
     }
 
     return transformations;
   });
 
-  type.def(
-      "__matmul__",
-      [](const SE3Group<Scalar>& transformations,
-         const Eigen::Matrix<Scalar, 3, Eigen::Dynamic>& matrix)
-          -> Eigen::Matrix<Scalar, 3, Eigen::Dynamic> {
-        if (matrix.cols() == 0 || transformations.size() == 0) {
-          throw std::domain_error("Both operand should have size greater than 0");
-        }
-        if (transformations.size() != 1) {
-          throw std::domain_error("Number of transformations must be 1.");
-        }
-
-        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> result(3, matrix.cols());
-        for (size_t i = 0; i < matrix.cols(); ++i) {
-          result.col(i) = transformations[0] * matrix.col(i);
-        }
-        return result;
-      });
+  type.def("__matmul__",
+           [](const SE3Group<Scalar>& transformations,
+              const Eigen::Matrix<Scalar, 3, Eigen::Dynamic>& matrix)
+               -> Eigen::Matrix<Scalar, 3, Eigen::Dynamic> {
+             if (matrix.cols() == 0 || transformations.size() == 0) {
+               throw std::domain_error(
+                   "Both operand should have size greater than 0");
+             }
+             if (transformations.size() != 1) {
+               throw std::domain_error("Number of transformations must be 1.");
+             }
+
+             Eigen::Matrix<Scalar, 3, Eigen::Dynamic> result(3, matrix.cols());
+             for (size_t i = 0; i < matrix.cols(); ++i) {
+               result.col(i) = transformations[0] * matrix.col(i);
+             }
+             return result;
+           });
 
   type.def(
       "__getitem__",
       [](const SE3Group<Scalar>& se3Vec,
          pybind11::object index_or_slice_or_list) -> SE3Group<Scalar> {
         if (pybind11::isinstance<pybind11::slice>(index_or_slice_or_list)) {
-          pybind11::slice slice = index_or_slice_or_list.cast<pybind11::slice>();
+          pybind11::slice slice =
+              index_or_slice_or_list.cast<pybind11::slice>();
           size_t start, stop, step, slicelength;
-          if (slice.compute(se3Vec.size(), &start, &stop, &step, &slicelength)) {
+          if (slice.compute(se3Vec.size(), &start, &stop, &step,
+                            &slicelength)) {
             SE3Group<Scalar> result;
             for (size_t i = 0; i < slicelength; ++i) {
               result.push_back(se3Vec[start + i * step]);
             }
             return result;
           }
-        } else if (pybind11::isinstance<pybind11::list>(index_or_slice_or_list)) {
-          pybind11::list index_list = index_or_slice_or_list.cast<pybind11::list>();
+        } else if (pybind11::isinstance<pybind11::list>(
+                       index_or_slice_or_list)) {
+          pybind11::list index_list =
+              index_or_slice_or_list.cast<pybind11::list>();
           SE3Group<Scalar> result;
           for (const auto index : index_list) {
             const auto intIndex = pybind11::cast<int>(index);
@@ -412,7 +448,8 @@ PybindSE3Type<Scalar> exportSE3Transformation(
             result.push_back(se3Vec[intIndex]);
           }
           return result;
-        } else if (pybind11::isinstance<pybind11::int_>(index_or_slice_or_list)) {
+        } else if (pybind11::isinstance<pybind11::int_>(
+                       index_or_slice_or_list)) {
           int index = index_or_slice_or_list.cast<int>();
           if (index < 0 || index >= se3Vec.size()) {
             throw std::out_of_range("Index out of range");
@@ -422,57 +459,57 @@ PybindSE3Type<Scalar> exportSE3Transformation(
         throw pybind11::type_error("Invalid index or list or slice");
       });
   // slice version
-  type.def(
-      "__setitem__",
-      [](SE3Group<Scalar>& se3Vec,
-         pybind11::object index_or_slice_or_list,
-         const SE3Group<Scalar>& value) {
-        if (pybind11::isinstance<pybind11::slice>(index_or_slice_or_list)) {
-          pybind11::slice slice(index_or_slice_or_list);
-          size_t start, stop, step, slicelength;
-          if (slice.compute(se3Vec.size(), &start, &stop, &step, &slicelength)) {
-            if (value.size() == slicelength) {
-              for (size_t i = 0; i < slicelength; ++i) {
-                se3Vec[start + i * step] = value[i];
-              }
-            } else if (value.size() == 1) {
-              for (size_t i = 0; i < slicelength; ++i) {
-                se3Vec[start + i * step] = value[0];
-              }
-            } else {
-              throw std::out_of_range(
-                  "The value to assigned should be of size 1 or equal to the size of the slide to be assigned.");
-            }
-          } else {
-            throw std::out_of_range("The slide is invalid.");
-          }
-        } else if (pybind11::isinstance<pybind11::list>(index_or_slice_or_list)) {
-          pybind11::list list(index_or_slice_or_list);
-          if (value.size() == list.size()) {
-            for (size_t i = 0; i < list.size(); ++i) {
-              se3Vec[i] = value[i];
-            }
-          } else if (value.size() == 1) {
-            for (size_t i = 0; i < list.size(); ++i) {
-              se3Vec[i] = value[0];
-            }
-          } else {
-            throw std::out_of_range(
-                "The value to assigned should be of size 1 or equal to the size of the list to be assigned.");
-          }
-        } else if (pybind11::isinstance<pybind11::int_>(index_or_slice_or_list)) {
-          int index = index_or_slice_or_list.cast<int>();
-          if (index < 0 || index >= se3Vec.size()) {
-            throw std::out_of_range("Index out of range");
+  type.def("__setitem__", [](SE3Group<Scalar>& se3Vec,
+                             pybind11::object index_or_slice_or_list,
+                             const SE3Group<Scalar>& value) {
+    if (pybind11::isinstance<pybind11::slice>(index_or_slice_or_list)) {
+      pybind11::slice slice(index_or_slice_or_list);
+      size_t start, stop, step, slicelength;
+      if (slice.compute(se3Vec.size(), &start, &stop, &step, &slicelength)) {
+        if (value.size() == slicelength) {
+          for (size_t i = 0; i < slicelength; ++i) {
+            se3Vec[start + i * step] = value[i];
           }
-          if (value.size() != 1) {
-            throw std::out_of_range("The value to assigned should be of size 1.");
+        } else if (value.size() == 1) {
+          for (size_t i = 0; i < slicelength; ++i) {
+            se3Vec[start + i * step] = value[0];
           }
-          se3Vec[index] = value[0];
         } else {
-          throw pybind11::type_error("Invalid index or list or slice");
+          throw std::out_of_range(
+              "The value to assigned should be of size 1 or equal to the size "
+              "of the slide to be assigned.");
         }
-      });
+      } else {
+        throw std::out_of_range("The slide is invalid.");
+      }
+    } else if (pybind11::isinstance<pybind11::list>(index_or_slice_or_list)) {
+      pybind11::list list(index_or_slice_or_list);
+      if (value.size() == list.size()) {
+        for (size_t i = 0; i < list.size(); ++i) {
+          se3Vec[i] = value[i];
+        }
+      } else if (value.size() == 1) {
+        for (size_t i = 0; i < list.size(); ++i) {
+          se3Vec[i] = value[0];
+        }
+      } else {
+        throw std::out_of_range(
+            "The value to assigned should be of size 1 or equal to the size of "
+            "the list to be assigned.");
+      }
+    } else if (pybind11::isinstance<pybind11::int_>(index_or_slice_or_list)) {
+      int index = index_or_slice_or_list.cast<int>();
+      if (index < 0 || index >= se3Vec.size()) {
+        throw std::out_of_range("Index out of range");
+      }
+      if (value.size() != 1) {
+        throw std::out_of_range("The value to assigned should be of size 1.");
+      }
+      se3Vec[index] = value[0];
+    } else {
+      throw pybind11::type_error("Invalid index or list or slice");
+    }
+  });
   return type;
 }
 
@@ -482,7 +519,8 @@ void exportSE3Average(pybind11::module& module) {
   module.def(
       "iterativeMean",
       [](const SE3Group<Scalar>& transformations) -> SE3Group<Scalar> {
-        return *(Sophus::iterativeMean<SE3Group<Scalar>>(transformations, kMaxAverageIteration));
+        return *(Sophus::iterativeMean<SE3Group<Scalar>>(transformations,
+                                                         kMaxAverageIteration));
       },
       "Compute the iterative mean of a sequence.");
 }
@@ -491,7 +529,8 @@ template <typename Scalar>
 void exportSE3Interpolate(pybind11::module& module) {
   module.def(
       "interpolate",
-      [](const SE3Group<Scalar>& a, const SE3Group<Scalar>& b, double t) -> Sophus::SE3<Scalar> {
+      [](const SE3Group<Scalar>& a, const SE3Group<Scalar>& b,
+         double t) -> Sophus::SE3<Scalar> {
         if (a.size() != b.size() && a.size() != 1) {
           throw std::domain_error("Should have SE3 of size 1.");
         }
@@ -500,4 +539,4 @@ void exportSE3Interpolate(pybind11::module& module) {
       "Interpolate two SE3s of size 1.");
 }
 
-} // namespace Sophus
+}  // namespace Sophus
diff --git a/sophus_pybind/SO3PyBind.h b/sophus_pybind/SO3PyBind.h
index 2b439fc48..74f02faf0 100644
--- a/sophus_pybind/SO3PyBind.h
+++ b/sophus_pybind/SO3PyBind.h
@@ -11,10 +11,10 @@
 
 namespace Sophus {
 
-// In python, we choose to export our Sophus::SO3 as a vector of SO3 objects by binding the cpp
-// object `SO3Group` defined below. This is because numerical code in python tends to work with
-// array of values to get efficient program. This approach is inspired by
-// scipy.spatial.transform.Rotation.
+// In python, we choose to export our Sophus::SO3 as a vector of SO3 objects by
+// binding the cpp object `SO3Group` defined below. This is because numerical
+// code in python tends to work with array of values to get efficient program.
+// This approach is inspired by scipy.spatial.transform.Rotation.
 template <typename Scalar>
 class SO3Group : public std::vector<Sophus::SO3<Scalar>> {
  public:
@@ -26,15 +26,15 @@ class SO3Group : public std::vector<Sophus::SO3<Scalar>> {
     this->push_back(in);
   }
 };
-} // namespace Sophus
+}  // namespace Sophus
 
-// The following caster makes so that, even if we wrap SO3Group in python, those can be
-// implicitly converted to the c++ Sophus::SO3 object at boundaries between languages.
-// This is so we can pass python SO3 object to c++ function as if they were regular 1-element
-// Sophus::SO3 object. This simplifies binding the rest of c++ code. This implicit cast fails if the
-// python object is not a 1-element SO3 object.
-// NOTE: this caster makes a copy, so can't not be used for passing a reference of a SO3 element to
-// a c++ function.
+// The following caster makes so that, even if we wrap SO3Group in python, those
+// can be implicitly converted to the c++ Sophus::SO3 object at boundaries
+// between languages. This is so we can pass python SO3 object to c++ function
+// as if they were regular 1-element Sophus::SO3 object. This simplifies binding
+// the rest of c++ code. This implicit cast fails if the python object is not a
+// 1-element SO3 object. NOTE: this caster makes a copy, so can't not be used
+// for passing a reference of a SO3 element to a c++ function.
 namespace pybind11 {
 namespace detail {
 template <>
@@ -48,42 +48,48 @@ struct type_caster<Sophus::SO3<double>> {
       Sophus::SO3Group<double>& ref = src.cast<Sophus::SO3Group<double>&>();
       if (ref.size() != 1) {
         throw std::domain_error(fmt::format(
-            "A element of size 1 is required here. Input has {} elements.", ref.size()));
+            "A element of size 1 is required here. Input has {} elements.",
+            ref.size()));
       }
       value = ref[0];
       return true;
     } catch (const pybind11::cast_error&) {
-      return false; // Conversion failed
+      return false;  // Conversion failed
     }
   }
 
   // converting from c++ -> python type
-  static handle cast(Sophus::SO3<double> src, return_value_policy policy, handle parent) {
-    return type_caster_base<Sophus::SO3Group<double>>::cast(Sophus::SO3Group<double>(src), policy, parent);
+  static handle cast(Sophus::SO3<double> src, return_value_policy policy,
+                     handle parent) {
+    return type_caster_base<Sophus::SO3Group<double>>::cast(
+        Sophus::SO3Group<double>(src), policy, parent);
   }
 };
-} // namespace detail
-} // namespace pybind11
+}  // namespace detail
+}  // namespace pybind11
 
 namespace Sophus {
 template <typename Scalar>
 using PybindSO3Group = pybind11::class_<SO3Group<Scalar>>;
 
 template <typename Scalar>
-PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::string& name) {
+PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module,
+                                      const std::string& name) {
   PybindSO3Group<Scalar> type(module, name.c_str());
-  type.def(
-      pybind11::init([]() {
-        SO3Group<Scalar> ret;
-        ret.push_back({});
-        return ret;
-      }),
-      " Default Constructor initializing a group containing 1 identity element");
-  type.def(pybind11::init<const Sophus::SO3<Scalar>&>(), "Copy constructor from single element");
+  type.def(pybind11::init([]() {
+             SO3Group<Scalar> ret;
+             ret.push_back({});
+             return ret;
+           }),
+           " Default Constructor initializing a group containing 1 identity "
+           "element");
+  type.def(pybind11::init<const Sophus::SO3<Scalar>&>(),
+           "Copy constructor from single element");
 
   type.def_static(
       "exp",
-      [](const Eigen::Matrix<Scalar, Eigen::Dynamic, 3>& rotvecs) -> SO3Group<Scalar> {
+      [](const Eigen::Matrix<Scalar, Eigen::Dynamic, 3>& rotvecs)
+          -> SO3Group<Scalar> {
         SO3Group<Scalar> output;
         output.reserve(rotvecs.rows());
         for (size_t i = 0; i < rotvecs.rows(); ++i) {
@@ -95,7 +101,8 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
 
   type.def_static(
       "from_quat",
-      [](const Scalar& w, const Eigen::Matrix<Scalar, 3, 1>& xyz) -> SO3Group<Scalar> {
+      [](const Scalar& w,
+         const Eigen::Matrix<Scalar, 3, 1>& xyz) -> SO3Group<Scalar> {
         Eigen::Quaternion<Scalar> quat(w, xyz[0], xyz[1], xyz[2]);
         quat.normalize();
         return {Sophus::SO3<Scalar>(quat)};
@@ -109,42 +116,51 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
         if (x_vec.size() != xyz_vec.rows()) {
           throw std::runtime_error(fmt::format(
               "Size of the real and imagery part is not the same: {} {}",
-              x_vec.size(),
-              xyz_vec.rows()));
+              x_vec.size(), xyz_vec.rows()));
         }
         SO3Group<Scalar> output;
         output.reserve(x_vec.size());
         for (size_t i = 0; i < x_vec.size(); ++i) {
-          Eigen::Quaternion<Scalar> quat(x_vec[i], xyz_vec(i, 0), xyz_vec(i, 1), xyz_vec(i, 2));
+          Eigen::Quaternion<Scalar> quat(x_vec[i], xyz_vec(i, 0), xyz_vec(i, 1),
+                                         xyz_vec(i, 2));
           quat.normalize();
           output.push_back(Sophus::SO3<Scalar>(quat));
         }
         return output;
       },
-      "Create rotations from a list of quaternions as w_vec: Nx1, xyz_vec: Nx3");
+      "Create rotations from a list of quaternions as w_vec: Nx1, xyz_vec: "
+      "Nx3");
 
-  type.def_static("from_matrix", [](const Eigen::Matrix<Scalar, 3, 3>& matrix) -> SO3Group<Scalar> {
-    return Sophus::SO3<Scalar>::fitToSO3(matrix);
-  });
-  type.def_static("from_matrix", [](const pybind11::array_t<Scalar>& matrices) -> SO3Group<Scalar> {
-    if (matrices.ndim() != 3 || matrices.shape(1) != 3 || matrices.shape(2) != 3) {
-      throw std::runtime_error(
-          fmt::format("The size of the input matrix should be Nx3x3 dimensions."));
-    }
+  type.def_static(
+      "from_matrix",
+      [](const Eigen::Matrix<Scalar, 3, 3>& matrix) -> SO3Group<Scalar> {
+        return Sophus::SO3<Scalar>::fitToSO3(matrix);
+      });
+  type.def_static(
+      "from_matrix",
+      [](const pybind11::array_t<Scalar>& matrices) -> SO3Group<Scalar> {
+        if (matrices.ndim() != 3 || matrices.shape(1) != 3 ||
+            matrices.shape(2) != 3) {
+          throw std::runtime_error(fmt::format(
+              "The size of the input matrix should be Nx3x3 dimensions."));
+        }
 
-    SO3Group<Scalar> output;
-    output.reserve(matrices.shape(0));
-    for (size_t i = 0; i < matrices.shape(0); ++i) {
-      Eigen::Map<const Eigen::Matrix<Scalar, 3, 3, Eigen::RowMajor>> mat(matrices.data(i, 0, 0));
-      output.push_back(Sophus::SO3<Scalar>::fitToSO3(mat));
-    }
-    return output;
-  });
+        SO3Group<Scalar> output;
+        output.reserve(matrices.shape(0));
+        for (size_t i = 0; i < matrices.shape(0); ++i) {
+          Eigen::Map<const Eigen::Matrix<Scalar, 3, 3, Eigen::RowMajor>> mat(
+              matrices.data(i, 0, 0));
+          output.push_back(Sophus::SO3<Scalar>::fitToSO3(mat));
+        }
+        return output;
+      });
 
   type.def(
       "to_quat",
-      [](const SO3Group<Scalar>& rotations) -> Eigen::Matrix<Scalar, Eigen::Dynamic, 4> {
-        auto quaternions = Eigen::Matrix<Scalar, Eigen::Dynamic, 4>(rotations.size(), 4);
+      [](const SO3Group<Scalar>& rotations)
+          -> Eigen::Matrix<Scalar, Eigen::Dynamic, 4> {
+        auto quaternions =
+            Eigen::Matrix<Scalar, Eigen::Dynamic, 4>(rotations.size(), 4);
         for (size_t i = 0; i < rotations.size(); ++i) {
           quaternions.row(i) = Eigen::Matrix<Scalar, 1, 4>{
               rotations[i].unit_quaternion().w(),
@@ -162,7 +178,8 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
       [](const SO3Group<Scalar>& rotations) -> pybind11::array_t<Scalar> {
         pybind11::array_t<Scalar> result(
             std::vector<long>{long(rotations.size()), 3, 3},
-            std::vector<long>{9 * sizeof(Scalar), 3 * sizeof(Scalar), sizeof(Scalar)});
+            std::vector<long>{9 * sizeof(Scalar), 3 * sizeof(Scalar),
+                              sizeof(Scalar)});
 
         for (size_t i = 0; i < rotations.size(); i++) {
           Eigen::Map<Eigen::Matrix<Scalar, 3, 3, Eigen::RowMajor>> map(
@@ -175,8 +192,10 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
 
   type.def(
       "log",
-      [](const SO3Group<Scalar>& rotations) -> Eigen::Matrix<Scalar, Eigen::Dynamic, 3> {
-        auto output = Eigen::Matrix<Scalar, Eigen::Dynamic, 3>(rotations.size(), 3);
+      [](const SO3Group<Scalar>& rotations)
+          -> Eigen::Matrix<Scalar, Eigen::Dynamic, 3> {
+        auto output =
+            Eigen::Matrix<Scalar, Eigen::Dynamic, 3>(rotations.size(), 3);
         for (size_t i = 0; i < rotations.size(); ++i) {
           output.row(i) = rotations[i].log();
         }
@@ -196,23 +215,22 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
       },
       "Compute the inverse of the rotations.");
 
-  type.def("__copy__", [](const SO3Group<Scalar>& rotations) -> SO3Group<Scalar> {
-    return rotations; // copy is done with the std::vector copy constructor
-  });
+  type.def(
+      "__copy__", [](const SO3Group<Scalar>& rotations) -> SO3Group<Scalar> {
+        return rotations;  // copy is done with the std::vector copy constructor
+      });
   type.def("__str__", [](const SO3Group<Scalar>& rotations) -> std::string {
     return fmt::format("Sophus.SO3 (x{})", rotations.size());
   });
-  type.def("__len__", [](const SO3Group<Scalar>& rotations) { return rotations.size(); });
+  type.def("__len__",
+           [](const SO3Group<Scalar>& rotations) { return rotations.size(); });
   type.def("__repr__", [](const SO3Group<Scalar>& rotations) -> std::string {
     std::stringstream stream;
     stream << fmt::format("SO3 (wxyz) (x{})\n[", rotations.size());
     for (const auto& r : rotations) {
-      stream << fmt::format(
-          "[{}, {}, {}, {}],\n",
-          r.unit_quaternion().w(),
-          r.unit_quaternion().x(),
-          r.unit_quaternion().y(),
-          r.unit_quaternion().z());
+      stream << fmt::format("[{}, {}, {}, {}],\n", r.unit_quaternion().w(),
+                            r.unit_quaternion().x(), r.unit_quaternion().y(),
+                            r.unit_quaternion().z());
     }
     // replace last to previous characters
     stream.seekp(-2, stream.cur);
@@ -220,30 +238,33 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
     return stream.str();
   });
 
-  type.def(
-      "__matmul__",
-      [](const SO3Group<Scalar>& rotations, const SO3Group<Scalar>& other) -> SO3Group<Scalar> {
-        if (other.size() == 0 || rotations.size() == 0) {
-          throw std::domain_error("Both operand should have size greater than 0");
-        }
-        SO3Group<Scalar> result;
-        if (other.size() == 1) {
-          result.reserve(rotations.size());
-          for (size_t i = 0; i < rotations.size(); ++i) {
-            result.push_back(rotations[i] * other[0]);
-          }
-        } else if (rotations.size() == 1) {
-          result.reserve(other.size());
-          for (size_t i = 0; i < other.size(); ++i) {
-            result.push_back(rotations[0] * other[i]);
-          }
-        } else {
-          throw std::domain_error(
-              "Only allows rotations of size 1 to N (or N to 1) multiplication.");
-        }
-        return result;
-      });
-  type.def("__imatmul__", [](SO3Group<Scalar>& rotations, const SO3Group<Scalar>& other) {
+  type.def("__matmul__",
+           [](const SO3Group<Scalar>& rotations,
+              const SO3Group<Scalar>& other) -> SO3Group<Scalar> {
+             if (other.size() == 0 || rotations.size() == 0) {
+               throw std::domain_error(
+                   "Both operand should have size greater than 0");
+             }
+             SO3Group<Scalar> result;
+             if (other.size() == 1) {
+               result.reserve(rotations.size());
+               for (size_t i = 0; i < rotations.size(); ++i) {
+                 result.push_back(rotations[i] * other[0]);
+               }
+             } else if (rotations.size() == 1) {
+               result.reserve(other.size());
+               for (size_t i = 0; i < other.size(); ++i) {
+                 result.push_back(rotations[0] * other[i]);
+               }
+             } else {
+               throw std::domain_error(
+                   "Only allows rotations of size 1 to N (or N to 1) "
+                   "multiplication.");
+             }
+             return result;
+           });
+  type.def("__imatmul__", [](SO3Group<Scalar>& rotations,
+                             const SO3Group<Scalar>& other) {
     if (rotations.size() == 0 || other.size() == 0) {
       throw std::domain_error("Both operand should have size greater than 0");
     }
@@ -257,46 +278,52 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
         rotations[i] = rotations[i] * other[0];
       }
     } else {
-      throw std::domain_error("Only allows rotations of size 1 to N (or N to 1) multiplication.");
+      throw std::domain_error(
+          "Only allows rotations of size 1 to N (or N to 1) multiplication.");
     }
 
     return rotations;
   });
 
-  type.def(
-      "__matmul__",
-      [](const SO3Group<Scalar>& rotations, const Eigen::Matrix<Scalar, 3, Eigen::Dynamic>& matrix)
-          -> Eigen::Matrix<Scalar, 3, Eigen::Dynamic> {
-        if (matrix.cols() == 0 || rotations.size() == 0) {
-          throw std::domain_error("Both operand should have size greater than 0");
-        }
-        if (rotations.size() != 1) {
-          throw std::domain_error("Number of rotations must be 1.");
-        }
+  type.def("__matmul__",
+           [](const SO3Group<Scalar>& rotations,
+              const Eigen::Matrix<Scalar, 3, Eigen::Dynamic>& matrix)
+               -> Eigen::Matrix<Scalar, 3, Eigen::Dynamic> {
+             if (matrix.cols() == 0 || rotations.size() == 0) {
+               throw std::domain_error(
+                   "Both operand should have size greater than 0");
+             }
+             if (rotations.size() != 1) {
+               throw std::domain_error("Number of rotations must be 1.");
+             }
 
-        Eigen::Matrix<Scalar, 3, Eigen::Dynamic> result(3, matrix.cols());
-        for (size_t i = 0; i < matrix.cols(); ++i) {
-          result.col(i) = rotations[0] * matrix.col(i);
-        }
-        return result;
-      });
+             Eigen::Matrix<Scalar, 3, Eigen::Dynamic> result(3, matrix.cols());
+             for (size_t i = 0; i < matrix.cols(); ++i) {
+               result.col(i) = rotations[0] * matrix.col(i);
+             }
+             return result;
+           });
 
   type.def(
       "__getitem__",
       [](const SO3Group<Scalar>& so3Vec,
          pybind11::object index_or_slice_or_list) -> SO3Group<Scalar> {
         if (pybind11::isinstance<pybind11::slice>(index_or_slice_or_list)) {
-          pybind11::slice slice = index_or_slice_or_list.cast<pybind11::slice>();
+          pybind11::slice slice =
+              index_or_slice_or_list.cast<pybind11::slice>();
           size_t start, stop, step, slicelength;
-          if (slice.compute(so3Vec.size(), &start, &stop, &step, &slicelength)) {
+          if (slice.compute(so3Vec.size(), &start, &stop, &step,
+                            &slicelength)) {
             SO3Group<Scalar> result;
             for (size_t i = 0; i < slicelength; ++i) {
               result.push_back(so3Vec[start + i * step]);
             }
             return result;
           }
-        } else if (pybind11::isinstance<pybind11::list>(index_or_slice_or_list)) {
-          pybind11::list index_list = index_or_slice_or_list.cast<pybind11::list>();
+        } else if (pybind11::isinstance<pybind11::list>(
+                       index_or_slice_or_list)) {
+          pybind11::list index_list =
+              index_or_slice_or_list.cast<pybind11::list>();
           SO3Group<Scalar> result;
           for (const auto index : index_list) {
             const auto intIndex = pybind11::cast<int>(index);
@@ -306,7 +333,8 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
             result.push_back(so3Vec[intIndex]);
           }
           return result;
-        } else if (pybind11::isinstance<pybind11::int_>(index_or_slice_or_list)) {
+        } else if (pybind11::isinstance<pybind11::int_>(
+                       index_or_slice_or_list)) {
           int index = index_or_slice_or_list.cast<int>();
           if (index < 0 || index >= so3Vec.size()) {
             throw std::out_of_range("Index out of range");
@@ -316,59 +344,60 @@ PybindSO3Group<Scalar> exportSO3Group(pybind11::module& module, const std::strin
         throw pybind11::type_error("Invalid index or list or slice");
       });
   // slice version
-  type.def(
-      "__setitem__",
-      [](SO3Group<Scalar>& so3Vec,
-         pybind11::object index_or_slice_or_list,
-         const SO3Group<Scalar>& value) {
-        if (pybind11::isinstance<pybind11::slice>(index_or_slice_or_list)) {
-          pybind11::slice slice(index_or_slice_or_list);
-          size_t start, stop, step, slicelength;
-          if (slice.compute(so3Vec.size(), &start, &stop, &step, &slicelength)) {
-            if (value.size() == slicelength) {
-              for (size_t i = 0; i < slicelength; ++i) {
-                so3Vec[start + i * step] = value[i];
-              }
-            } else if (value.size() == 1) {
-              for (size_t i = 0; i < slicelength; ++i) {
-                so3Vec[start + i * step] = value[0];
-              }
-            } else {
-              throw std::out_of_range(
-                  "The value to assigned should be of size 1 or equal to the size of the slide to be assigned.");
-            }
-          } else {
-            throw std::out_of_range("The slide is invalid.");
+  type.def("__setitem__", [](SO3Group<Scalar>& so3Vec,
+                             pybind11::object index_or_slice_or_list,
+                             const SO3Group<Scalar>& value) {
+    if (pybind11::isinstance<pybind11::slice>(index_or_slice_or_list)) {
+      pybind11::slice slice(index_or_slice_or_list);
+      size_t start, stop, step, slicelength;
+      if (slice.compute(so3Vec.size(), &start, &stop, &step, &slicelength)) {
+        if (value.size() == slicelength) {
+          for (size_t i = 0; i < slicelength; ++i) {
+            so3Vec[start + i * step] = value[i];
           }
-        } else if (pybind11::isinstance<pybind11::list>(index_or_slice_or_list)) {
-          pybind11::list list(index_or_slice_or_list);
-          if (value.size() == list.size()) {
-            for (size_t i = 0; i < list.size(); ++i) {
-              so3Vec[i] = value[i];
-            }
-          } else if (value.size() == 1) {
-            for (size_t i = 0; i < list.size(); ++i) {
-              so3Vec[i] = value[0];
-            }
-          } else {
-            throw std::out_of_range(
-                "The value to assigned should be of size 1 or equal to the size of the list to be assigned.");
+        } else if (value.size() == 1) {
+          for (size_t i = 0; i < slicelength; ++i) {
+            so3Vec[start + i * step] = value[0];
           }
-        } else if (pybind11::isinstance<pybind11::int_>(index_or_slice_or_list)) {
-          int index = index_or_slice_or_list.cast<int>();
-          if (index < 0 || index >= so3Vec.size()) {
-            throw std::out_of_range("Index out of range");
-          }
-          if (value.size() != 1) {
-            throw std::out_of_range(
-                "The value to assigned should be of size 1 or equal to the size of the slide to be assigned.");
-          }
-          so3Vec[index] = value[0];
         } else {
-          throw pybind11::type_error("Invalid index or list or slice");
+          throw std::out_of_range(
+              "The value to assigned should be of size 1 or equal to the size "
+              "of the slide to be assigned.");
         }
-      });
+      } else {
+        throw std::out_of_range("The slide is invalid.");
+      }
+    } else if (pybind11::isinstance<pybind11::list>(index_or_slice_or_list)) {
+      pybind11::list list(index_or_slice_or_list);
+      if (value.size() == list.size()) {
+        for (size_t i = 0; i < list.size(); ++i) {
+          so3Vec[i] = value[i];
+        }
+      } else if (value.size() == 1) {
+        for (size_t i = 0; i < list.size(); ++i) {
+          so3Vec[i] = value[0];
+        }
+      } else {
+        throw std::out_of_range(
+            "The value to assigned should be of size 1 or equal to the size of "
+            "the list to be assigned.");
+      }
+    } else if (pybind11::isinstance<pybind11::int_>(index_or_slice_or_list)) {
+      int index = index_or_slice_or_list.cast<int>();
+      if (index < 0 || index >= so3Vec.size()) {
+        throw std::out_of_range("Index out of range");
+      }
+      if (value.size() != 1) {
+        throw std::out_of_range(
+            "The value to assigned should be of size 1 or equal to the size of "
+            "the slide to be assigned.");
+      }
+      so3Vec[index] = value[0];
+    } else {
+      throw pybind11::type_error("Invalid index or list or slice");
+    }
+  });
   return type;
 }
 
-} // namespace Sophus
+}  // namespace Sophus
diff --git a/sophus_pybind/SophusPyBind.h b/sophus_pybind/SophusPyBind.h
index 930da07f0..4e39644bb 100644
--- a/sophus_pybind/SophusPyBind.h
+++ b/sophus_pybind/SophusPyBind.h
@@ -4,8 +4,8 @@
 #include "SO3PyBind.h"
 
 #include <sstream>
-// By default, Sophus calls std::abort when a pre-condition fails. Register a handler that raises
-// an exception so we don't crash the Python process.
+// By default, Sophus calls std::abort when a pre-condition fails. Register a
+// handler that raises an exception so we don't crash the Python process.
 #ifdef SOPHUS_DISABLE_ENSURES
 #undef SOPHUS_DISABLE_ENSURES
 #endif
@@ -14,11 +14,12 @@
 #endif
 
 namespace Sophus {
-inline void
-ensureFailed(char const* function, char const* file, int line, char const* description) {
+inline void ensureFailed(char const* function, char const* file, int line,
+                         char const* description) {
   std::stringstream message;
-  message << "'SOPHUS_ENSURE' failed in function '" << function << "', on line '" << line
-          << "' of file '" << file << "'. Full description:" << std::endl
+  message << "'SOPHUS_ENSURE' failed in function '" << function
+          << "', on line '" << line << "' of file '" << file
+          << "'. Full description:" << std::endl
           << description;
   throw std::domain_error(message.str());
 }
@@ -31,4 +32,4 @@ inline void exportSophus(pybind11::module& module) {
   exportSE3Interpolate<double>(module);
 }
 
-} // namespace Sophus
+}  // namespace Sophus
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 6c869ff63..520b3bcce 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,2 +1,2 @@
-ADD_SUBDIRECTORY(core)
-ADD_SUBDIRECTORY(ceres)
+add_subdirectory(core)
+add_subdirectory(ceres)
diff --git a/test/core/CMakeLists.txt b/test/core/CMakeLists.txt
index b982736ec..d4bbde1c9 100644
--- a/test/core/CMakeLists.txt
+++ b/test/core/CMakeLists.txt
@@ -1,24 +1,26 @@
-#Tests to run
-SET( TEST_SOURCES test_common
-                  test_cartesian2
-                  test_cartesian3
-                  test_so2
-                  test_se2
-                  test_rxso2
-                  test_sim2
-                  test_so3
-                  test_se3
-                  test_rxso3
-                  test_sim3
-                  test_geometry)
-find_package( Ceres 2 )
+# Tests to run
+set(TEST_SOURCES
+    test_common
+    test_cartesian2
+    test_cartesian3
+    test_so2
+    test_se2
+    test_rxso2
+    test_sim2
+    test_so3
+    test_se3
+    test_rxso3
+    test_sim3
+    test_geometry)
+find_package(Ceres 2)
 
-FOREACH(test_src ${TEST_SOURCES})
-  ADD_EXECUTABLE( ${test_src} ${test_src}.cpp tests.hpp ../../sophus/test_macros.hpp)
-  TARGET_LINK_LIBRARIES( ${test_src} sophus)
-  if( Ceres_FOUND )
-      TARGET_LINK_LIBRARIES( ${test_src} Ceres::ceres )
-      ADD_DEFINITIONS(-DSOPHUS_CERES)
+foreach(test_src ${TEST_SOURCES})
+  add_executable(${test_src} ${test_src}.cpp tests.hpp
+                             ../../sophus/test_macros.hpp)
+  target_link_libraries(${test_src} sophus)
+  if(Ceres_FOUND)
+    target_link_libraries(${test_src} Ceres::ceres)
+    add_definitions(-DSOPHUS_CERES)
   endif(Ceres_FOUND)
-  ADD_TEST( ${test_src} ${test_src} )
-ENDFOREACH(test_src)
+  add_test(${test_src} ${test_src})
+endforeach(test_src)