├── doc ├── OptimizingRotations.pdf ├── OptimizingRotations.bib └── OptimizingRotations.tex ├── .gitignore ├── README.md ├── cmake ├── target_link_libraries_system.cmake └── Warnings.cmake ├── rotation_optimization.hh ├── CMakeLists.txt ├── unit_tests.cc └── rotation_optimization.inl /doc/OptimizingRotations.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EPFL-LGG/RotationOptimization/HEAD/doc/OptimizingRotations.pdf -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 3rdparty 2 | build 3 | build_* 4 | results 5 | TODO 6 | *.aux 7 | *.bbl 8 | *.blg 9 | *.fdb_latexmk 10 | *.fls 11 | *.log 12 | *.out 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Optimization over SO(3) 2 | Gradients and Hessians needed to optimize over the space of rotations, 3 | parametrized using the tangent space to SO(3)/the exponential map. 4 | See `doc/OptimizingRotations.pdf` for more explanation and the derivations. 5 | 6 | This code provides the gradients and Hessians for rotation matrices and rotated 7 | vectors/matrices with respect to the variables parametrizing the rotation. 8 | -------------------------------------------------------------------------------- /doc/OptimizingRotations.bib: -------------------------------------------------------------------------------- 1 | @inproceedings{kugelstadt2018fast, 2 | title={Fast Corotated FEM using Operator Splitting}, 3 | author={Kugelstadt, T and Koschier, D and Bender, J}, 4 | booktitle={Computer Graphics Forum}, 5 | volume={37}, 6 | number={8}, 7 | pages={149--160}, 8 | year={2018}, 9 | organization={Wiley Online Library} 10 | } 11 | 12 | @article{taylor1994minimization, 13 | title={Minimization on the Lie group SO (3) and related manifolds}, 14 | author={Taylor, Camillo J and Kriegman, David J}, 15 | journal={Yale University}, 16 | volume={16}, 17 | pages={155}, 18 | year={1994} 19 | } 20 | 21 | @article{grassia1998practical, 22 | title={Practical parameterization of rotations using the exponential map}, 23 | author={Grassia, F Sebastian}, 24 | journal={Journal of graphics tools}, 25 | volume={3}, 26 | number={3}, 27 | pages={29--48}, 28 | year={1998}, 29 | publisher={Taylor \& Francis} 30 | } 31 | -------------------------------------------------------------------------------- /cmake/target_link_libraries_system.cmake: -------------------------------------------------------------------------------- 1 | # From https://stackoverflow.com/a/52136398/122710 2 | function(target_link_libraries_system target) 3 | set(options PRIVATE PUBLIC INTERFACE) 4 | cmake_parse_arguments(TLLS "${options}" "" "" ${ARGN}) 5 | foreach(op ${options}) 6 | if(TLLS_${op}) 7 | set(scope ${op}) 8 | endif() 9 | endforeach(op) 10 | set(libs ${TLLS_UNPARSED_ARGUMENTS}) 11 | 12 | foreach(lib ${libs}) 13 | get_target_property(lib_include_dirs ${lib} INTERFACE_INCLUDE_DIRECTORIES) 14 | if(lib_include_dirs) 15 | if(scope) 16 | target_include_directories(${target} SYSTEM ${scope} ${lib_include_dirs}) 17 | else() 18 | target_include_directories(${target} SYSTEM PRIVATE ${lib_include_dirs}) 19 | endif() 20 | else() 21 | message("Warning: ${lib} doesn't set INTERFACE_INCLUDE_DIRECTORIES. No include_directories set.") 22 | endif() 23 | if(scope) 24 | target_link_libraries(${target} ${scope} ${lib}) 25 | else() 26 | target_link_libraries(${target} ${lib}) 27 | endif() 28 | endforeach() 29 | endfunction(target_link_libraries_system) 30 | -------------------------------------------------------------------------------- /rotation_optimization.hh: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // rotation_optimization.hh 3 | //////////////////////////////////////////////////////////////////////////////// 4 | /*! @file 5 | // Useful functions for optimizing over the space of rotations: 6 | // We use the tangent space to SO(3) at a reference rotation as the optimization 7 | // domain and provide functions to apply the represented rotation to a given 8 | // vector and compute gradients/Hessians of the rotation matrix/rotated vector. 9 | */ 10 | // Author: Julian Panetta (jpanetta), julian.panetta@gmail.com 11 | // Created: 11/01/2018 11:38:57 12 | //////////////////////////////////////////////////////////////////////////////// 13 | #ifndef ROTATION_OPTIMIZATION_HH 14 | #define ROTATION_OPTIMIZATION_HH 15 | #include 16 | #include 17 | #include 18 | 19 | template 20 | struct rotation_optimization { 21 | using Vec3 = Eigen::Matrix; 22 | using Mat3 = Eigen::Matrix; 23 | 24 | //////////////////////////////////////////////////////////////////////////////// 25 | // Rotation matrix, rotated vectors, and rotated matrices 26 | //////////////////////////////////////////////////////////////////////////////// 27 | // Compute R(w) 28 | static Mat3 rotation_matrix(const Vec3 &w); 29 | 30 | // Compute R(w) v 31 | static Vec3 rotated_vector(const Vec3 &w, const Vec3 &v); 32 | 33 | // Compute R(w) A 34 | template 35 | static Eigen::Matrix rotated_matrix(const Vec3 &w, const Eigen::Matrix &A); 36 | 37 | //////////////////////////////////////////////////////////////////////////////// 38 | // Gradient of rotation matrix, rotated vectors, and rotated matrices 39 | //////////////////////////////////////////////////////////////////////////////// 40 | // Gradient of R(w). This is a third order tensor: 41 | // g_ijk = D [R(w)]_ij / dw_k 42 | static Eigen::Tensor grad_rotation_matrix(const Vec3 &w); 43 | 44 | // Gradient of R(w) v. This is a second order tensor, returned as a 3x3 matrix: 45 | // g_ij = D [R(w) v]_i / dw_j 46 | static Mat3 grad_rotated_vector(const Vec3 &w, const Vec3 &v); 47 | 48 | // Gradient of R(w) A. This is a third order tensor: 49 | // g_ijk = D [R(w) A]_ij / dw_k 50 | template 51 | static Eigen::Tensor grad_rotated_matrix(const Vec3 &w, const Eigen::Matrix &A); 52 | 53 | //////////////////////////////////////////////////////////////////////////////// 54 | // Hessian of rotation matrix, rotated vectors, and rotated matrices 55 | //////////////////////////////////////////////////////////////////////////////// 56 | // The Hessian of R(w). this is a fourth order tensor: 57 | // H_ijkl = d [R(w)]_ij / (dw_k dw_l) 58 | static Eigen::Tensor hess_rotation_matrix(const Vec3 &w); 59 | 60 | // Hessian of R(w) v. This is a third order tensor: 61 | // H_ijk = d [R(w) v]_i / (dw_j dw_k) 62 | // We output the i^th slice of this tensor (the Hessian of rotated vector 63 | // component i) in hess_comp[i]. 64 | static void hess_rotated_vector(const Vec3 &w, const Vec3 &v, std::array, 3> hess_comp); 65 | 66 | // Hessian of R(w) v. This is a third order tensor: 67 | // H_ijk = d [R(w) v]_i / (dw_j dw_k) 68 | // We output the i^th slice of this tensor (the Hessian of rotated vector 69 | // component i) in hess_comp[i]. 70 | static void hess_rotated_vector(const Vec3 &w, const Vec3 &v, std::array &hess_comp) { 71 | std::array, 3> hc_refs{{hess_comp[0], hess_comp[1], hess_comp[2]}}; 72 | hess_rotated_vector(w, v, hc_refs); 73 | } 74 | 75 | // d_i H_ijk where H_ijk is the Hessian of R(w) v. 76 | static Mat3 d_contract_hess_rotated_vector(const Vec3 &w, const Vec3 &v, const Vec3 &d); 77 | 78 | // The Hessian of R(w) A for 3xN matrix A; this is a fourth order tensor: 79 | // H_ijkl = d [R(w) A]_ij / (dw_k dw_l) 80 | template 81 | static Eigen::Tensor hess_rotated_matrix(const Vec3 &w, const Eigen::Matrix &A); 82 | 83 | //////////////////////////////////////////////////////////////////////////////// 84 | // Helper functions 85 | //////////////////////////////////////////////////////////////////////////////// 86 | static Mat3 cross_product_matrix(const Vec3 &v); 87 | }; 88 | 89 | #include "rotation_optimization.inl" 90 | 91 | #endif /* end of include guard: ROTATION_OPTIMIZATION_HH */ 92 | -------------------------------------------------------------------------------- /cmake/Warnings.cmake: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | cmake_minimum_required(VERSION 3.1) 3 | ################################################################################ 4 | # See comments and discussions here: 5 | # http://stackoverflow.com/questions/5088460/flags-to-enable-thorough-and-verbose-g-warnings 6 | ################################################################################ 7 | 8 | if(TARGET warnings::all) 9 | return() 10 | endif() 11 | 12 | set(MY_FLAGS 13 | -Wall 14 | -Wextra 15 | -pedantic 16 | 17 | # -Wconversion 18 | #-Wunsafe-loop-optimizations # broken with C++11 loops 19 | -Wunused 20 | 21 | -Wno-long-long 22 | -Wpointer-arith 23 | -Wformat=2 24 | -Wuninitialized 25 | -Wcast-qual 26 | -Wmissing-noreturn 27 | -Wmissing-format-attribute 28 | -Wredundant-decls 29 | 30 | #-Werror=implicit # not valid for C++ 31 | -Werror=nonnull 32 | -Werror=init-self 33 | -Werror=main 34 | -Werror=missing-braces 35 | -Werror=sequence-point 36 | -Werror=return-type 37 | -Werror=trigraphs 38 | -Werror=array-bounds 39 | -Werror=write-strings 40 | -Werror=address 41 | -Werror=int-to-pointer-cast 42 | #-Werror=pointer-to-int-cast # not valid for C++ 43 | 44 | -Wno-unused-variable 45 | -Wunused-but-set-variable 46 | -Wno-unused-parameter 47 | 48 | #-Weffc++ 49 | -Wno-old-style-cast 50 | # -Wno-sign-conversion 51 | #-Wsign-conversion 52 | 53 | -Wshadow 54 | 55 | -Wstrict-null-sentinel 56 | -Woverloaded-virtual 57 | -Wsign-promo 58 | -Wstack-protector 59 | -Wstrict-aliasing 60 | -Wstrict-aliasing=2 61 | -Wswitch-default 62 | -Wswitch-enum 63 | -Wswitch-unreachable 64 | 65 | -Wcast-align 66 | -Wdisabled-optimization 67 | #-Winline # produces warning on default implicit destructor 68 | -Winvalid-pch 69 | # -Wmissing-include-dirs 70 | -Wpacked 71 | -Wno-padded 72 | -Wstrict-overflow 73 | -Wstrict-overflow=2 74 | 75 | -Wctor-dtor-privacy 76 | -Wlogical-op 77 | -Wnoexcept 78 | -Woverloaded-virtual 79 | # -Wundef 80 | 81 | -Wnon-virtual-dtor 82 | -Wdelete-non-virtual-dtor 83 | -Werror=non-virtual-dtor 84 | -Werror=delete-non-virtual-dtor 85 | 86 | -wno-sign-compare 87 | 88 | ########### 89 | # GCC 6.1 # 90 | ########### 91 | 92 | -Wnull-dereference 93 | -fdelete-null-pointer-checks 94 | -Wduplicated-cond 95 | -Wmisleading-indentation 96 | 97 | #-Weverything 98 | 99 | ########################### 100 | # Enabled by -Weverything # 101 | ########################### 102 | 103 | #-Wdocumentation 104 | #-Wdocumentation-unknown-command 105 | #-Wfloat-equal 106 | #-Wcovered-switch-default 107 | 108 | #-Wglobal-constructors 109 | #-Wexit-time-destructors 110 | #-Wmissing-variable-declarations 111 | #-Wextra-semi 112 | #-Wweak-vtables 113 | #-Wno-source-uses-openmp 114 | #-Wdeprecated 115 | #-Wnewline-eof 116 | #-Wmissing-prototypes 117 | 118 | #-Wno-c++98-compat 119 | #-Wno-c++98-compat-pedantic 120 | 121 | ########################### 122 | # Need to check if those are still valid today 123 | ########################### 124 | 125 | #-Wimplicit-atomic-properties 126 | #-Wmissing-declarations 127 | #-Wmissing-prototypes 128 | #-Wstrict-selector-match 129 | #-Wundeclared-selector 130 | #-Wunreachable-code 131 | 132 | # Not a warning, but enable link-time-optimization 133 | # TODO: Check out modern CMake version of setting this flag 134 | # https://cmake.org/cmake/help/latest/module/CheckIPOSupported.html 135 | #-flto 136 | 137 | # Gives meaningful stack traces 138 | -fno-omit-frame-pointer 139 | -fno-optimize-sibling-calls 140 | ) 141 | 142 | # Flags above don't make sense for MSVC 143 | if(MSVC) 144 | set(MY_FLAGS) 145 | endif() 146 | 147 | include(CheckCXXCompilerFlag) 148 | 149 | add_library(warnings_all INTERFACE) 150 | add_library(warnings::all ALIAS warnings_all) 151 | 152 | foreach(FLAG IN ITEMS ${MY_FLAGS}) 153 | string(REPLACE "=" "-" FLAG_VAR "${FLAG}") 154 | if(NOT DEFINED IS_SUPPORTED_${FLAG_VAR}) 155 | check_cxx_compiler_flag("${FLAG}" IS_SUPPORTED_${FLAG_VAR}) 156 | endif() 157 | if(IS_SUPPORTED_${FLAG_VAR}) 158 | target_compile_options(warnings_all INTERFACE ${FLAG}) 159 | endif() 160 | endforeach() 161 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # General Informations 3 | ################################################################################ 4 | 5 | cmake_minimum_required(VERSION 3.14) 6 | project(rotation_optimization) 7 | 8 | # CMP0063: Honor visibility properties for all target types. 9 | if (POLICY CMP0063) 10 | cmake_policy(SET CMP0063 NEW) 11 | endif() 12 | 13 | set(CMAKE_MACOSX_RPATH 1) 14 | 15 | ################################################################################ 16 | 17 | set(THIRD_PARTY_DIR ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/) 18 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 19 | 20 | set(CMAKE_CXX_FLAGS_RELWITHASSERT "-O3" CACHE STRING "Flags used during RelWithAssertions builds" FORCE) 21 | set(CMAKE_C_FLAGS_RELWITHASSERT "-O3" CACHE STRING "Flags used during RelWithAssertions builds" FORCE) 22 | set(CMAKE_EXE_LINKER_FLAGS_RELWITHASSERT "" CACHE STRING "Flags used during RelWithAssertions builds" FORCE) 23 | set(CMAKE_SHARED_LINKER_FLAGS_RELWITHASSERT "" CACHE STRING "Flags used during RelWithAssertions builds" FORCE) 24 | set(CMAKE_MODULE_LINKER_FLAGS_RELWITHASSERT "" CACHE STRING "Flags used during RelWithAssertions builds" FORCE) 25 | set(CMAKE_STATIC_LINKER_FLAGS_RELWITHASSERT "" CACHE STRING "Flags used during RelWithAssertions builds" FORCE) 26 | mark_as_advanced( 27 | CMAKE_CXX_FLAGS_RELWITHASSERT 28 | CMAKE_C_FLAGS_RELWITHASSERT 29 | CMAKE_EXE_LINKER_FLAGS_RELWITHASSERT 30 | CMAKE_SHARED_LINKER_FLAGS_RELWITHASSERT 31 | CMAKE_MODULE_LINKER_FLAGS_RELWITHASSERT 32 | CMAKE_STATIC_LINKER_FLAGS_RELWITHASSERT 33 | ) 34 | 35 | if(NOT CMAKE_BUILD_TYPE) 36 | message(STATUS "No build type selected, default to RelWithAssert") 37 | set(CMAKE_BUILD_TYPE "RelWithAssert") 38 | endif() 39 | 40 | set(CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" CACHE STRING "Choose the type of build. Options are: None Debug Release RelWithDebInfo MinSizeRel RelWithAssert" FORCE) 41 | 42 | # Enable more warnings 43 | include(Warnings) 44 | 45 | # Make sure warnings/errors are still colorized when using Ninja for building. 46 | if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "Intel") 47 | add_definitions(-fdiagnostics-color=always) 48 | endif() 49 | 50 | # Export compile flags(used for autocompletion of the C++ code) 51 | set(CMAKE_EXPORT_COMPILE_COMMANDS 1) 52 | 53 | # CMake plugin for vscode 54 | include(CMakeToolsHelpers OPTIONAL) 55 | 56 | # Enable more warnings 57 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -pedantic -Wno-comment") 58 | 59 | # We need -fPIC when compiling our libraries and our dependencies for 60 | # the python bindings to link. 61 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 62 | 63 | # Work-around issue where include directories of imported targets are not 64 | # interpreted as SYSTEM contrary to the claims here: 65 | # https://discourse.cmake.org/t/declare-imported-targets-as-system-to-suppress-warnings/555 66 | include(target_link_libraries_system) 67 | 68 | ################################################################################ 69 | # Common libraries 70 | ################################################################################ 71 | include(FetchContent) 72 | set(ROTOPT_ROOT "${CMAKE_CURRENT_LIST_DIR}") 73 | set(ROTOPT_EXTERNAL "${ROTOPT_ROOT}/3rdparty") 74 | 75 | function(rotopt_download_project name) 76 | FetchContent_Declare(${name} 77 | SOURCE_DIR ${ROTOPT_EXTERNAL}/${name} 78 | DOWNLOAD_DIR ${ROTOPT_EXTERNAL}/.cache/${name} 79 | QUIET 80 | ${ARGN} 81 | ) 82 | set(OLD_CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE}) 83 | set(CMAKE_BUILD_TYPE Release) 84 | FetchContent_MakeAvailable(${name}) 85 | set(CMAKE_BUILD_TYPE ${OLD_CMAKE_BUILD_TYPE}) 86 | endfunction() 87 | 88 | 89 | # Eigen3 library 90 | if(NOT TARGET Eigen3::Eigen) 91 | rotopt_download_project(Eigen 92 | URL https://gitlab.com/libeigen/eigen/-/archive/3.4-rc1/eigen-3.4-rc1.tar.gz 93 | URL_MD5 0839b9721e65d2328fb96eb4290d74cc 94 | ) 95 | endif() 96 | 97 | ################################################################################ 98 | # Rotation optimization library 99 | ################################################################################ 100 | add_library(rotation_optimization INTERFACE) 101 | target_link_libraries_system(rotation_optimization INTERFACE Eigen3::Eigen) 102 | target_include_directories(rotation_optimization INTERFACE .) 103 | 104 | ################################################################################ 105 | # Unit tests 106 | ################################################################################ 107 | if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) 108 | # Catch2 109 | if(NOT TARGET Catch2::Catch2) 110 | rotopt_download_project(Catch2 111 | URL https://github.com/catchorg/Catch2/archive/v2.13.3.tar.gz 112 | URL_MD5 57612324e1e0b49dfc8eab68c03f8473 113 | ) 114 | endif() 115 | 116 | add_executable(unit_tests unit_tests.cc) 117 | set_target_properties(unit_tests PROPERTIES CXX_STANDARD 14) 118 | 119 | target_link_libraries(unit_tests PUBLIC 120 | Catch2::Catch2 121 | rotation_optimization 122 | warnings::all 123 | ) 124 | 125 | target_compile_options(unit_tests PRIVATE -Wno-unused-parameter) 126 | endif() 127 | -------------------------------------------------------------------------------- /doc/OptimizingRotations.tex: -------------------------------------------------------------------------------- 1 | \documentclass[10pt]{article} 2 | 3 | \usepackage[latin1]{inputenc} 4 | \usepackage{amsmath, amssymb, amsfonts, amsthm} 5 | \usepackage{upgreek} 6 | \usepackage{amsthm} 7 | \usepackage{fullpage} 8 | \usepackage{graphicx} 9 | \usepackage{cancel} 10 | \usepackage{subfigure} 11 | \usepackage{mathrsfs} 12 | \usepackage{outlines} 13 | \usepackage[font={sf,it}, labelfont={sf,bf}, labelsep=space, belowskip=5pt]{caption} 14 | \usepackage{hyperref} 15 | % \usepackage{minted} 16 | \usepackage{titling} 17 | \usepackage{xifthen} 18 | \usepackage{color} 19 | 20 | \usepackage{fancyhdr} 21 | \usepackage[title]{appendix} 22 | \usepackage{float} 23 | 24 | \usepackage{bm} 25 | 26 | \newcommand{\documenttitle}{Optimizing over SO(3)} 27 | 28 | \DeclareMathOperator{\tr}{tr} 29 | \DeclareMathOperator{\sgn}{sgn} 30 | \DeclareMathOperator{\sinc}{sinc} 31 | \DeclareMathOperator{\rref}{rref} 32 | \DeclareMathOperator{\cof}{cof} 33 | \DeclareMathOperator*{\sym}{sym} 34 | 35 | \DeclareMathOperator{\diag}{diag} 36 | \DeclareMathOperator*{\argmax}{argmax} 37 | \DeclareMathOperator*{\argmin}{argmin} 38 | \newcommand{\defeq}{\vcentcolon=} 39 | \renewcommand{\Re}{\operatorname{Re}} \renewcommand{\Im}{\operatorname{Im}} 40 | \allowdisplaybreaks 41 | 42 | \pagestyle{fancy} 43 | \headheight 24pt 44 | \headsep 12pt 45 | \lhead{\documenttitle} 46 | \rhead{\today} 47 | \fancyfoot[C]{} % hide the default page number at the bottom 48 | \lfoot{} 49 | \rfoot{\thepage} 50 | \renewcommand{\headrulewidth}{0.4pt} 51 | \renewcommand\footrulewidth{0.4pt} 52 | \providecommand{\abs}[1]{\lvert#1\rvert} 53 | \providecommand{\norm}[1]{\lVert#1\rVert} 54 | \providecommand{\normlr}[1]{\left\lVert#1\right\rVert} 55 | \providecommand{\dx}{\, \mathrm{d}x} 56 | \providecommand{\ds}{\, \mathrm{d}s} 57 | \providecommand{\lint}[3]{\int_{#1}^{#2} \! #3 \, \ds} 58 | % \providecommand{\vint}[2]{\int_{#1} \! #2 \, \mathrm{d}x} 59 | % \providecommand{\sint}[2]{\int_{\partial #1} \! #2 \, \mathrm{d}A} 60 | \renewcommand{\div}{\nabla \cdot} 61 | \providecommand{\cross}{\times} 62 | \providecommand{\curl}{\nabla \cross} 63 | \providecommand{\grad}{\nabla} 64 | \providecommand{\laplacian}{\bigtriangleup} 65 | \providecommand{\shape}{\Omega} 66 | \providecommand{\mesh}{\mathcal{M}} 67 | \providecommand{\boundary}{\partial \shape} 68 | \def\d{\mathrm{d}} 69 | \providecommand{\vint}[3][\x]{\int_{#2} \! #3 \, \mathrm{d}#1} 70 | \providecommand{\sint}[3][\x]{\int_{#2} \! #3 \, \mathrm{d}A(#1)} 71 | \providecommand{\pder}[2]{\frac{\partial #1}{\partial #2}} 72 | \providecommand{\spder}[3]{\frac{\partial^2 #1}{\partial #2 \partial #3}} 73 | \providecommand{\tder}[2]{\frac{\mathrm{d} #1}{\mathrm{d} #2}} 74 | \providecommand{\evalat}[2]{\left.#1\right|_{#2}} 75 | \renewcommand{\vec}[1]{{\bf #1}} 76 | 77 | \providecommand{\tderatzero}[2]{\left.\frac{\mathrm{d} #1}{\mathrm{d} #2}\right|_{#2 = 0}} 78 | 79 | \newcommand{\TODO}[1]{\textbf{****** {\bf{[#1]}} ******}} 80 | 81 | \usepackage{prettyref} 82 | \newrefformat{sec}{Section~\ref{#1}} 83 | \newrefformat{tbl}{Table~\ref{#1}} 84 | \newrefformat{fig}{Figure~\ref{#1}} 85 | \newrefformat{chp}{Chapter~\ref{#1}} 86 | \newrefformat{eqn}{\eqref{#1}} 87 | \newrefformat{set}{\eqref{#1}} 88 | \newrefformat{alg}{Algorithm~\ref{#1}} 89 | \newrefformat{apx}{Appendix~\ref{#1}} 90 | \newrefformat{prop}{Proposition~\ref{#1}} 91 | \newcommand\pr[1]{\prettyref{#1}} 92 | 93 | \def\normal{{\bm \nu}} 94 | \def\n{\normal} 95 | \def\a{\vec{a}} 96 | \def\b{\vec{b}} 97 | \def\t{\vec{t}} 98 | \def\x{\vec{x}} 99 | \def\X{\vec{X}} 100 | \def\y{\vec{y}} 101 | \def\z{\vec{z}} 102 | \def\u{\vec{u}} 103 | \def\f{\vec{f}} 104 | \def\w{\boldsymbol{\omega}} 105 | \def\wn{\norm{\w}} 106 | \def\p{\vec{p}} 107 | \def\v{\vec{v}} 108 | \def\e{\vec{e}} 109 | \def\ue{\vec{u}^\e} 110 | \def\fu{\pder{\f}{u}} 111 | \def\fv{\pder{\f}{v}} 112 | \def\strain{\varepsilon} 113 | \def\stress{\sigma} 114 | \def\kb{\kappa \b} 115 | \def\kbi{(\kappa \b)_i} 116 | \def\k{\kappa} 117 | \def\R{\, \mathbb{R}} 118 | \def\L{\, \mathcal{L}} 119 | 120 | \providecommand{\compose}{\circ} 121 | \providecommand{\surface}{\Gamma} 122 | \providecommand{\surfacegrad}{\nabla_\surface} 123 | \providecommand{\surfacediv}{\surfacegrad \cdot} 124 | \providecommand{\surfacelaplacian}{\laplacian_\surface} 125 | 126 | \providecommand{\epssurface}{{\Gamma_\epsilon}} 127 | \providecommand{\epssurfacegrad}{\nabla_\epssurface} 128 | \providecommand{\epssurfacediv}{\epssurfacegrad \cdot} 129 | \providecommand{\epsnormal}{\normal_\epsilon} 130 | \providecommand{\epsnormalmat}{\tilde{\normal}_\epsilon} 131 | \providecommand{\epsphi}{\phi_\epsilon} 132 | \providecommand{\normalmatder}{\dot{\normal}} 133 | \providecommand{\shapefunc}{{\bm \phi}} 134 | 135 | \def\vt{\vec{v}_t} 136 | \def\k{\kappa} 137 | 138 | \newcommand*{\rom}[1]{\expandafter\@slowromancap\romannumeral #1@} 139 | \newcommand{\RN}[1]{\textup{\uppercase\expandafter{\romannumeral#1}}} 140 | 141 | \newtheorem{lemma}{Lemma} 142 | \newtheorem{proposition}{Proposition} 143 | \newtheorem{corollary}{Corollary} 144 | 145 | \makeatletter 146 | \usepackage{mathtools} 147 | \newcases{mycases}{\quad}{% 148 | \hfil$\m@th\displaystyle{##}$}{$\m@th\displaystyle{##}$\hfil}{\lbrace}{.} 149 | \makeatother 150 | 151 | \setlength{\droptitle}{-50pt} 152 | \title{\documenttitle} 153 | \author{Julian Panetta} 154 | 155 | % BEGIN DOCUMENT 156 | \begin{document} 157 | \maketitle 158 | 159 | To optimize an objective function involving rotational 160 | degrees of freedom, we first need to choose a representation for rotations. 161 | Our goal is to select a parametrization of $SO(3)$ that avoids 162 | singularities to the extent possible and that makes our optimizer's job easier. 163 | 164 | We could use unit quaternions, which would solve the problem 165 | of singularities, but would require us to impose unit norm constraints during the optimization. We could 166 | use Euler angles, but these will run into singularities (gimbal lock) after just 167 | a $\frac{\pi}{2}$ rotation. Instead, we use the tangent space to $SO(3)$ 168 | (infinitesimal rotations) at some ``reference'' rotation $R_0$. In this representation, the additional 169 | rotation to be applied after $R_0$ is encoded as a vector pointing along the 170 | rotation axis with length equal to the rotation angle. The rotation is then 171 | obtained by the exponential map (more precisely, we construct the skew-symmetric cross-product matrix ``$X$'' for this 172 | vector and calculate $e^X R_0$). 173 | 174 | This representation is nice because it allows rotations of up to $\pi$ before 175 | running into singularities. We can avoid singularities entirely by setting bound 176 | constraints on our infinitesimal rotation components and then updating the 177 | parametrization (changing $R_0$ to the current rotation) if the optimizer 178 | terminates with one of these bounds active. We could even update $R_0$ 179 | at every step of the optimization, which would greatly 180 | simplify the gradient and Hessian formulas as we'll see in \pr{sec:around_identity} 181 | (and as exploited 182 | in \cite{kugelstadt2018fast} and \cite{taylor1994minimization}). However, we 183 | derive the full formulas for the gradient and Hessian away from the identity, since 184 | updating the parametrization---changing the optimization variables---at every 185 | step isn't supported in off-the-shelf optimization libraries (e.g. Knitro or 186 | IPOPT). Note that \cite{grassia1998practical} proposes using the same 187 | parametrization, though they only provide gradient formulas, not Hessian 188 | formulas (and work with quaternions instead of Rodrigues' rotation formula). 189 | 190 | \section{Representation and Exponential Map} 191 | \label{sec:representation} 192 | We denote our infinitesimal rotation by vector $\w$, which encodes the rotation axis $\frac{\w}{\norm{\w}}$ 193 | and angle $\norm{\w}$. 194 | We can apply the rotation computed by the exponential map to a vector $\v$ using Rodrigues' rotation formula. 195 | For simplicity, we assume $R_0 = I$; this simplification can be applied in practice by first rotating $\v$ by $R_0$. 196 | $$ 197 | \tilde{\v} = R(\w) \v = 198 | \v \cos(\wn) + \w \w^T \v \frac{1 - \cos(\wn)}{\wn^2} + (\w \cross \v) \frac{\sin(\wn)}{\wn}. 199 | $$ 200 | (We could obtain the entire rotation matrix by substituting the canonical basis vectors $\e^0$, $\e^1$, $\e^2$ in for $\v$.) 201 | 202 | \section{Gradients and Hessians} 203 | Now we compute derivatives of the rotated vector with respect to $\w$: 204 | 205 | \begin{flalign*} 206 | \begin{aligned} 207 | \pder{\tilde{\v}}{\w} &= 208 | -(\v \otimes \w) \frac{\sin(\wn)}{\wn} 209 | + \big[(\w \cdot \v) I + \w \otimes \v \big] \left(\frac{1 - \cos(\wn)}{\wn^2}\right) 210 | + (\w \otimes \w)\left((\w \cdot \v) \frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 211 | \\ &\quad 212 | - [\v]_\cross \frac{\sin(\wn)}{\wn} 213 | + [(\w \cross \v) \otimes \w]\frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3} 214 | \end{aligned} 215 | \end{flalign*} 216 | \begin{flalign*} 217 | \boxed{ 218 | \begin{aligned} 219 | &= 220 | -(\v \otimes \w + [\v]_\cross) \frac{\sin(\wn)}{\wn} 221 | + \big[(\w \cdot \v) I + \w \otimes \v \big] \left(\frac{1 - \cos(\wn)}{\wn^2}\right) 222 | \\ &\quad 223 | + (\w \otimes \w)\left((\w \cdot \v) \frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 224 | + [(\w \cross \v) \otimes \w]\frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3}, 225 | \end{aligned} 226 | } 227 | \end{flalign*} 228 | where $[\v]_\cross$ is the cross product matrix for $\v$. 229 | Next, we differentiate again to get the Hessian (a third order tensor whose two 230 | ``rightmost'' slots correspond to the differentiation variables): 231 | 232 | \begin{align*} 233 | \frac{\partial^2 \tilde{\v}}{\partial \w^2} 234 | &= 235 | -(\v \otimes I) \frac{\sin(\wn)}{\wn} 236 | - \big[(\v \otimes \w + [\v]_\cross) \otimes \w\big] \left(\frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3}\right) 237 | \\ &\quad 238 | + \big[ I \otimes \v + \e^i \otimes \v \otimes \e^i \big] \left(\frac{1 - \cos(\wn)}{\wn^2}\right) 239 | + \big[ (\w \cdot \v) I + \w \otimes \v \big] \otimes \w \left(\frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 240 | \\ &\quad 241 | + \big[\e^i \otimes \w \otimes \e^i + \w \otimes \e^i \otimes \e^i\big] \left((\w \cdot \v) \frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 242 | \\ &\quad 243 | + \w \otimes \w \otimes \v \left(\frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 244 | \\ &\quad 245 | + \w \otimes \w \otimes \w \left((\w \cdot \v) \frac{8 + (\wn^2 - 8) \cos(\wn) - 5 \wn \sin(\wn)}{\wn^6}\right) 246 | \\ &\quad 247 | + \big[-\e^i \otimes \w \otimes [\v]_\cross^i + (\w\cross\v) \otimes I \big] \left(\frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3}\right) 248 | \\ &\quad 249 | + \big[(\w \cross \v) \otimes \w \otimes \w\big]\left(-\frac{3 \wn \cos(\wn) + (\wn^2 - 3) \sin(\wn)}{\wn^5}\right), 250 | \end{align*} 251 | were we sum over repeated superscripts ($i \in {0, 1, 2}$) and defined $[\v]_\cross^i$ to be the vector holding the $i^\text{th}$ 252 | \emph{row} of the cross product matrix $[\v]_\cross$: 253 | $$ 254 | [\v]_\cross = 255 | \begin{pmatrix} 256 | 0 & -v_2 & v_1 \\ 257 | v_2 & 0 & -v_0 \\ 258 | -v_1 & v_0 & 0 \\ 259 | \end{pmatrix} 260 | \quad \Longrightarrow \quad 261 | [\v]_\cross^0 = \begin{pmatrix} 0 \\ -v_2 \\ v_1 \end{pmatrix},\,\, 262 | [\v]_\cross^1 = \begin{pmatrix} v_2 \\ 0 \\ -v_0 \end{pmatrix},\,\, 263 | [\v]_\cross^2 = \begin{pmatrix} -v_1 \\ v_0 \\ 0 \end{pmatrix}. 264 | $$ 265 | We can simplify this Hessian into a form that reveals the expected symmetry with respect to the two rightmost indices: 266 | \begin{equation*} 267 | \boxed{ 268 | \begin{aligned} 269 | \frac{\partial^2 \tilde{\v}}{\partial \w^2} 270 | &= 271 | -(\v \otimes I) \frac{\sin(\wn)}{\wn} 272 | - \big[(\v \otimes \w \otimes \w + \e^i \otimes ([\v]_\cross^i \otimes \w + \w \otimes [\v]_\cross^i) + (\v \cross \w) \otimes I \big] \left(\frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3}\right) 273 | \\ &\quad 274 | + \big[ \e^i \otimes (\e^i \otimes \v + \v \otimes \e^i) \big] \left(\frac{1 - \cos(\wn)}{\wn^2}\right) 275 | \\ &\quad 276 | + \bigg[ (\w \cdot \v) \Big(\e^i \otimes (\e^i \otimes \w + \w \otimes \e^i) + \w \otimes I\Big) + \w \otimes (\v \otimes \w + \w \otimes \v) \bigg] \left(\frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 277 | \\ &\quad 278 | + \w \otimes \w \otimes \w \left((\w \cdot \v) \frac{8 + (\wn^2 - 8) \cos(\wn) - 5 \wn \sin(\wn)}{\wn^6}\right) 279 | \\ &\quad 280 | + \big[(\v \cross \w) \otimes \w \otimes \w\big]\left(\frac{3 \wn \cos(\wn) + (\wn^2 - 3) \sin(\wn)}{\wn^5}\right). 281 | \end{aligned} 282 | } 283 | \end{equation*} 284 | 285 | \clearpage 286 | \section{Numerically Robust Formulas} 287 | The rotation formula and its derivatives must be evaluated with care: around $\w = 0$, a naive implementation would 288 | attempt to calculate (approximately) $\frac{0}{0}$ for several of the expressions. In particular, we must use the following 289 | Taylor expansions to evaluate the problematic terms for $\wn \ll 1$: 290 | \begin{align*} 291 | \frac{\sin{\wn}}{\wn} &= 1 - \frac{\wn^2}{6} + O(\wn^4) \\ 292 | \frac{1 - \cos(\wn)}{\wn^2} &= \frac{1}{2} - \frac{\wn^2}{24} + O(\wn^4) \\ 293 | \frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3} &= -\frac{1}{3} + \frac{\wn^2}{30} + O(\wn^4) \\ 294 | \frac{2 \cos(\wn] - 2 + \wn \sin(\wn)}{\wn^4} &= -\frac{1}{12} + \frac{\wn^2}{180} + O(\wn^4) \\ 295 | \frac{8 + (\wn^2 - 8)\cos(\wn) - 5 \wn \sin(\wn)}{\wn^6} &= \frac{1}{90} - \frac{\wn^2}{1680} + O(\wn^4) \\ 296 | \frac{3 \wn \cos(\wn) + (\wn^2 - 3) \sin(\wn)}{\wn^5} &= -\frac{1}{15} + \frac{\wn^2}{210} + O(\wn^4). 297 | \end{align*} 298 | 299 | \section{Variations around the Identity} 300 | \label{sec:around_identity} 301 | Most of the terms in the gradient and Hessian formulas vanish when we evaluate at $\w = 0$. This means that if we update the 302 | parametrization at every iteration of Newton's method, we can use much simpler formulas: 303 | \begin{equation*} 304 | \left.\pder{\tilde{\v}}{\w}\right|_{\w = 0} = 305 | - [\v]_\cross, 306 | \quad \quad 307 | \left.\frac{\partial^2 \tilde{\v}}{\partial \w^2}\right|_{\w = 0} = 308 | -(\v \otimes I) 309 | + \left.\frac{1}{2} \middle[ \e^i \otimes (\e^i \otimes \v + \v \otimes \e^i) \right]. 310 | \end{equation*} 311 | 312 | \section{Full Rotation Matrix and its Derivatives} 313 | As mentioned in \pr{sec:representation}, could evaluate the rotation matrix and 314 | its derivatives using the formulas derived above for a single rotated vector: 315 | apply them to each of the three canonical basis 316 | vectors $\e^0, \e^1, \e^2$. However, due to the basis vectors' sparsity, 317 | we can derive more efficient expressions. The rotation matrix is: 318 | $$ 319 | R(\w) = I \cos(\wn) + (\w \otimes \w) \frac{1 - \cos(\wn)}{\wn^2} + [\w]_\cross \frac{\sin(\wn)}{\wn}. 320 | $$ 321 | The gradients and Hessians are now $3^\text{rd}$ and $4^\text{th}$ order tensors, respectively. The left two 322 | indices of these tensors pick a component of $R$ and the remaining indices pick differentiation variables 323 | from $\w$. 324 | \begin{align*} 325 | \pder{R}{\w} 326 | &= 327 | ([\e^i]_\cross \otimes \e^i - I \otimes \w) \frac{\sin(\wn)}{\wn} 328 | + \big[(\e^i \otimes \w + \w \otimes \e^i) \otimes \e^i \big] \left(\frac{1 - \cos(\wn)}{\wn^2}\right) 329 | \\ &\quad 330 | + (\w \otimes \w \otimes \w)\left(\frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 331 | + \big([\w]_\times \otimes \w\big)\frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3}, 332 | \\ \frac{\partial^2 R}{\partial \w^2} 333 | &= 334 | -(I \otimes I) \frac{\sin(\wn)}{\wn} 335 | +\big([\e^i]_\cross \otimes \e^i - I \otimes \w\big) \otimes \w \left(\frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3}\right) 336 | \\ &\quad 337 | + \big[(\e^i \otimes \e^k + \e^k \otimes \e^i) \otimes \e^i \otimes \e^k\big] \left(\frac{1 - \cos(\wn)}{\wn^2}\right) 338 | \\ &\quad 339 | + \big[(\e^i \otimes \w + \w \otimes \e^i) \otimes \e^i \otimes \w \big] \left(\frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 340 | \\ &\quad 341 | + \big[(\e^i \otimes \w \otimes \w + \w \otimes \e^i \otimes \w + \w \otimes \w \otimes \e^i \big] \otimes \e^i \left(\frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 342 | \\ &\quad 343 | + \big(\w \otimes \w \otimes \w \otimes \w\big) \left( \frac{8 + (\wn^2 - 8) \cos(\wn) - 5 \wn \sin(\wn)}{\wn^6}\right) 344 | \\ &\quad 345 | + \big([\e^i]_\cross \otimes \w \otimes \e^i + [\w]_\cross \otimes \e^i \otimes \e^i\big) \frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3} 346 | \\ &\quad 347 | - \big([\w]_\times \otimes \w\big) \otimes \w \left(\frac{3 \wn \cos(\wn) + (\wn^2 - 3) \sin(\wn)}{\wn^5}\right) 348 | \\ &= 349 | -(I \otimes I) \frac{\sin(\wn)}{\wn} 350 | +\big([\e^i]_\cross \otimes (\e^i \otimes \w + \w \otimes \e^i) - I \otimes \w \otimes \w + [\w]_\cross \otimes I \big) \left(\frac{\wn \cos(\wn) - \sin(\wn)}{\wn^3}\right) 351 | \\ &\quad 352 | + \big[\e^i \otimes \e^k \otimes (\e^i \otimes \e^k + \e^k \otimes \e^i)\big] \left(\frac{1 - \cos(\wn)}{\wn^2}\right) 353 | \\ &\quad 354 | + \big[(\e^i \otimes \w + \w \otimes \e^i) \otimes (\e^i \otimes \w + \w \otimes \e^i) + \w \otimes \w \otimes I \big] \left(\frac{2 \cos(\wn) - 2 + \wn \sin(\wn)}{\wn^4}\right) 355 | \\ &\quad 356 | + \big(\w \otimes \w \otimes \w \otimes \w\big) \left( \frac{8 + (\wn^2 - 8) \cos(\wn) - 5 \wn \sin(\wn)}{\wn^6}\right) 357 | \\ &\quad 358 | - \big([\w]_\times \otimes \w\big) \otimes \w \left(\frac{3 \wn \cos(\wn) + (\wn^2 - 3) \sin(\wn)}{\wn^5}\right). 359 | \end{align*} 360 | 361 | \bibliographystyle{plain} 362 | \bibliography{OptimizingRotations} 363 | 364 | \end{document} 365 | -------------------------------------------------------------------------------- /unit_tests.cc: -------------------------------------------------------------------------------- 1 | #define CATCH_CONFIG_MAIN 2 | #include 3 | #include "rotation_optimization.hh" 4 | #include 5 | 6 | // random value in [0, 1] 7 | double randDouble() { 8 | return double(random()) / RAND_MAX; 9 | } 10 | 11 | using ropt = rotation_optimization; 12 | using Mat3 = ropt::Mat3; 13 | using Vec3 = ropt::Vec3; 14 | 15 | Vec3 randomAxisAngle(double magnitude) { 16 | Vec3 a(Vec3::Random()); // random vector in [-1, 1]^3 17 | double theta = magnitude * (2 * randDouble() - 1.0); // try random angles in the interval [-magnitude, magnitude] 18 | return theta * (a / a.norm()); 19 | } 20 | 21 | //////////////////////////////////////////////////////////////////////////////// 22 | // Test Rotated Vectors 23 | //////////////////////////////////////////////////////////////////////////////// 24 | // Test rotations around all three axes of vectors along all three axes; 25 | TEST_CASE("Rotated vector test", "[rotated vector]" ) { 26 | Mat3 I(Mat3::Identity()); 27 | for (int j = 0; j < 3; ++j) { 28 | Vec3 v = I.col(j); 29 | for (size_t t = 0; t < 10000; ++t) { 30 | auto w = randomAxisAngle(M_PI); // try random axes and rotation angles in the interval [-pi, pi] 31 | auto vrot = ropt::rotated_vector(w, v); 32 | Vec3 rot_ground_truth = Eigen::AngleAxisd(w.norm(), w.normalized()) * v; 33 | REQUIRE((vrot - rot_ground_truth).norm() < 1e-15); 34 | } 35 | } 36 | } 37 | 38 | Mat3 finite_diff_gradient(const Vec3 &w, const Vec3 &v, const double eps) { 39 | Mat3 I(Mat3::Identity()); 40 | Mat3 result; 41 | for (size_t j = 0; j < 3; ++j) result.col(j) = (0.5 / eps) * (ropt::rotated_vector(w + eps * I.col(j), v) - ropt::rotated_vector(w - eps * I.col(j), v)); 42 | return result; 43 | } 44 | 45 | void finite_diff_hessian(const Vec3 &w, const Vec3 &v, 46 | std::array, 3> hess_comp, const double eps) { 47 | Mat3 I(Mat3::Identity()); 48 | for (size_t j = 0; j < 3; ++j) { 49 | Mat3 gdiff = (0.5 / eps) * (ropt::grad_rotated_vector(w + eps * I.col(j), v) - 50 | ropt::grad_rotated_vector(w - eps * I.col(j), v)); 51 | for (size_t i = 0; i < 3; ++i) { 52 | for (size_t k = 0; k < 3; ++k) 53 | hess_comp[i](j, k) = gdiff(i, k); 54 | } 55 | } 56 | } 57 | 58 | // Exhaustively test gradient and Hessian using finite difference: 59 | // By linearity, we can test on the three canonical basis vectors. 60 | // 61 | // We test the formulas exactly at the identity (w = 0) (where a simpler formula is used) 62 | // Near the identity with many random axes (||w|| << 1) (where Taylor expansions are used to avoid approximating 0/0) 63 | // Farther away from the identity with random axes/angles (where the full formula with trig functions is evaluated) 64 | TEST_CASE("Grad rotated vector tests", "[grad rotated vector]" ) { 65 | Mat3 I(Mat3::Identity()); 66 | const double eps = 1e-7; 67 | 68 | SECTION("Variation around identity") { 69 | for (size_t i = 0; i < 3; ++i) { 70 | auto g_fd = finite_diff_gradient(Vec3::Zero(), I.col(i), eps); 71 | auto g = ropt::grad_rotated_vector(Vec3::Zero(), I.col(i)); 72 | 73 | // std::cout << g << std::endl << std::endl; 74 | // std::cout << g_fd << std::endl << std::endl; 75 | 76 | REQUIRE((g - g_fd).cwiseAbs().maxCoeff() / g_fd.norm() < 1e-14); 77 | } 78 | } 79 | SECTION("Variation around small rotations") { 80 | for (size_t i = 0; i < 3; ++i) { 81 | for (size_t t = 0; t < 10000; ++t) { 82 | auto w = randomAxisAngle(1e-6); 83 | auto g_fd = finite_diff_gradient(w, I.col(i), eps); 84 | auto g = ropt::grad_rotated_vector(w, I.col(i)); 85 | REQUIRE((g - g_fd).cwiseAbs().maxCoeff() / g_fd.norm() < 1e-9); 86 | } 87 | } 88 | } 89 | SECTION("Variation around large rotations") { 90 | for (size_t i = 0; i < 3; ++i) { 91 | for (size_t t = 0; t < 10000; ++t) { 92 | auto w = randomAxisAngle(0.9 * M_PI); 93 | auto g_fd = finite_diff_gradient(w, I.col(i), eps); 94 | auto g = ropt::grad_rotated_vector(w, I.col(i)); 95 | REQUIRE((g - g_fd).cwiseAbs().maxCoeff() / g_fd.norm() < 1e-7); 96 | } 97 | } 98 | } 99 | } 100 | 101 | TEST_CASE("Hessian rotated vector tests", "[Hessian rotated vector]" ) { 102 | Mat3 I(Mat3::Identity()); 103 | Eigen::Matrix H_fd, H; 104 | std::array, 3> fd_hess_comp{{ H_fd.block<3, 3>(0, 0), H_fd.block<3, 3>(3, 0), H_fd.block<3, 3>(6, 0) }}; 105 | std::array, 3> hess_comp{{ H .block<3, 3>(0, 0), H .block<3, 3>(3, 0), H .block<3, 3>(6, 0) }}; 106 | const double eps = 5e-6; 107 | 108 | SECTION("Variation around identity") { 109 | for (size_t i = 0; i < 3; ++i) { 110 | finite_diff_hessian(Vec3::Zero(), I.col(i), fd_hess_comp, eps); 111 | ropt::hess_rotated_vector(Vec3::Zero(), I.col(i), hess_comp); 112 | 113 | REQUIRE((H - H_fd).cwiseAbs().maxCoeff() / H_fd.norm() < 1e-10); 114 | } 115 | } 116 | 117 | SECTION("Variation around small rotation") { 118 | for (size_t i = 0; i < 3; ++i) { 119 | for (size_t t = 0; t < 10000; ++t) { 120 | auto w = randomAxisAngle(1e-6); 121 | finite_diff_hessian(w, I.col(i), fd_hess_comp, eps); 122 | ropt::hess_rotated_vector(w, I.col(i), hess_comp); 123 | 124 | REQUIRE((H - H_fd).cwiseAbs().maxCoeff() / H_fd.norm() < 1e-9); 125 | } 126 | } 127 | } 128 | 129 | SECTION("Variation around large rotation") { 130 | for (size_t i = 0; i < 3; ++i) { 131 | for (size_t t = 0; t < 10000; ++t) { 132 | auto w = randomAxisAngle(0.9 * M_PI); 133 | finite_diff_hessian(w, I.col(i), fd_hess_comp, 1e-5); 134 | ropt::hess_rotated_vector(w, I.col(i), hess_comp); 135 | 136 | REQUIRE((H - H_fd).cwiseAbs().maxCoeff() / H_fd.norm() < 2e-8); 137 | } 138 | } 139 | } 140 | 141 | SECTION("Storage-backed interface") { 142 | std::array hess_comp_storage; 143 | for (size_t i = 0; i < 3; ++i) { 144 | for (size_t t = 0; t < 10000; ++t) { 145 | auto w = randomAxisAngle(0.9 * M_PI); 146 | ropt::hess_rotated_vector(w, I.col(i), hess_comp); 147 | ropt::hess_rotated_vector(w, I.col(i), hess_comp_storage); 148 | double diff = 0; 149 | for (size_t c = 0; c < 3; ++c) 150 | diff += (hess_comp[c] - hess_comp_storage[c]).norm(); 151 | REQUIRE(diff == 0); 152 | } 153 | } 154 | } 155 | 156 | SECTION("Contraction with vector") { 157 | std::array hess_comp_storage; 158 | for (size_t i = 0; i < 3; ++i) { 159 | for (size_t t = 0; t < 10000; ++t) { 160 | auto w = randomAxisAngle(0.9 * M_PI); 161 | auto d = randomAxisAngle(0.9 * M_PI); 162 | Mat3 result = ropt::d_contract_hess_rotated_vector(w, I.col(i), d); 163 | ropt::hess_rotated_vector(w, I.col(i), hess_comp_storage); 164 | Mat3 result_ground_truth = Mat3::Zero(); 165 | for (size_t c = 0; c < 3; ++c) 166 | result_ground_truth += d[c] * hess_comp_storage[c]; 167 | REQUIRE((result - result_ground_truth).norm() < 1e-15 * result.norm()); 168 | } 169 | } 170 | } 171 | } 172 | 173 | double norm(const Eigen::Tensor &a) { 174 | Eigen::array, 3> contraction_indices{{Eigen::IndexPair(0, 0), Eigen::IndexPair(1, 1), Eigen::IndexPair(2, 2)}}; 175 | Eigen::Tensor sumSquared = a.contract(a, contraction_indices); 176 | return std::sqrt(sumSquared(0)); 177 | } 178 | 179 | double norm(const Eigen::Tensor &a) { 180 | Eigen::array, 4> contraction_indices{{Eigen::IndexPair(0, 0), Eigen::IndexPair(1, 1), Eigen::IndexPair(2, 2), Eigen::IndexPair(3, 3)}}; 181 | Eigen::Tensor sumSquared = a.contract(a, contraction_indices); 182 | return std::sqrt(sumSquared(0)); 183 | } 184 | 185 | template 186 | double relError(const Eigen::Tensor &a, const Eigen::Tensor &b) { 187 | Eigen::Tensor absMax = (a - b).abs().maximum(); 188 | return absMax(0) / norm(b); 189 | } 190 | 191 | //////////////////////////////////////////////////////////////////////////////// 192 | // Test Rotated Matrices 193 | //////////////////////////////////////////////////////////////////////////////// 194 | template 195 | Eigen::Tensor finite_diff_rotated_matrix_gradient(const Vec3 &w, const Mat &A, const double eps) { 196 | Mat3 I(Mat3::Identity()); 197 | const int ncols = A.cols(); 198 | Eigen::Tensor result(3, ncols, 3); 199 | for (int k = 0; k < 3; ++k) { 200 | auto fdslice = ((0.5 / eps) * (ropt::rotated_matrix(w + eps * I.col(k), A) - ropt::rotated_matrix(w - eps * I.col(k), A))).eval(); 201 | for (int i = 0; i < 3; ++i) 202 | for (int j = 0; j < ncols; ++j) 203 | result(i, j, k) = fdslice(i, j); 204 | } 205 | return result; 206 | } 207 | 208 | template 209 | Eigen::Tensor finite_diff_rotated_matrix_hessian(const Vec3 &w, const Mat &A, const double eps) { 210 | Mat3 I(Mat3::Identity()); 211 | const int ncols = A.cols(); 212 | Eigen::Tensor result(3, ncols, 3, 3); 213 | for (int l = 0; l < 3; ++l) { 214 | Eigen::Tensor fdslice = (0.5 / eps) * (ropt::grad_rotated_matrix(w + eps * I.col(l), A) - 215 | ropt::grad_rotated_matrix(w - eps * I.col(l), A)); 216 | for (int i = 0; i < 3; ++i) { 217 | for (int j = 0; j < ncols; ++j) { 218 | for (int k = 0; k < 3; ++k) 219 | result(i, j, k, l) = fdslice(i, j, k); 220 | } 221 | } 222 | } 223 | return result; 224 | } 225 | 226 | template 227 | void run_rotated_matrix_test(const Test &the_test, double angleMag, int ncols = N) { 228 | Eigen::Matrix A(3, ncols); 229 | 230 | for (int mat_choice = 0; mat_choice < 100; ++mat_choice) { 231 | A.setRandom(); // Random components in [-1, 1] 232 | if (angleMag == 0) 233 | the_test(Vec3::Zero(), A); 234 | else { 235 | for (int t = 0; t < 1000; ++t) 236 | the_test(randomAxisAngle(angleMag), A); 237 | } 238 | } 239 | } 240 | 241 | TEST_CASE("Rotated matrix test", "[rotated matrix]" ) { 242 | auto the_test = [](const Vec3 &w, const auto &A) { 243 | auto rot = ropt::rotated_matrix(w, A); 244 | auto rot_ground_truth = (Eigen::AngleAxisd(w.norm(), w.normalized()).matrix() * A).eval(); 245 | REQUIRE((rot - rot_ground_truth).norm() < 1e-9); 246 | }; 247 | run_rotated_matrix_test< 1>(the_test, M_PI); 248 | run_rotated_matrix_test< 2>(the_test, M_PI); 249 | run_rotated_matrix_test< 3>(the_test, M_PI); 250 | run_rotated_matrix_test< 4>(the_test, M_PI); 251 | run_rotated_matrix_test(the_test, M_PI, 1); 252 | run_rotated_matrix_test(the_test, M_PI, 2); 253 | run_rotated_matrix_test(the_test, M_PI, 3); 254 | run_rotated_matrix_test(the_test, M_PI, 4); 255 | } 256 | 257 | TEST_CASE("Grad rotated matrix test", "[grad rotated matrix]" ) { 258 | const double eps = 5e-6; 259 | 260 | double tolerance = 0; 261 | bool verbose = false; 262 | auto the_test = [&](const Vec3 &w, const auto &A) { 263 | auto g = ropt::grad_rotated_matrix(w, A); 264 | auto g_fd = finite_diff_rotated_matrix_gradient(w, A, eps); 265 | if (verbose) { 266 | std::cout << "analytic: \n" << g << std::endl << std::endl; 267 | std::cout << "fte diff: \n" << g_fd << std::endl << std::endl; 268 | } 269 | REQUIRE(relError(g, g_fd) < tolerance); 270 | }; 271 | 272 | SECTION("Variation around identity") { 273 | tolerance = 1e-9; 274 | run_rotated_matrix_test<1>(the_test, 0); 275 | run_rotated_matrix_test<2>(the_test, 0); 276 | run_rotated_matrix_test<3>(the_test, 0); 277 | run_rotated_matrix_test<4>(the_test, 0); 278 | run_rotated_matrix_test(the_test, 0, 1); 279 | run_rotated_matrix_test(the_test, 0, 2); 280 | run_rotated_matrix_test(the_test, 0, 3); 281 | run_rotated_matrix_test(the_test, 0, 4); 282 | } 283 | SECTION("Variation around small rotations") { 284 | tolerance = 1e-8; 285 | run_rotated_matrix_test< 1>(the_test, 1e-6); 286 | run_rotated_matrix_test< 2>(the_test, 1e-6); 287 | run_rotated_matrix_test< 3>(the_test, 1e-6); 288 | run_rotated_matrix_test< 4>(the_test, 1e-6); 289 | run_rotated_matrix_test(the_test, 1e-6, 1); 290 | run_rotated_matrix_test(the_test, 1e-6, 2); 291 | run_rotated_matrix_test(the_test, 1e-6, 3); 292 | run_rotated_matrix_test(the_test, 1e-6, 4); 293 | } 294 | SECTION("Variation around large rotations") { 295 | tolerance = 2e-8; 296 | run_rotated_matrix_test< 1>(the_test, 0.9 * M_PI); 297 | run_rotated_matrix_test< 2>(the_test, 0.9 * M_PI); 298 | run_rotated_matrix_test< 3>(the_test, 0.9 * M_PI); 299 | run_rotated_matrix_test< 4>(the_test, 0.9 * M_PI); 300 | run_rotated_matrix_test(the_test, 0.9 * M_PI, 1); 301 | run_rotated_matrix_test(the_test, 0.9 * M_PI, 2); 302 | run_rotated_matrix_test(the_test, 0.9 * M_PI, 3); 303 | run_rotated_matrix_test(the_test, 0.9 * M_PI, 4); 304 | } 305 | } 306 | 307 | TEST_CASE("Hessian rotated matrix test", "[Hessian rotated matrix]" ) { 308 | double eps = 5e-6; 309 | 310 | double tolerance = 0; 311 | bool verbose = false; 312 | auto the_test = [&](const Vec3 &w, const auto &A) { 313 | auto H = ropt::hess_rotated_matrix(w, A); 314 | auto H_fd = finite_diff_rotated_matrix_hessian(w, A, eps); 315 | if (verbose) { 316 | std::cout << "analytic: \n" << H << std::endl << std::endl; 317 | std::cout << "fte diff: \n" << H_fd << std::endl << std::endl; 318 | } 319 | REQUIRE(relError(H, H_fd) < tolerance); 320 | }; 321 | 322 | SECTION("Variation around identity: static") { 323 | tolerance = 1e-9; 324 | run_rotated_matrix_test<1>(the_test, 0); 325 | run_rotated_matrix_test<2>(the_test, 0); 326 | run_rotated_matrix_test<3>(the_test, 0); 327 | } 328 | SECTION("Variation around identity: dynamic") { 329 | tolerance = 1e-9; 330 | run_rotated_matrix_test(the_test, 0, 1); 331 | run_rotated_matrix_test(the_test, 0, 2); 332 | run_rotated_matrix_test(the_test, 0, 3); 333 | run_rotated_matrix_test(the_test, 0, 4); 334 | } 335 | SECTION("Variation around small rotations") { 336 | tolerance = 2e-8; 337 | run_rotated_matrix_test< 1>(the_test, 1e-6); 338 | run_rotated_matrix_test< 2>(the_test, 1e-6); 339 | run_rotated_matrix_test< 3>(the_test, 1e-6); 340 | run_rotated_matrix_test(the_test, 1e-6, 1); 341 | run_rotated_matrix_test(the_test, 1e-6, 2); 342 | run_rotated_matrix_test(the_test, 1e-6, 3); 343 | run_rotated_matrix_test(the_test, 1e-6, 4); 344 | } 345 | SECTION("Variation around large rotations") { 346 | eps = 1e-5; 347 | tolerance = 2e-8; 348 | run_rotated_matrix_test< 1>(the_test, 0.9 * M_PI); 349 | run_rotated_matrix_test< 2>(the_test, 0.9 * M_PI); 350 | run_rotated_matrix_test< 3>(the_test, 0.9 * M_PI); 351 | run_rotated_matrix_test(the_test, 0.9 * M_PI, 1); 352 | run_rotated_matrix_test(the_test, 0.9 * M_PI, 2); 353 | run_rotated_matrix_test(the_test, 0.9 * M_PI, 3); 354 | run_rotated_matrix_test(the_test, 0.9 * M_PI, 4); 355 | } 356 | } 357 | 358 | //////////////////////////////////////////////////////////////////////////////// 359 | // Test Rotation Matrices 360 | //////////////////////////////////////////////////////////////////////////////// 361 | Eigen::Tensor finite_diff_rotation_gradient(const Vec3 &w, const double eps) { 362 | Mat3 I(Mat3::Identity()); 363 | Eigen::Tensor result(3, 3, 3); 364 | for (int k = 0; k < 3; ++k) { 365 | Mat3 fdslice = (0.5 / eps) * (ropt::rotation_matrix(w + eps * I.col(k)) - ropt::rotation_matrix(w - eps * I.col(k))); 366 | for (int i = 0; i < 3; ++i) 367 | for (int j = 0; j < 3; ++j) 368 | result(i, j, k) = fdslice(i, j); 369 | } 370 | return result; 371 | } 372 | 373 | Eigen::Tensor finite_diff_rotation_hessian(const Vec3 &w, const double eps) { 374 | Mat3 I(Mat3::Identity()); 375 | Eigen::Tensor result(3, 3, 3, 3); 376 | for (int l = 0; l < 3; ++l) { 377 | Eigen::Tensor fdslice = (0.5 / eps) * (ropt::grad_rotation_matrix(w + eps * I.col(l)) - 378 | ropt::grad_rotation_matrix(w - eps * I.col(l))); 379 | for (int i = 0; i < 3; ++i) { 380 | for (int j = 0; j < 3; ++j) { 381 | for (int k = 0; k < 3; ++k) 382 | result(i, j, k, l) = fdslice(i, j, k); 383 | } 384 | } 385 | } 386 | return result; 387 | } 388 | 389 | TEST_CASE("Rotation matrix test", "[rotation matrix]" ) { 390 | for (size_t t = 0; t < 10000; ++t) { 391 | auto w = randomAxisAngle(M_PI); // try random axes and rotation angles in the interval [-pi, pi] 392 | Mat3 rot = ropt::rotation_matrix(w); 393 | Mat3 rot_ground_truth = Eigen::AngleAxisd(w.norm(), w.normalized()).matrix(); 394 | REQUIRE((rot - rot_ground_truth).norm() < 1e-10); 395 | } 396 | } 397 | 398 | TEST_CASE("Grad rotation matrix test", "[grad rotation matrix]" ) { 399 | const double eps = 1e-7; 400 | 401 | SECTION("Variation around identity") { 402 | auto g_fd = finite_diff_rotation_gradient(Vec3::Zero(), eps); 403 | auto g = ropt::grad_rotation_matrix(Vec3::Zero()); 404 | 405 | REQUIRE(relError(g, g_fd) < 1e-14); 406 | } 407 | SECTION("Variation around small rotations") { 408 | for (size_t t = 0; t < 10000; ++t) { 409 | auto w = randomAxisAngle(1e-6); 410 | auto g_fd = finite_diff_rotation_gradient(w, eps); 411 | auto g = ropt::grad_rotation_matrix(w); 412 | REQUIRE(relError(g, g_fd) < 1e-9); 413 | } 414 | } 415 | SECTION("Variation around large rotations") { 416 | for (size_t t = 0; t < 10000; ++t) { 417 | auto w = randomAxisAngle(0.9 * M_PI); 418 | auto g_fd = finite_diff_rotation_gradient(w, eps); 419 | auto g = ropt::grad_rotation_matrix(w); 420 | 421 | REQUIRE(relError(g, g_fd) < 2e-8); 422 | } 423 | } 424 | } 425 | 426 | TEST_CASE("Hessian rotation matrix test", "[Hessian rotation matrix]" ) { 427 | const double eps = 5e-6; 428 | 429 | SECTION("Variation around identity") { 430 | auto H_fd = finite_diff_rotation_hessian(Vec3::Zero(), eps); 431 | auto H = ropt::hess_rotation_matrix(Vec3::Zero()); 432 | 433 | // std::cout << H << std::endl << std::endl; 434 | // std::cout << H_fd << std::endl << std::endl; 435 | // std::cout << "relError: " << relError(H, H_fd) << std::endl; 436 | 437 | REQUIRE(relError(H, H_fd) < 1e-11); 438 | } 439 | SECTION("Variation around small rotations") { 440 | for (size_t t = 0; t < 10000; ++t) { 441 | auto w = randomAxisAngle(1e-6); 442 | auto H_fd = finite_diff_rotation_hessian(w, eps); 443 | auto H = ropt::hess_rotation_matrix(w); 444 | 445 | // std::cout << H << std::endl << std::endl; 446 | // std::cout << H_fd << std::endl << std::endl; 447 | // std::cout << "relError: " << relError(H, H_fd) << std::endl; 448 | 449 | REQUIRE(relError(H, H_fd) < 1e-9); 450 | } 451 | } 452 | 453 | SECTION("Variation around large rotations") { 454 | for (size_t t = 0; t < 10000; ++t) { 455 | auto w = randomAxisAngle(0.9 * M_PI); 456 | auto H_fd = finite_diff_rotation_hessian(w, eps); 457 | auto H = ropt::hess_rotation_matrix(w); 458 | 459 | // std::cout << H << std::endl << std::endl; 460 | // std::cout << H_fd << std::endl << std::endl; 461 | // std::cout << "w: " << w.transpose() << std::endl; 462 | // std::cout << "relError: " << relError(H, H_fd) << std::endl; 463 | 464 | REQUIRE(relError(H, H_fd) < 2e-8); 465 | } 466 | } 467 | } 468 | -------------------------------------------------------------------------------- /rotation_optimization.inl: -------------------------------------------------------------------------------- 1 | #ifndef ROTATION_OPTIMIZATION_INL_HH 2 | #define ROTATION_OPTIMIZATION_INL_HH 3 | 4 | #include "rotation_optimization.hh" 5 | #include 6 | #include 7 | 8 | //////////////////////////////////////////////////////////////////////////////// 9 | // Numerically robust formulas for the trig expressions in our formulas 10 | //////////////////////////////////////////////////////////////////////////////// 11 | // Choose a good tradeoff between catastrophic cancellation in the direct 12 | // calculation and truncation error in the Taylor series approximation. 13 | constexpr double theta_sq_crossover_threshold = 2e-6; 14 | 15 | // cos(theta); needed for robustness when Real_ is an automatic differentiation type 16 | template 17 | Real_ cos(Real_ theta, Real_ theta_sq) { 18 | if (theta_sq < theta_sq_crossover_threshold) { return 1.0 - 0.5 * theta_sq + theta_sq * theta_sq / 24; } 19 | return cos(theta); 20 | } 21 | 22 | // sin(theta) / theta 23 | template 24 | Real_ sinc(Real_ theta, Real_ theta_sq) { 25 | if (theta_sq < theta_sq_crossover_threshold) { return 1.0 - theta_sq / 6.0; } 26 | return sin(theta) / theta; 27 | } 28 | 29 | // (1 - cos(theta)) / theta^2 30 | template 31 | Real_ one_minus_cos_div_theta_sq(Real_ theta, Real_ theta_sq) { 32 | if (theta_sq < theta_sq_crossover_threshold) { return 0.5 - theta_sq / 24.0; } 33 | return (1 - cos(theta)) / theta_sq; 34 | } 35 | 36 | // (theta cos(theta) - sin(theta)) / theta^3 37 | template 38 | Real_ theta_cos_minus_sin_div_theta_cubed(Real_ theta, Real_ theta_sq) { 39 | if (theta_sq < theta_sq_crossover_threshold) { return -1.0 / 3.0 + theta_sq / 30.0; } 40 | return (theta * cos(theta) - sin(theta)) / (theta * theta_sq); 41 | } 42 | 43 | // (2 cos(theta) - 2 + theta sin(theta)) / theta^4 44 | template 45 | Real_ two_cos_minus_2_plus_theta_sin_div_theta_pow_4(Real_ theta, Real_ theta_sq) { 46 | if (theta_sq < theta_sq_crossover_threshold) { return -1.0 / 12.0 + theta_sq / 180.0; } 47 | return (2 * cos(theta) - 2 + theta * sin(theta)) / (theta_sq * theta_sq); 48 | } 49 | 50 | // (8 + (theta^2 - 8) cos(theta) - 5 theta sin(theta)) / theta^6 51 | template 52 | Real_ eight_plus_theta_sq_minus_eight_cos_minus_five_theta_sin_div_theta_pow_6(Real_ theta, Real_ theta_sq) { 53 | if (theta_sq < theta_sq_crossover_threshold) { return 1.0 / 90.0 - theta_sq / 1680.0; } 54 | return (8 + (theta_sq - 8) * cos(theta) - 5 * theta * sin(theta)) / (theta_sq * theta_sq * theta_sq); 55 | } 56 | 57 | // (3 theta cos(theta) + (theta^2 - 3) sin(theta)) / theta^5 58 | template 59 | Real_ three_theta_cos_plus_theta_sq_minus_3_sin_div_theta_pow_5(Real_ theta, Real_ theta_sq) { 60 | if (theta_sq < theta_sq_crossover_threshold) { return -1.0 / 15.0 + theta_sq / 210.0; } 61 | return (3 * theta * cos(theta) + (theta_sq - 3) * sin(theta)) / (theta_sq * theta_sq * theta); 62 | } 63 | 64 | // (theta - sin(theta)) / theta^3 65 | template 66 | Real_ theta_minus_sin_div_theta_cubed(Real_ theta, Real_ theta_sq) { 67 | if (theta_sq < theta_sq_crossover_threshold) { return 1.0 / 6.0 - theta_sq / 120.0; } 68 | return (theta - sin(theta)) / (theta * theta_sq); 69 | } 70 | 71 | // (3 sin(theta) - theta * (2 + cos(theta))) / theta^5 72 | template 73 | Real_ three_sin_minus_theta_times_two_plus_cos_div_theta_pow_5(Real_ theta, Real_ theta_sq) { 74 | if (theta_sq < theta_sq_crossover_threshold) { return -1.0 / 60.0 + theta_sq / 1260.0; } 75 | return (3 * sin(theta) - theta * (2 + cos(theta))) / (theta_sq * theta_sq * theta); 76 | } 77 | 78 | // ((theta^2 - 15) sin(theta) + 8 theta + 7 theta * cos(theta))/theta^7 79 | template 80 | Real_ theta_sq_minus_15_sin_plus_8_theta_plus_7_theta_cos_div_theta_pow_7(Real_ theta, Real_ theta_sq) { 81 | if (theta_sq < theta_sq_crossover_threshold) { return 1.0 / 630.0 - theta_sq / 15120.0; } 82 | return ((theta_sq - 15) * sin(theta) + 8 * theta + 7 * theta * cos(theta)) / (theta_sq * theta_sq * theta_sq * theta); 83 | } 84 | 85 | //////////////////////////////////////////////////////////////////////////////// 86 | // Helper functions 87 | //////////////////////////////////////////////////////////////////////////////// 88 | template 89 | auto rotation_optimization::cross_product_matrix(const Vec3 &v) -> Mat3 { 90 | Mat3 result; 91 | result << 0, -v[2], v[1], 92 | v[2], 0, -v[0], 93 | -v[1], v[0], 0; 94 | return result; 95 | } 96 | 97 | //////////////////////////////////////////////////////////////////////////////// 98 | // Rotation matrix, rotated vectors, and rotated matrices 99 | //////////////////////////////////////////////////////////////////////////////// 100 | // Compute R(w) 101 | template 102 | auto rotation_optimization::rotation_matrix(const Vec3 &w) -> Mat3 { 103 | // When theta_sq = 0, sqrt(theta_sq) is non-differentiable, so the autodiff code will produce "NaN"s for theta's derivative. 104 | // However, using the taylor expansions of the output terms ignores theta (and its derivative) in the small angle case, 105 | // so this can safely be ignored. 106 | const Real_ theta_sq = w.squaredNorm(); 107 | const Real_ theta = sqrt(theta_sq); 108 | const Real_ cos_th = cos(theta, theta_sq); 109 | 110 | return cos_th * Mat3::Identity() + (w * w.transpose()) * one_minus_cos_div_theta_sq(theta, theta_sq) + cross_product_matrix(w) * sinc(theta, theta_sq); 111 | } 112 | 113 | // Compute R(w) v 114 | template 115 | auto rotation_optimization::rotated_vector(const Vec3 &w, const Vec3 &v) -> Vec3 { 116 | const Real_ theta_sq = w.squaredNorm(); 117 | const Real_ theta = sqrt(theta_sq); 118 | const Real_ cos_th = cos(theta, theta_sq); 119 | return v * cos_th + w * (w.dot(v) * one_minus_cos_div_theta_sq(theta, theta_sq)) + w.cross(v) * sinc(theta, theta_sq); 120 | } 121 | 122 | template 123 | template 124 | auto rotation_optimization::rotated_matrix(const Vec3 &w, const Eigen::Matrix &A) -> Eigen::Matrix { 125 | const Real_ theta_sq = w.squaredNorm(); 126 | const Real_ theta = sqrt(theta_sq); 127 | const Real_ cos_th = cos(theta, theta_sq); 128 | 129 | return A * cos_th + w * (w.transpose() * A) * one_minus_cos_div_theta_sq(theta, theta_sq) - A.colwise().cross(w * sinc(theta, theta_sq)); 130 | } 131 | 132 | //////////////////////////////////////////////////////////////////////////////// 133 | // Gradient of rotation matrix, rotated vectors, and rotated matrices 134 | //////////////////////////////////////////////////////////////////////////////// 135 | // The gradient of R(w). This is a third order tensor: 136 | // g_ijk = D [R(w)]_ij / dw_k 137 | template 138 | auto rotation_optimization::grad_rotation_matrix(const Vec3 &w) -> Eigen::Tensor { 139 | Eigen::Tensor g(3, 3, 3); 140 | g.setZero(); 141 | 142 | const Real_ theta_sq = w.squaredNorm(); 143 | // Use simpler formula for variation around the identity 144 | // (But only if we're using a native floating point type; 145 | // for autodiff types, we need the full formula). 146 | if ((theta_sq == 0) && (std::is_arithmetic::value)) { 147 | // [e^i]_cross otimes e^i 148 | g(1, 2, 0) = -1; g(2, 1, 0) = 1; 149 | g(0, 2, 1) = 1; g(2, 0, 1) = -1; 150 | g(0, 1, 2) = -1; g(1, 0, 2) = 1; 151 | return g; 152 | } 153 | 154 | const Real_ theta = sqrt(theta_sq); 155 | const Real_ coeff0 = sinc(theta, theta_sq); 156 | const Real_ coeff1 = one_minus_cos_div_theta_sq(theta, theta_sq); 157 | const Real_ coeff2 = two_cos_minus_2_plus_theta_sin_div_theta_pow_4(theta, theta_sq); 158 | const Real_ coeff3 = theta_cos_minus_sin_div_theta_cubed(theta, theta_sq); 159 | 160 | // I otimes w * (-sinc(theta)) 161 | for (int k = 0; k < 3; ++k) g(0, 0, k) = g(1, 1, k) = g(2, 2, k) = -coeff0 * w[k]; 162 | // [e^i]_cross otimes e^i * (sinc(theta)) 163 | g(1, 2, 0) = -coeff0; g(2, 1, 0) = coeff0; 164 | g(0, 2, 1) = coeff0; g(2, 0, 1) = -coeff0; 165 | g(0, 1, 2) = -coeff0; g(1, 0, 2) = coeff0; 166 | 167 | // (e^i otimes w + w otimes e^i) otimes e^i * coeff1 168 | for (int j = 0; j < 3; ++j) { 169 | const Real_ val = w[j] * coeff1; 170 | for (int i = 0; i < 3; ++i) { 171 | g(i, j, i) += val; 172 | g(j, i, i) += val; 173 | } 174 | } 175 | 176 | Mat3 w_cross = cross_product_matrix(w); 177 | // w otimes w otimes w coeff2 + [w]_x otimes w coeff3 178 | for (int i = 0; i < 3; ++i) 179 | for (int j = 0; j < 3; ++j) 180 | for (int k = 0; k < 3; ++k) 181 | g(i, j, k) += w[i] * w[j] * w[k] * coeff2 + w_cross(i, j) * w[k] * coeff3; 182 | 183 | return g; 184 | } 185 | 186 | // Gradient of R(w) v. This is a second order tensor, returned as a 3x3 matrix: 187 | // g_ij = D [R(w) v]_i / dw_j 188 | template 189 | auto rotation_optimization::grad_rotated_vector(const Vec3 &w, const Vec3 &v) -> Mat3 { 190 | const Real_ theta_sq = w.squaredNorm(); 191 | 192 | // Use simpler formula for variation around the identity 193 | // (But only if we're using a native floating point type; 194 | // for autodiff types, we need the full formula). 195 | if ((theta_sq == 0) && (std::is_arithmetic::value)) { return -cross_product_matrix(v); } 196 | 197 | const Real_ theta = sqrt(theta_sq); 198 | const Real_ w_dot_v = w.dot(v); 199 | 200 | // Mat3 result = (v * w.transpose() + cross_product_matrix(v)) * -sinc(theta, theta_sq); 201 | // result += (w_dot_v * Mat3::Identity() + w * v.transpose()) * one_minus_cos_div_theta_sq(theta, theta_sq); 202 | // result += (w * w.transpose()) * (w_dot_v * two_cos_minus_2_plus_theta_sin_div_theta_pow_4(theta, theta_sq)); 203 | // result += (w.cross(v) * w.transpose()) * theta_cos_minus_sin_div_theta_cubed(theta, theta_sq); 204 | 205 | const Vec3 neg_v_sinc = v * (-sinc(theta, theta_sq)); 206 | const Real_ coeff = one_minus_cos_div_theta_sq(theta, theta_sq); 207 | 208 | Mat3 result = (neg_v_sinc + w.cross(v) * theta_cos_minus_sin_div_theta_cubed(theta, theta_sq) + w * (w_dot_v * two_cos_minus_2_plus_theta_sin_div_theta_pow_4(theta, theta_sq))) * w.transpose() 209 | + (coeff * w) * v.transpose(); 210 | result.diagonal().array() += w_dot_v * coeff; 211 | result(0, 1) += -neg_v_sinc[2]; 212 | result(0, 2) += neg_v_sinc[1]; 213 | result(1, 0) += neg_v_sinc[2]; 214 | result(1, 2) += -neg_v_sinc[0]; 215 | result(2, 0) += -neg_v_sinc[1]; 216 | result(2, 1) += neg_v_sinc[0]; 217 | 218 | return result; 219 | } 220 | 221 | // Gradient of R(w) A. This is a third order tensor, returned as a 3x3 matrix: 222 | // g_ijk = D [R(w) A]_ij / dw_k 223 | // Could be optimized... 224 | template 225 | template 226 | auto rotation_optimization::grad_rotated_matrix(const Vec3 &w, const Eigen::Matrix &A) -> Eigen::Tensor { 227 | int ncols = A.cols(); 228 | Eigen::Tensor g(3, ncols, 3); 229 | 230 | // Use simpler formula for variation around the identity 231 | // (But only if we're using a native floating point type; 232 | // for autodiff types, we need the full formula). 233 | const Real_ theta_sq = w.squaredNorm(); 234 | if ((theta_sq == 0) && (std::is_arithmetic::value)) { 235 | for (int j = 0; j < ncols; ++j) { 236 | Mat3 tmp = -cross_product_matrix(A.col(j)); 237 | for (int i = 0; i < 3; ++i) { 238 | for (int k = 0; k < 3; ++k) 239 | g(i, j, k) = tmp(i, k); 240 | } 241 | } 242 | return g; 243 | } 244 | 245 | const Real_ theta = sqrt(theta_sq); 246 | const Real_ coeff0 = -sinc(theta, theta_sq); 247 | const Real_ coeff1 = one_minus_cos_div_theta_sq(theta, theta_sq); 248 | const Real_ coeff2 = two_cos_minus_2_plus_theta_sin_div_theta_pow_4(theta, theta_sq); 249 | const Real_ coeff3 = theta_cos_minus_sin_div_theta_cubed(theta, theta_sq); 250 | 251 | for (int j = 0; j < ncols; ++j) { 252 | const auto &v = A.col(j); 253 | const Real_ w_dot_v = w.dot(v); 254 | 255 | Mat3 tmp = (v * w.transpose() + cross_product_matrix(v)) * coeff0 256 | + (w_dot_v * Mat3::Identity() + w * v.transpose()) * coeff1 257 | + (w * w.transpose()) * (w_dot_v * coeff2) 258 | + (w.cross(v) * w.transpose()) * coeff3; 259 | for (int i = 0; i < 3; ++i) { 260 | for (int k = 0; k < 3; ++k) 261 | g(i, j, k) = tmp(i, k); 262 | } 263 | } 264 | 265 | return g; 266 | } 267 | 268 | //////////////////////////////////////////////////////////////////////////////// 269 | // Hessian of rotation matrix, rotated vectors, and rotated matrices 270 | //////////////////////////////////////////////////////////////////////////////// 271 | // The Hessian of R(w). This is a fourth order tensor: 272 | // H_ijkl = d [R(w)]_ij / (dw_k dw_l) 273 | template 274 | auto rotation_optimization::hess_rotation_matrix(const Vec3 &w) -> Eigen::Tensor { 275 | Eigen::Tensor H(3, 3, 3, 3); 276 | H.setZero(); 277 | 278 | const Real_ theta_sq = w.squaredNorm(); 279 | // Use simpler formula for variation around the identity 280 | // (But only if we're using a native floating point type; 281 | // for autodiff types, we need the full formula). 282 | if ((theta_sq == 0) && (std::is_arithmetic::value)) { 283 | // - I otimes I 284 | for (int i = 0; i < 3; ++i) 285 | for (int k = 0; k < 3; ++k) 286 | H(i, i, k, k) = -1; 287 | // 0.5 (e^i otimes e^k otimes (e^i otimes e^k + e^k otimes e^i)) 288 | for (int i = 0; i < 3; ++i) { 289 | for (int k = 0; k < 3; ++k) { 290 | H(i, k, i, k) += 0.5; 291 | H(i, k, k, i) += 0.5; 292 | } 293 | } 294 | return H; 295 | } 296 | 297 | const Real_ theta = sqrt(theta_sq); 298 | const Real_ coeff0 = sinc (theta, theta_sq), 299 | coeff1 = theta_cos_minus_sin_div_theta_cubed (theta, theta_sq), 300 | coeff2 = one_minus_cos_div_theta_sq (theta, theta_sq), 301 | coeff3 = two_cos_minus_2_plus_theta_sin_div_theta_pow_4 (theta, theta_sq), 302 | coeff4 = eight_plus_theta_sq_minus_eight_cos_minus_five_theta_sin_div_theta_pow_6(theta, theta_sq), 303 | coeff5 = three_theta_cos_plus_theta_sq_minus_3_sin_div_theta_pow_5 (theta, theta_sq); 304 | Mat3 w_cross = cross_product_matrix(w); 305 | 306 | // -(I otimes I) sinc(theta) 307 | for (int i = 0; i < 3; ++i) 308 | for (int k = 0; k < 3; ++k) 309 | H(i, i, k, k) = -coeff0; 310 | // (-I otimes w otimes w + w_cross otimes I) coeff1 311 | for (int i = 0; i < 3; ++i) { 312 | for (int k = 0; k < 3; ++k) { 313 | for (int l = 0; l < 3; ++l) { 314 | H(i, i, k, l) -= w[k] * w[l] * coeff1; 315 | H(i, k, l, l) += w_cross(i, k) * coeff1; 316 | } 317 | } 318 | } 319 | // ([e^i]_x otimes (e^i otimes w + w otimes e^i)) coeff1 320 | for (int k = 0; k < 3; ++k) { 321 | Real_ val = coeff1 * w[k]; 322 | H(1, 2, 0, k) += -val; H(2, 1, 0, k) += val; 323 | H(0, 2, 1, k) += val; H(2, 0, 1, k) += -val; 324 | H(0, 1, 2, k) += -val; H(1, 0, 2, k) += val; 325 | 326 | H(1, 2, k, 0) += -val; H(2, 1, k, 0) += val; 327 | H(0, 2, k, 1) += val; H(2, 0, k, 1) += -val; 328 | H(0, 1, k, 2) += -val; H(1, 0, k, 2) += val; 329 | } 330 | 331 | // (e^i otimes e^k otimes (e^i otimes e^k + e^k otimes e^i)) coeff2 332 | for (int i = 0; i < 3; ++i) { 333 | for (int k = 0; k < 3; ++k) { 334 | H(i, k, i, k) += coeff2; 335 | H(i, k, k, i) += coeff2; 336 | } 337 | } 338 | 339 | // ((e^i otimes w + w otimes e^i) otimes (e^i otimes w + w otimes e^i) + w otimes w otimes e^i otimes e^i) coeff3 340 | for (int i = 0; i < 3; ++i) { 341 | for (int j = 0; j < 3; ++j) { 342 | for (int k = 0; k < 3; ++k) { 343 | const Real_ val = coeff3 * w[j] * w[k]; 344 | H(i, j, i, k) += val; 345 | H(i, j, k, i) += val; 346 | H(j, i, i, k) += val; 347 | H(j, i, k, i) += val; 348 | 349 | H(j, k, i, i) += val; 350 | } 351 | } 352 | } 353 | 354 | // (w otimes w otimes w otimes w) coeff4 - ([w]_x otimes w otimes w) coeff5 355 | for (int i = 0; i < 3; ++i) 356 | for (int j = 0; j < 3; ++j) 357 | for (int k = 0; k < 3; ++k) 358 | for (int l = 0; l < 3; ++l) 359 | H(i, j, k, l) += (w[i] * w[j] * coeff4 - w_cross(i, j) * coeff5) * w[k] * w[l]; 360 | 361 | return H; 362 | } 363 | 364 | // The Hessian of R(w) v. This is a third order tensor: 365 | // H_ijk = d [R(w) v]_i / (dw_j dw_k) 366 | // We output the i^th slice of this tensor 367 | // (the Hessian of rotated vector component i) in hess_comp[i]. 368 | template 369 | void rotation_optimization:: 370 | hess_rotated_vector(const Vec3 &w, const Vec3 &v, 371 | std::array, 3> hess_comp) { 372 | const Real_ theta_sq = w.squaredNorm(); 373 | 374 | // Use simpler formula for variation around the identity 375 | // (But only if we're using a native floating point type; 376 | // for autodiff types, we need the full formula). 377 | if ((theta_sq == 0) && (std::is_arithmetic::value)) { 378 | const Vec3 half_v = 0.5 * v; 379 | for (int i = 0; i < 3; ++i) { 380 | // hess_comp[i] = -v[i] * I + 0.5 * (I.col(i) * v.transpose() + v * I.row(i)); 381 | hess_comp[i].setZero(); 382 | hess_comp[i].diagonal().array() = -v[i]; 383 | hess_comp[i].row(i) += half_v.transpose(); 384 | hess_comp[i].col(i) += half_v; 385 | } 386 | return; 387 | } 388 | 389 | const Real_ theta = sqrt(theta_sq); 390 | const Real_ w_dot_v = w.dot(v); 391 | const Real_ coeff0 = sinc (theta, theta_sq), 392 | coeff1 = theta_cos_minus_sin_div_theta_cubed (theta, theta_sq), 393 | coeff2 = one_minus_cos_div_theta_sq (theta, theta_sq), 394 | coeff3 = two_cos_minus_2_plus_theta_sin_div_theta_pow_4 (theta, theta_sq), 395 | coeff4 = w_dot_v * eight_plus_theta_sq_minus_eight_cos_minus_five_theta_sin_div_theta_pow_6(theta, theta_sq), 396 | coeff5 = three_theta_cos_plus_theta_sq_minus_3_sin_div_theta_pow_5 (theta, theta_sq); 397 | const Mat3 coeff1_v_cross = cross_product_matrix(coeff1 * v); 398 | const Vec3 v_cross_w = v.cross(w); 399 | const Vec3 term1 = coeff2 * v + (coeff3 * w_dot_v) * w; 400 | const Vec3 coeff3_w = coeff3 * w; 401 | Mat3 coeff3_w_otimes_v = coeff3_w * v.transpose(); 402 | for (int i = 0; i < 3; ++i) { 403 | // hess_comp[i] = (-coeff0 * v[i]) * I 404 | // - coeff1 * (v[i] * w_otimes_w + w * v_cross.row(i) + v_cross.row(i).transpose() * w.transpose() + v_cross_w[i] * I) 405 | // + coeff2 * (I.col(i) * v.transpose() + v * I.row(i)) 406 | // + coeff3 * (w_dot_v * (I.col(i) * w.transpose() + w * I.row(i) + w[i] * I) + w[i] * (w_otimes_v + w_otimes_v.transpose())) 407 | // + (coeff4 * w[i] + coeff5 * v_cross_w[i]) * w_otimes_w; 408 | hess_comp[i] = ((coeff4 * w[i] + coeff5 * v_cross_w[i] - coeff1 * v[i]) * w + coeff1_v_cross.col(i) + coeff3_w[i] * v) * w.transpose() 409 | - w * coeff1_v_cross.row(i) 410 | + w[i] * coeff3_w_otimes_v; 411 | hess_comp[i].col(i) += term1; 412 | hess_comp[i].row(i) += term1.transpose(); 413 | hess_comp[i].diagonal().array() += w_dot_v * coeff3_w[i] - coeff0 * v[i] - coeff1 * v_cross_w[i]; 414 | } 415 | } 416 | 417 | // d_i H_ijk where H_ijk is the Hessian of R(w) v. 418 | template 419 | Eigen::Matrix rotation_optimization:: 420 | d_contract_hess_rotated_vector(const Vec3 &w, const Vec3 &v, const Vec3 &d) { 421 | const Real_ theta_sq = w.squaredNorm(); 422 | 423 | Mat3 result; 424 | result.setZero(); 425 | 426 | // Use simpler formula for variation around the identity 427 | // (But only if we're using a native floating point type; 428 | // for autodiff types, we need the full formula). 429 | if ((theta_sq == 0) && (std::is_arithmetic::value)) { 430 | const Vec3 half_v = 0.5 * v; 431 | for (int i = 0; i < 3; ++i) { 432 | // hess_comp[i] = -v[i] * I + 0.5 * (I.col(i) * v.transpose() + v * I.row(i)); 433 | result.diagonal().array() += -v[i] * d[i]; 434 | result.row(i) += d[i] * half_v.transpose(); 435 | result.col(i) += d[i] * half_v; 436 | } 437 | return result; 438 | } 439 | 440 | const Real_ theta = sqrt(theta_sq); 441 | const Real_ w_dot_v = w.dot(v); 442 | const Real_ coeff0 = sinc (theta, theta_sq), 443 | coeff1 = theta_cos_minus_sin_div_theta_cubed (theta, theta_sq), 444 | coeff2 = one_minus_cos_div_theta_sq (theta, theta_sq), 445 | coeff3 = two_cos_minus_2_plus_theta_sin_div_theta_pow_4 (theta, theta_sq), 446 | coeff4 = w_dot_v * eight_plus_theta_sq_minus_eight_cos_minus_five_theta_sin_div_theta_pow_6(theta, theta_sq), 447 | coeff5 = three_theta_cos_plus_theta_sq_minus_3_sin_div_theta_pow_5 (theta, theta_sq); 448 | const Mat3 coeff1_v_cross = cross_product_matrix(coeff1 * v); 449 | const Vec3 v_cross_w = v.cross(w); 450 | const Vec3 term1 = coeff2 * v + (coeff3 * w_dot_v) * w; 451 | const Vec3 coeff3_w = coeff3 * w; 452 | Mat3 coeff3_w_otimes_v = coeff3_w * v.transpose(); 453 | for (int i = 0; i < 3; ++i) { 454 | result += d[i] * (((coeff4 * w[i] + coeff5 * v_cross_w[i] - coeff1 * v[i]) * w + coeff1_v_cross.col(i) + coeff3_w[i] * v) * w.transpose() 455 | - w * coeff1_v_cross.row(i) 456 | + w[i] * coeff3_w_otimes_v); 457 | result.col(i) += d[i] * term1; 458 | result.row(i) += d[i] * term1.transpose(); 459 | result.diagonal().array() += d[i] * (w_dot_v * coeff3_w[i] - coeff0 * v[i] - coeff1 * v_cross_w[i]); 460 | } 461 | return result; 462 | } 463 | 464 | // The Hessian of R(w) A for 3xN matrix A. This is a fourth order tensor: 465 | // H_ijkl = d [R(w) A]_ij / (dw_k dw_l) 466 | template 467 | template 468 | auto rotation_optimization::hess_rotated_matrix(const Vec3 &w, const Eigen::Matrix &A) -> Eigen::Tensor { 469 | int numCols = A.cols(); 470 | Eigen::Tensor H(3, numCols, 3, 3); 471 | const Real_ theta_sq = w.squaredNorm(); 472 | 473 | // Use simpler formula for variation around the identity 474 | // (But only if we're using a native floating point type; 475 | // for autodiff types, we need the full formula). 476 | Mat3 I(Mat3::Identity()); 477 | if ((theta_sq == 0) && (std::is_arithmetic::value)) { 478 | H.setZero(); 479 | for (int j = 0; j < numCols; ++j) { 480 | for (int i = 0; i < 3; ++i) { 481 | H(i, j, 0, 0) = H(i, j, 1, 1) = H(i, j, 2, 2) = -A(i, j); 482 | for (int k = 0; k < 3; ++k) { 483 | H(i, j, i, k) += 0.5 * A(k, j); 484 | H(i, j, k, i) += 0.5 * A(k, j); 485 | } 486 | } 487 | } 488 | return H; 489 | } 490 | 491 | const Real_ theta = sqrt(theta_sq); 492 | const Real_ coeff0 = sinc (theta, theta_sq), 493 | coeff1 = theta_cos_minus_sin_div_theta_cubed (theta, theta_sq), 494 | coeff2 = one_minus_cos_div_theta_sq (theta, theta_sq), 495 | coeff3 = two_cos_minus_2_plus_theta_sin_div_theta_pow_4 (theta, theta_sq), 496 | coeff4 = eight_plus_theta_sq_minus_eight_cos_minus_five_theta_sin_div_theta_pow_6(theta, theta_sq), 497 | coeff5 = three_theta_cos_plus_theta_sq_minus_3_sin_div_theta_pow_5 (theta, theta_sq); 498 | H.setZero(); 499 | for (int j = 0; j < numCols; ++j) { // Compute the Hessian of each rotated column vector of A 500 | const auto &v = A.col(j); 501 | const Real_ w_dot_v = w.dot(v); 502 | Mat3 v_cross = cross_product_matrix(v); 503 | Vec3 v_cross_w = v.cross(w); 504 | for (int i = 0; i < 3; ++i) { // Compute the Hessian of each component of the rotated column vector. 505 | H(i, j, 0, 0) = H(i, j, 1, 1) = H(i, j, 2, 2) = -coeff0 * v[i] - coeff1 * v_cross_w[i] + coeff3 * w_dot_v * w[i]; // Identity coefficients 506 | for (int k = 0; k < 3; ++k) { 507 | for (int l = 0; l < 3; ++l) { 508 | const Real_ tmp = coeff3 * w[i] * w[k] * v[l] - coeff1 * (w[k] * v_cross(i, l)); 509 | H(i, j, k, l) += tmp + (w_dot_v * coeff4 * w[i] + coeff5 * v_cross_w[i] - coeff1 * v[i]) * w[k] * w[l]; 510 | H(i, j, l, k) += tmp; 511 | } 512 | const Real_ tmp = coeff2 * v[k] + coeff3 * w_dot_v * w[k]; 513 | H(i, j, i, k) += tmp; 514 | H(i, j, k, i) += tmp; 515 | } 516 | } 517 | } 518 | return H; 519 | } 520 | 521 | #endif /* end of include guard: ROTATION_OPTIMIZATION_INL_HH */ 522 | --------------------------------------------------------------------------------