├── minkindr_python ├── python │ └── minkindr │ │ └── __init__.py ├── dependencies.rosinstall ├── setup.py ├── src │ ├── module.cpp │ ├── export-transformation.cc │ └── export-rotation-quaternion.cc ├── package.xml └── CMakeLists.txt ├── .gitignore ├── dependencies.rosinstall ├── minkindr ├── package.xml ├── CMakeLists.txt ├── test │ ├── test-sim3.cc │ ├── test-main.cc │ ├── test-transformation-2d.cc │ ├── test-rotation.cc │ └── test-transformation.cc └── include │ └── kindr │ └── minimal │ ├── position.h │ ├── quat-sim-transform.h │ ├── common.h │ ├── transform-2d.h │ ├── implementation │ ├── quat-sim-transform-inl.h │ ├── transform-2d-inl.h │ ├── angle-axis-inl.h │ ├── quat-transformation-inl.h │ └── rotation-quaternion-inl.h │ ├── angle-axis.h │ ├── quat-transformation.h │ └── rotation-quaternion.h ├── LICENSE └── README.md /minkindr_python/python/minkindr/__init__.py: -------------------------------------------------------------------------------- 1 | import numpy_eigen 2 | 3 | from .libminkindr_python import * 4 | -------------------------------------------------------------------------------- /minkindr_python/dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_boost_python_buildtool 3 | uri: https://github.com/ethz-asl/catkin_boost_python_buildtool 4 | - git: 5 | local-name: numpy_eigen 6 | uri: https://github.com/ethz-asl/numpy_eigen.git 7 | -------------------------------------------------------------------------------- /minkindr_python/setup.py: -------------------------------------------------------------------------------- 1 | 2 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 3 | 4 | from distutils.core import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | # fetch values from package.xml 8 | setup_args = generate_distutils_setup( 9 | packages=['minkindr'], 10 | package_dir={'':'python'}) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /minkindr_python/src/module.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | using namespace boost::python; 4 | 5 | void exportRotationQuaternion(); 6 | void exportTransformation(); 7 | 8 | BOOST_PYTHON_MODULE(libminkindr_python) 9 | { 10 | exportRotationQuaternion(); 11 | exportTransformation(); 12 | } 13 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | *~ 23 | 24 | # Eclipse project files 25 | .project 26 | .cproject 27 | .settings 28 | *~ 29 | 30 | # Backup files 31 | *.orig 32 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: https://github.com/catkin/catkin_simple.git 4 | - git: 5 | local-name: gflags_catkin 6 | uri: https://github.com/ethz-asl/gflags_catkin 7 | - git: 8 | local-name: glog_catkin 9 | uri: https://github.com/ethz-asl/glog_catkin 10 | - git: 11 | local-name: eigen_catkin 12 | uri: https://github.com/ethz-asl/eigen_catkin.git 13 | - git: 14 | local-name: eigen_checks 15 | uri: https://github.com/ethz-asl/eigen_checks.git 16 | -------------------------------------------------------------------------------- /minkindr/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | minkindr 4 | 1.0.0 5 | minkindr 6 | 7 | Paul Furgale 8 | BSD 9 | 10 | catkin 11 | catkin_simple 12 | 13 | eigen_catkin 14 | glog_catkin 15 | eigen_checks 16 | 17 | 18 | -------------------------------------------------------------------------------- /minkindr_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | minkindr_python 4 | 1.0.0 5 | Python exports for minkindr. 6 | 7 | Mathias Buerki 8 | New BSD 9 | catkin 10 | catkin_simple 11 | 12 | catkin_boost_python_buildtool 13 | glog_catkin 14 | minkindr 15 | numpy_eigen 16 | 17 | 18 | -------------------------------------------------------------------------------- /minkindr/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(minkindr) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | find_package(Threads) 8 | 9 | catkin_add_gtest(${PROJECT_NAME}_tests 10 | test/test-main.cc 11 | test/test-rotation.cc 12 | test/test-sim3.cc 13 | test/test-transformation.cc 14 | test/test-transformation-2d.cc 15 | ) 16 | target_link_libraries(${PROJECT_NAME}_tests ${catkin_LIBRARIES} 17 | ${CMAKE_THREAD_LIBS_INIT}) 18 | 19 | # Add all files to show up in QtCreator. 20 | FILE(GLOB_RECURSE LibFiles "include/*") 21 | add_custom_target(headers SOURCES ${LibFiles}) 22 | 23 | cs_install() 24 | cs_export() 25 | 26 | -------------------------------------------------------------------------------- /minkindr_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(minkindr_python) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | set (CMAKE_CXX_FLAGS "-Wall ${CMAKE_CXX_FLAGS}") 8 | 9 | # Enable C++11 support. 10 | if (CMAKE_VERSION VERSION_LESS "3.1") 11 | if (MSVC) 12 | message(FATAL_ERROR "CMake version 3.1 or later is required to compiler ${PROJECT_NAME} with Microsoft Visual C++") 13 | endif () 14 | if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 15 | set (CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") 16 | else () 17 | set (CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 18 | endif () 19 | else () 20 | set (CMAKE_CXX_STANDARD 11) 21 | endif () 22 | 23 | # Set up the python exports. 24 | SET(PY_PROJECT_NAME minkindr_python) 25 | SET(PY_PACKAGE_DIR python/minkindr) 26 | 27 | add_python_export_library(${PY_PROJECT_NAME} ${PY_PACKAGE_DIR} 28 | src/module.cpp 29 | src/export-rotation-quaternion.cc 30 | src/export-transformation.cc 31 | ) 32 | 33 | cs_install() 34 | cs_export() 35 | -------------------------------------------------------------------------------- /minkindr_python/src/export-transformation.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace boost::python; 5 | 6 | using Transformation = kindr::minimal::QuatTransformationTemplate; 7 | 8 | Eigen::Vector3d getPosition(const Transformation& transformation) { 9 | return transformation.getPosition(); 10 | } 11 | 12 | Transformation::Rotation getRotation(const Transformation& transformation) { 13 | return transformation.getRotation(); 14 | } 15 | 16 | void exportTransformation() { 17 | using namespace boost::python; 18 | 19 | class_< Transformation, boost::shared_ptr>("Transformation", init<>()) 20 | .def(init()) 21 | .def(init()) 22 | .def("getTransformationMatrix", &Transformation::getTransformationMatrix) 23 | .def("getRotation", getRotation) 24 | .def("getRotationMatrix", &Transformation::getRotationMatrix) 25 | .def("getPosition", getPosition) 26 | .def("inverse", &Transformation::inverse) 27 | .def(self * self) 28 | ; 29 | 30 | def("interpolateLinearly", &kindr::minimal::interpolateComponentwise); 31 | } 32 | -------------------------------------------------------------------------------- /minkindr/test/test-sim3.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "kindr/minimal/quat-sim-transform.h" 5 | 6 | #ifndef TEST 7 | #define TEST(a, b) int Test_##a##_##b() 8 | #endif 9 | 10 | namespace kindr { 11 | namespace minimal { 12 | 13 | TEST(QuatSimTransformTest, Transform) { 14 | Eigen::Matrix3Xd input(3, 1); 15 | input << 1., 1., 0.; 16 | Eigen::Matrix4d transformation_matrix; 17 | transformation_matrix << 1, 0, 0, 0, 18 | 0, 0, -1, 1, 19 | 0, 1, 0, 0, 20 | 0, 0, 0, 1; 21 | const QuatSimTransform sim3(QuatTransformation( 22 | transformation_matrix), 2.0); 23 | Eigen::Matrix3Xd expected_output(3, 1); 24 | expected_output << 2., 1., 2.; 25 | 26 | EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(sim3 * input, expected_output)); 27 | } 28 | 29 | TEST(QuatSimTransformTest, Inverse) { 30 | const RotationQuaternion q(Eigen::Quaterniond( 31 | 0.64491714, 0.26382416, 0.51605132, 0.49816637)); 32 | const Eigen::Vector3d t(4.67833851, 8.52053031, 6.71796159 ); 33 | const QuatSimTransform sim3(QuatTransformation(q, t), M_PI); 34 | 35 | // 3Xd intended - it's the input type to operator*. 36 | const Eigen::Matrix3Xd input = Eigen::Matrix3d::Random(); 37 | 38 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 39 | sim3 * (sim3.inverse() * input), input, 1e-6)); 40 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 41 | sim3.inverse() * (sim3 * input), input, 1e-6)); 42 | } 43 | 44 | } // namespace minimal 45 | } // namespace kindr 46 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of ETH Zurich nor the names of its contributors may 12 | be used to endorse or promote products derived from this software 13 | without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, 19 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 20 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 21 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 22 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 23 | OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 24 | OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Transformations simple. A minimal library for transformations, following the kindr interface. Uses active quaternions of rotation in Hamilton notation. 2 | 3 | Licensed under the 3-clause BSD license ("New BSD"). 4 | 5 | ``` 6 | Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 7 | All rights reserved. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions are met: 11 | * Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | * Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | * Neither the name of the nor the 17 | names of its contributors may be used to endorse or promote products 18 | derived from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | ``` -------------------------------------------------------------------------------- /minkindr/test/test-main.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #include 26 | 27 | /// Run all the tests that were declared with TEST() 28 | int main(int argc, char **argv) { 29 | testing::InitGoogleTest(&argc, argv); 30 | return RUN_ALL_TESTS(); 31 | } 32 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/position.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MINIMAL_POSITION_H 26 | #define KINDR_MINIMAL_POSITION_H 27 | 28 | #include 29 | 30 | namespace kindr { 31 | namespace minimal { 32 | 33 | template 34 | using PositionTemplate = Eigen::Matrix; 35 | 36 | typedef PositionTemplate Position; 37 | 38 | } // namespace minimal 39 | } // namespace kindr 40 | 41 | #endif /* KINDR_MINIMAL_POSITION_H */ 42 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/quat-sim-transform.h: -------------------------------------------------------------------------------- 1 | #ifndef KINDR_MINIMAL_QUAT_SIM_TRANSFORM_H_ 2 | #define KINDR_MINIMAL_QUAT_SIM_TRANSFORM_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "kindr/minimal/quat-transformation.h" 9 | 10 | namespace kindr { 11 | namespace minimal { 12 | 13 | // Scale & rotate then translate = scale then transform. 14 | // In particular, the transformation matrix is: 15 | // 16 | // R*s t R t s*I 0 17 | // 0 1 = 0 1 * 0 1 18 | template 19 | class QuatSimTransformTemplate { 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | typedef QuatTransformationTemplate Transform; 24 | typedef QuatSimTransformTemplate Sim3; 25 | typedef Eigen::Matrix Vector3; 26 | typedef Eigen::Matrix Vector7; 27 | typedef Eigen::Matrix Matrix3X; 28 | 29 | // Creates identity similarity transform. 30 | QuatSimTransformTemplate(); 31 | 32 | QuatSimTransformTemplate(const Transform& T_A_B, const Scalar scale_A_B); 33 | 34 | QuatSimTransformTemplate(const Vector7& log_vector); 35 | 36 | inline Vector3 operator*(const Vector3& rhs) const; 37 | 38 | // Vectorized, applies operator * to each column vector. 39 | inline Matrix3X operator*(const Matrix3X& rhs) const; 40 | 41 | inline Sim3 operator*(const Sim3& rhs) const; 42 | 43 | inline Sim3 operator*(const Transform& rhs) const; 44 | 45 | inline Sim3 inverse() const; 46 | 47 | inline Vector7 log() const; 48 | 49 | inline Eigen::Matrix getTransformationMatrix() const; 50 | inline const Transform& getTransform() const { return T_A_B_; } 51 | inline Scalar getScale() const { return scale_A_B_; } 52 | 53 | inline void setScale(const Scalar scale_A_B) { scale_A_B_ = scale_A_B; } 54 | 55 | private: 56 | Transform T_A_B_; 57 | Scalar scale_A_B_; 58 | 59 | template 60 | friend std::ostream & operator<<( 61 | std::ostream &, const QuatSimTransformTemplate&); 62 | }; 63 | 64 | typedef QuatSimTransformTemplate QuatSimTransform; 65 | 66 | template 67 | inline QuatSimTransformTemplate operator*( 68 | const QuatTransformationTemplate& lhs, 69 | const QuatSimTransformTemplate& rhs); 70 | 71 | template 72 | std::ostream & operator<<(std::ostream & out, 73 | const QuatSimTransformTemplate& sim_3); 74 | 75 | } // namespace minimal 76 | } // namespace kindr 77 | 78 | #include "kindr/minimal/implementation/quat-sim-transform-inl.h" 79 | 80 | #endif // KINDR_MINIMAL_QUAT_SIM_TRANSFORM_H_ 81 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/common.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef MINKINDR_MINIMAL_COMMON_H 26 | #define MINKINDR_MINIMAL_COMMON_H 27 | 28 | #include 29 | #include 30 | 31 | namespace kindr { 32 | namespace minimal { 33 | 34 | inline void skewMatrix(const Eigen::Vector3d& v, Eigen::Matrix3d * skew) { 35 | CHECK_NOTNULL(skew); 36 | skew->setZero(); 37 | (*skew)(0,1) = -v[2]; 38 | (*skew)(1,0) = v[2]; 39 | (*skew)(0,2) = v[1]; 40 | (*skew)(2,0) = -v[1]; 41 | (*skew)(1,2) = -v[0]; 42 | (*skew)(2,1) = v[0]; 43 | } 44 | 45 | inline void skewMatrix(const Eigen::Vector3d& v, Eigen::Map * skew) { 46 | CHECK_NOTNULL(skew); 47 | skew->setZero(); 48 | (*skew)(0,1) = -v[2]; 49 | (*skew)(1,0) = v[2]; 50 | (*skew)(0,2) = v[1]; 51 | (*skew)(2,0) = -v[1]; 52 | (*skew)(1,2) = -v[0]; 53 | (*skew)(2,1) = v[0]; 54 | } 55 | 56 | inline Eigen::Matrix3d skewMatrix(const Eigen::Vector3d& v) { 57 | Eigen::Matrix3d skew; 58 | skewMatrix(v, &skew); 59 | return skew; 60 | } 61 | 62 | } // namespace minimal 63 | } // namespace kindr 64 | 65 | #endif // MINKINDR_MINIMAL_COMMON_H 66 | -------------------------------------------------------------------------------- /minkindr_python/src/export-rotation-quaternion.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace boost::python; 5 | 6 | typedef kindr::minimal::RotationQuaternionTemplate Quaternion; 7 | 8 | Eigen::Vector4d getQuaternionXYZW(const Quaternion& quaternion) { 9 | return Eigen::Vector4d( 10 | quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w()); 11 | } 12 | 13 | Eigen::Vector4d getQuaternionWXYZ(const Quaternion& quaternion) { 14 | return Eigen::Vector4d( 15 | quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); 16 | } 17 | 18 | 19 | Quaternion createQuaternionFromXYZW(const Eigen::Vector4d& xyzw) { 20 | return Quaternion(xyzw(3), xyzw(0), xyzw(1), xyzw(2)); 21 | } 22 | 23 | Quaternion createQuaternionFromWXYZ(const Eigen::Vector4d& wxyz) { 24 | return Quaternion(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); 25 | } 26 | 27 | Quaternion createQuaternionFromApproximateRotationMatrix( 28 | const Eigen::Matrix3d& R) { 29 | return Quaternion::constructAndRenormalize(R); 30 | } 31 | 32 | Quaternion createQuaternionFromRotationVectorRads(const Eigen::Vector3d& rotation_vector_xyz_rads) { 33 | return Quaternion(rotation_vector_xyz_rads); 34 | } 35 | 36 | Eigen::Vector3d getRotationVector(const Quaternion& quaternion) { 37 | const kindr::minimal::AngleAxis angle_axis(quaternion); 38 | return angle_axis.axis() * angle_axis.angle(); 39 | } 40 | 41 | void normalize(Quaternion& quaternion) { 42 | quaternion.normalize(); 43 | } 44 | 45 | void exportRotationQuaternion() { 46 | using namespace boost::python; 47 | 48 | class_< Quaternion, boost::shared_ptr >( "Quaternion", init<>() ) 49 | .def(init()) 50 | .def(init( 51 | "Quaternion(w, x, y, z)")) 52 | .def("w", &Quaternion::w) 53 | .def("x", &Quaternion::x) 54 | .def("y", &Quaternion::y) 55 | .def("z", &Quaternion::z) 56 | .def("getRotationMatrix", &Quaternion::getRotationMatrix) 57 | .def("getQuaternionWXYZ", getQuaternionWXYZ) 58 | .def("getQuaternionXYZW", getQuaternionXYZW) 59 | .def("inverse", &Quaternion::inverse) 60 | .def("getRotationVector", getRotationVector) 61 | .def("normalize", normalize) 62 | .def(self * self) 63 | ; 64 | 65 | def("createQuaternionFromXYZW", createQuaternionFromXYZW, 66 | "Creates a Quaternion from a 4D vector [x,y,z,w]."); 67 | def("createQuaternionFromWXYZ", createQuaternionFromWXYZ, 68 | "Creates a Quaternion from a 4D vector [w, x,y,z]."); 69 | def("createQuaternionFromApproximateRotationMatrix", 70 | createQuaternionFromApproximateRotationMatrix, 71 | "Creates a quaternion from an (approximate) numpy 3x3 rotation matrix."); 72 | def("createQuaternionFromRotationVectorRads", 73 | createQuaternionFromRotationVectorRads, 74 | "Creates a quaternion from a rotation vector [x, y, z] in radians."); 75 | } 76 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/transform-2d.h: -------------------------------------------------------------------------------- 1 | #ifndef KINDR_MINIMAL_TRANSFORM_2D_H_ 2 | #define KINDR_MINIMAL_TRANSFORM_2D_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace kindr { 10 | namespace minimal { 11 | 12 | template 13 | using Position2DTemplate = Eigen::Matrix; 14 | 15 | template 16 | using Rotation2DTemplate = Eigen::Rotation2D; 17 | 18 | template 19 | class Transformation2DTemplate { 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | using Vector2 = Eigen::Matrix; 24 | using Matrix2X = Eigen::Matrix; 25 | 26 | using Rotation = Rotation2DTemplate; 27 | using Position = Position2DTemplate; 28 | using RotationMatrix = Eigen::Matrix; 29 | using TransformationMatrix = Eigen::Matrix; 30 | 31 | Transformation2DTemplate(); 32 | 33 | Transformation2DTemplate(const Rotation r_A_B, const Position& A_t_A_B); 34 | 35 | explicit Transformation2DTemplate(const TransformationMatrix& T); 36 | 37 | ~Transformation2DTemplate(); 38 | 39 | void setIdentity(); 40 | 41 | // Non-const getter for the position vector. 42 | Position& getPosition(); 43 | 44 | // Const getter for the position vector. 45 | const Position& getPosition() const; 46 | 47 | // Non-const getter for the rotation. Setting a new rotation angle can be done 48 | // as follows: 49 | // 50 | // Transformation2D T; 51 | // T.getRotation().angle() = new_angle; 52 | Rotation& getRotation(); 53 | 54 | // Const getter for the rotation. 55 | const Rotation& getRotation() const; 56 | 57 | RotationMatrix getRotationMatrix() const; 58 | 59 | TransformationMatrix getTransformationMatrix() const; 60 | 61 | // Get the rotation angle and the position as a vector: [angle, x, y] 62 | Eigen::Matrix asVector() const; 63 | 64 | // Compose two transformations. 65 | Transformation2DTemplate operator*( 66 | const Transformation2DTemplate& rhs) const; 67 | 68 | // Transform a point. 69 | Vector2 operator*(const Vector2& rhs) const; 70 | 71 | // Transform a point. 72 | Vector2 transform(const Vector2& rhs) const; 73 | 74 | // Transform points. 75 | Matrix2X transformVectorized(const Matrix2X& rhs) const; 76 | 77 | // Returns a copy of the inverted transformation. 78 | Transformation2DTemplate inverse() const; 79 | 80 | // Check binary equality. 81 | bool operator==(const Transformation2DTemplate& rhs) const; 82 | 83 | // Check binary inequality. 84 | bool operator!=(const Transformation2DTemplate& rhs) const; 85 | 86 | // Cast scalar elements to another type. 87 | template 88 | Transformation2DTemplate cast() const; 89 | 90 | private: 91 | // The rotation that takes vectors from B to A. 92 | // 93 | // A_v = r_A_B * B_v; 94 | Rotation r_A_B_; 95 | 96 | // The vector from the origin of A to the origin of B, expressed in A. 97 | Position A_t_A_B_; 98 | }; 99 | 100 | using Position2D = Position2DTemplate; 101 | using Rotation2D = Rotation2DTemplate; 102 | using Transformation2D = Transformation2DTemplate; 103 | 104 | template 105 | std::ostream & operator<<(std::ostream & out, 106 | const Transformation2DTemplate& rhs); 107 | 108 | } // namespace minimal 109 | } // namespace kindr 110 | 111 | #include "kindr/minimal/implementation/transform-2d-inl.h" 112 | 113 | #endif // KINDR_MINIMAL_TRANSFORM_2D_H_ 114 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/implementation/quat-sim-transform-inl.h: -------------------------------------------------------------------------------- 1 | #ifndef KINDR_MINIMAL_IMPLEMENTATION_QUAT_SIM_TRANSFORM_INL_H_ 2 | #define KINDR_MINIMAL_IMPLEMENTATION_QUAT_SIM_TRANSFORM_INL_H_ 3 | 4 | #include 5 | 6 | namespace kindr { 7 | namespace minimal { 8 | 9 | template 10 | QuatSimTransformTemplate::QuatSimTransformTemplate() : scale_A_B_(1.) {} 11 | 12 | template 13 | QuatSimTransformTemplate::QuatSimTransformTemplate( 14 | const Transform& T_A_B, const Scalar scale_A_B) 15 | : T_A_B_(T_A_B), scale_A_B_(scale_A_B) { 16 | CHECK_GT(scale_A_B, 0.); 17 | } 18 | 19 | template 20 | QuatSimTransformTemplate::QuatSimTransformTemplate( 21 | const Vector7& log_vector) 22 | : T_A_B_(Eigen::Matrix(log_vector.template head<6>())), 23 | scale_A_B_(log_vector(6)) { 24 | CHECK_GT(scale_A_B_, 0.); 25 | } 26 | 27 | template 28 | typename QuatSimTransformTemplate::Vector3 29 | QuatSimTransformTemplate::operator*(const Vector3& rhs) const { 30 | return T_A_B_ * (scale_A_B_ * rhs); 31 | } 32 | 33 | template 34 | typename QuatSimTransformTemplate::Matrix3X 35 | QuatSimTransformTemplate::operator*(const Matrix3X& rhs) const { 36 | return T_A_B_.transformVectorized(scale_A_B_ * rhs); 37 | } 38 | 39 | template 40 | QuatSimTransformTemplate QuatSimTransformTemplate::operator*( 41 | const QuatSimTransformTemplate& rhs) const { 42 | // Rl*sl tl Rr*sr tr Rl*sl*Rr*sr Rl*sl*tr+tl 43 | // 0 1 * 0 1 = 0 1 44 | // 45 | // -> R = Rl*Rr; t = Rl*sl*tr+tl = Siml*tr; s = sl*sr 46 | return QuatSimTransformTemplate( 47 | QuatTransformationTemplate( 48 | T_A_B_.getRotation() * rhs.T_A_B_.getRotation(), 49 | (*this) * rhs.T_A_B_.getPosition()), 50 | scale_A_B_ * rhs.scale_A_B_); 51 | } 52 | 53 | template 54 | QuatSimTransformTemplate QuatSimTransformTemplate::operator*( 55 | const QuatTransformationTemplate& rhs) const { 56 | return operator*(QuatSimTransformTemplate(rhs, 1.)); 57 | } 58 | 59 | template 60 | QuatSimTransformTemplate 61 | QuatSimTransformTemplate::inverse() const { 62 | return QuatSimTransformTemplate( 63 | QuatTransformationTemplate( 64 | T_A_B_.getRotation().inverse(), -T_A_B_.getRotation().inverseRotate( 65 | T_A_B_.getPosition() / scale_A_B_)), 1. / scale_A_B_); 66 | } 67 | 68 | template 69 | inline Eigen::Matrix 70 | QuatSimTransformTemplate::log() const { 71 | Eigen::Matrix result; 72 | result << T_A_B_.log(), scale_A_B_; 73 | return result; 74 | } 75 | 76 | template 77 | Eigen::Matrix 78 | QuatSimTransformTemplate::getTransformationMatrix() const { 79 | Eigen::Matrix result = T_A_B_.getTransformationMatrix(); 80 | result.template topLeftCorner<3, 3>() *= scale_A_B_; 81 | return result; 82 | } 83 | 84 | template 85 | inline QuatSimTransformTemplate operator*( 86 | const QuatTransformationTemplate& lhs, 87 | const QuatSimTransformTemplate& rhs) { 88 | return QuatSimTransformTemplate(lhs, 1.) * rhs; 89 | } 90 | 91 | template 92 | std::ostream & operator<<(std::ostream & out, 93 | const QuatSimTransformTemplate& sim_3) { 94 | out << "Transform:" << std::endl << sim_3.T_A_B_.getTransformationMatrix() << 95 | std::endl; 96 | out << "Scale: " << sim_3.scale_A_B_; 97 | return out; 98 | } 99 | 100 | } // namespace minimal 101 | } // namespace kindr 102 | 103 | #endif // KINDR_MINIMAL_IMPLEMENTATION_QUAT_SIM_TRANSFORM_INL_H_ 104 | -------------------------------------------------------------------------------- /minkindr/test/test-transformation-2d.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "kindr/minimal/transform-2d.h" 9 | 10 | #ifndef TEST 11 | #define TEST(a, b) int Test_##a##_##b() 12 | #endif 13 | 14 | namespace kindr { 15 | namespace minimal { 16 | 17 | constexpr double kEpsilon = 1e-10; 18 | static const Rotation2D r(0.77); 19 | static const Position2D t(1.23597132, -2.247102); 20 | static Eigen::Vector2d v(6.26257419, 1.58356548); 21 | 22 | TEST(TestTransformation2D, TestInitialization) { 23 | const Transformation2D T; 24 | EXPECT_NEAR(T.getRotation().angle(), 0.0, kEpsilon); 25 | EXPECT_TRUE( 26 | EIGEN_MATRIX_NEAR(T.getPosition(), Eigen::Vector2d(0.0, 0.0), kEpsilon)); 27 | 28 | const Transformation2D T_rot_pos(r, t); 29 | EXPECT_NEAR(T_rot_pos.getRotation().angle(), r.angle(), kEpsilon); 30 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(T_rot_pos.getPosition(), t, kEpsilon)); 31 | 32 | const Eigen::Matrix transformation_matrix = 33 | (Eigen::Matrix() << std::cos(r.angle()), 34 | -std::sin(r.angle()), t.x(), std::sin(r.angle()), std::cos(r.angle()), 35 | t.y(), 0.0, 0.0, 1.0) 36 | .finished(); 37 | const Transformation2D T_from_mat(transformation_matrix); 38 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 39 | transformation_matrix, T_from_mat.getTransformationMatrix(), kEpsilon)); 40 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 41 | transformation_matrix, T_rot_pos.getTransformationMatrix(), kEpsilon)); 42 | 43 | const Eigen::Matrix invalid_transformation_matrix = 44 | (Eigen::Matrix() << 1.0, 1.0, t.x(), 1.0, 1.0, t.y(), 0.0, 45 | 0.0, 1.1) 46 | .finished(); 47 | EXPECT_DEATH( 48 | const Transformation2D T_invalid(invalid_transformation_matrix), ""); 49 | } 50 | 51 | TEST(TestTransformation2D, TestGettersAndSetters) { 52 | Transformation2D T; 53 | T.getRotation() = r; 54 | T.getPosition() = t; 55 | EXPECT_NEAR(T.getRotation().angle(), r.angle(), kEpsilon); 56 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(T.getPosition(), t, kEpsilon)); 57 | 58 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 59 | T.asVector(), (Eigen::Vector3d() << r.angle(), t).finished(), kEpsilon)); 60 | 61 | constexpr double angle_new = 3.1415; 62 | T.getRotation().angle() = angle_new; 63 | EXPECT_NEAR(T.getRotation().angle(), angle_new, kEpsilon); 64 | } 65 | 66 | Eigen::Vector2d fromHomogeneous(const Eigen::Vector3d& v) { 67 | return v.head<2>() / v[2]; 68 | } 69 | 70 | Eigen::Vector3d toHomogeneous(const Eigen::Vector2d& v) { 71 | return (Eigen::Vector3d() << v, 1.0).finished(); 72 | } 73 | 74 | TEST(TestTransformation2D, TestComposition) { 75 | const Eigen::Vector3d vh = toHomogeneous(v); 76 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(v, fromHomogeneous(vh), kEpsilon)); 77 | 78 | const Transformation2D T(r, t); 79 | const Eigen::Matrix T_mat = T.getTransformationMatrix(); 80 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(T * v, fromHomogeneous(T_mat * vh), kEpsilon)); 81 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 82 | T * T * v, fromHomogeneous(T_mat * T_mat * vh), kEpsilon)); 83 | 84 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 85 | T.inverse().getTransformationMatrix(), T_mat.inverse().eval(), kEpsilon)); 86 | 87 | const Eigen::MatrixXd v_vectorized = Eigen::MatrixXd::Random(2, 10); 88 | const Eigen::MatrixXd v_result = T.transformVectorized(v_vectorized); 89 | for (int i = 0; i < v_vectorized.cols(); ++i) { 90 | EXPECT_TRUE( 91 | EIGEN_MATRIX_NEAR(v_result.col(i), T * v_vectorized.col(i), kEpsilon)); 92 | } 93 | } 94 | 95 | TEST(TestTransformation2D, TestCast) { 96 | const Transformation2D T_double(r, t); 97 | const Transformation2DTemplate T_float = 98 | T_double.template cast(); 99 | 100 | EXPECT_NEAR( 101 | T_double.getRotation().angle(), 102 | static_cast(T_float.getRotation().angle()), 1e-5); 103 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 104 | T_double.getPosition(), T_float.getPosition().cast(), 1e-5)); 105 | } 106 | 107 | } // namespace minimal 108 | } // namespace kindr 109 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/implementation/transform-2d-inl.h: -------------------------------------------------------------------------------- 1 | #ifndef KINDR_MINIMAL_IMPLEMENTATION_TRANSFORM_2D_INL_H_ 2 | #define KINDR_MINIMAL_IMPLEMENTATION_TRANSFORM_2D_INL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace kindr { 10 | namespace minimal { 11 | 12 | template 13 | Transformation2DTemplate::Transformation2DTemplate() { 14 | setIdentity(); 15 | } 16 | 17 | template 18 | Transformation2DTemplate::Transformation2DTemplate( 19 | const Rotation r_A_B, const Position& A_t_A_B) 20 | : r_A_B_(r_A_B), A_t_A_B_(A_t_A_B) {} 21 | 22 | template 23 | Transformation2DTemplate::Transformation2DTemplate( 24 | const TransformationMatrix& T) 25 | : Transformation2DTemplate( 26 | Rotation2D().fromRotationMatrix(T.template topLeftCorner<2, 2>().eval()), 27 | T.template topRightCorner<2, 1>().eval()) { 28 | constexpr Scalar kEpsilon = std::numeric_limits::epsilon(); 29 | CHECK_LE((T(2, 2) - static_cast(1.0)), kEpsilon); 30 | const Eigen::Matrix rotation_matrix = 31 | T.template topLeftCorner<2, 2>().eval(); 32 | CHECK_NEAR(rotation_matrix.determinant(), static_cast(1.0), kEpsilon); 33 | } 34 | 35 | template 36 | Transformation2DTemplate::~Transformation2DTemplate() {} 37 | 38 | template 39 | void Transformation2DTemplate::setIdentity() { 40 | r_A_B_ = Rotation2D(static_cast(0.0)); 41 | A_t_A_B_.setZero(); 42 | } 43 | 44 | template 45 | typename Transformation2DTemplate::Position& 46 | Transformation2DTemplate::getPosition() { 47 | return A_t_A_B_; 48 | } 49 | 50 | template 51 | const typename Transformation2DTemplate::Position& 52 | Transformation2DTemplate::getPosition() const { 53 | return A_t_A_B_; 54 | } 55 | 56 | template 57 | typename Transformation2DTemplate::Rotation& 58 | Transformation2DTemplate::getRotation() { 59 | return r_A_B_; 60 | } 61 | 62 | template 63 | const typename Transformation2DTemplate::Rotation& 64 | Transformation2DTemplate::getRotation() const { 65 | return r_A_B_; 66 | } 67 | 68 | template 69 | typename Transformation2DTemplate::RotationMatrix 70 | Transformation2DTemplate::getRotationMatrix() const { 71 | return r_A_B_.toRotationMatrix(); 72 | } 73 | 74 | template 75 | typename Transformation2DTemplate::TransformationMatrix 76 | Transformation2DTemplate::getTransformationMatrix() const { 77 | TransformationMatrix transformation_matrix; 78 | transformation_matrix.template topLeftCorner<2, 2>() = 79 | r_A_B_.toRotationMatrix(); 80 | transformation_matrix.template topRightCorner<2, 1>() = A_t_A_B_; 81 | transformation_matrix.template bottomRows<1>() = 82 | (Eigen::Matrix() << static_cast(0.0), 83 | static_cast(0.0), static_cast(1.0)) 84 | .finished(); 85 | return transformation_matrix; 86 | } 87 | 88 | template 89 | Eigen::Matrix Transformation2DTemplate::asVector() const { 90 | return (Eigen::Matrix() << r_A_B_.angle(), A_t_A_B_).finished(); 91 | } 92 | 93 | template 94 | Transformation2DTemplate Transformation2DTemplate::operator*( 95 | const Transformation2DTemplate& rhs) const { 96 | return Transformation2DTemplate( 97 | r_A_B_ * rhs.r_A_B_, A_t_A_B_ + r_A_B_ * rhs.A_t_A_B_); 98 | } 99 | 100 | template 101 | typename Transformation2DTemplate::Vector2 102 | Transformation2DTemplate::operator*(const Vector2& rhs) const { 103 | return transform(rhs); 104 | } 105 | 106 | template 107 | typename Transformation2DTemplate::Vector2 108 | Transformation2DTemplate::transform(const Vector2& rhs) const { 109 | return r_A_B_ * rhs + A_t_A_B_; 110 | } 111 | 112 | template 113 | typename Transformation2DTemplate::Matrix2X 114 | Transformation2DTemplate::transformVectorized( 115 | const Matrix2X& rhs) const { 116 | return (r_A_B_.toRotationMatrix() * rhs).colwise() + A_t_A_B_; 117 | } 118 | 119 | template 120 | Transformation2DTemplate Transformation2DTemplate::inverse() 121 | const { 122 | return Transformation2DTemplate( 123 | r_A_B_.inverse(), -(r_A_B_.inverse() * A_t_A_B_)); 124 | } 125 | 126 | template 127 | bool Transformation2DTemplate::operator==( 128 | const Transformation2DTemplate& rhs) const { 129 | return r_A_B_.angle() == rhs.r_A_B_.angle() && A_t_A_B_ == rhs.A_t_A_B_; 130 | } 131 | 132 | template 133 | bool Transformation2DTemplate::operator!=( 134 | const Transformation2DTemplate& rhs) const { 135 | return !(*this == rhs); 136 | } 137 | 138 | template 139 | std::ostream & operator<<(std::ostream & out, 140 | const Transformation2DTemplate& rhs) { 141 | out << "[" < 147 | template 148 | Transformation2DTemplate 149 | Transformation2DTemplate::cast() const { 150 | return Transformation2DTemplate( 151 | getRotation().template cast(), 152 | getPosition().template cast()); 153 | } 154 | 155 | } // namespace minimal 156 | } // namespace kindr 157 | 158 | #endif // KINDR_MINIMAL_IMPLEMENTATION_TRANSFORM_2D_INL_H_ 159 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/angle-axis.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MIN_ROTATION_ANGLE_AXIS_H_ 26 | #define KINDR_MIN_ROTATION_ANGLE_AXIS_H_ 27 | 28 | #include 29 | 30 | #include 31 | 32 | namespace kindr { 33 | namespace minimal { 34 | 35 | template 36 | class RotationQuaternionTemplate; 37 | 38 | /// \class AngleAxis 39 | /// \brief a minimal implementation of an angle and axis representation of 40 | /// rotation 41 | /// This rotation takes vectors from frame B to frame A, written 42 | /// as \f${}_{A}\mathbf{v} = \mathbf{C}_{AB} {}_{B}\mathbf{v}\f$ 43 | /// 44 | /// The angle is assumed to be in radians everywhere 45 | /// 46 | /// In code, we write: 47 | /// 48 | /// \code{.cpp} 49 | /// A_v = C_A_B.rotate(B_v); 50 | /// \endcode 51 | /// 52 | template 53 | class AngleAxisTemplate { 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | typedef Eigen::Matrix Vector3; 58 | 59 | typedef Eigen::Matrix Vector4; 60 | 61 | typedef Eigen::AngleAxis Implementation; 62 | 63 | typedef Eigen::Matrix RotationMatrix; 64 | 65 | /// \brief initialize to identity. 66 | AngleAxisTemplate(); 67 | AngleAxisTemplate(const AngleAxisTemplate& other) = default; 68 | 69 | /// \brief initialize from the angle and rotation axis (angle first). 70 | AngleAxisTemplate(Scalar angle, Scalar v1, Scalar v2, Scalar v3); 71 | 72 | /// \brief initialize from the angle and rotation axis. 73 | AngleAxisTemplate(Scalar angle, const Vector3& axis); 74 | 75 | /// \brief initialize from an Eigen angleAxis. 76 | AngleAxisTemplate(const Implementation& angleAxis); 77 | 78 | /// \brief initialize from a rotation matrix. 79 | AngleAxisTemplate(const RotationMatrix& matrix); 80 | 81 | /// \brief initialize from an Eigen quaternion. 82 | AngleAxisTemplate(const RotationQuaternionTemplate& quat); 83 | 84 | /// \brief initialize from a angle-scaled axis vector. 85 | AngleAxisTemplate(const Vector3& angleAxis); 86 | 87 | ~AngleAxisTemplate(); 88 | 89 | /// \brief Returns the rotation angle. 90 | Scalar angle() const; 91 | 92 | /// \brief Sets the rotation angle. 93 | void setAngle(Scalar angle); 94 | 95 | /// \brief Returns the rotation axis. 96 | const Vector3& axis() const; 97 | 98 | /// \brief Sets the rotation axis. 99 | void setAxis(const Vector3& axis); 100 | 101 | /// \brief Sets the rotation axis. 102 | void setAxis(Scalar v1, Scalar v2, Scalar v3); 103 | 104 | /// \brief get the components of the angle/axis as a vector (angle first). 105 | Vector4 vector() const; 106 | 107 | /// \brief get a copy of the representation that is unique. 108 | AngleAxisTemplate getUnique() const; 109 | 110 | /// \brief set the angle/axis to its unique representation. 111 | AngleAxisTemplate& setUnique(); 112 | 113 | /// \brief set the rotation to identity. 114 | AngleAxisTemplate& setIdentity(); 115 | 116 | /// \brief get a copy of the rotation inverted. 117 | AngleAxisTemplate inverse() const; 118 | 119 | /// \deprecated use inverse() instead. 120 | AngleAxisTemplate inverted() const __attribute__((deprecated)); 121 | 122 | /// \brief rotate a vector, v. 123 | Vector3 rotate(const Vector3& v) const; 124 | 125 | /// \brief rotate a vector, v. 126 | Vector4 rotate4(const Vector4& v) const; 127 | 128 | /// \brief rotate a vector, v. 129 | Vector3 inverseRotate(const Vector3& v) const; 130 | 131 | /// \brief rotate a vector, v. 132 | Vector4 inverseRotate4(const Vector4& v) const; 133 | 134 | /// \brief cast to the implementation type. 135 | Implementation& toImplementation(); 136 | 137 | /// \brief cast to the implementation type. 138 | const Implementation& toImplementation() const; 139 | 140 | /// \brief get the angle between this and the other rotation. 141 | Scalar getDisparityAngle(const AngleAxisTemplate& rhs) const; 142 | 143 | /// \brief enforce the unit length constraint. 144 | AngleAxisTemplate& normalize(); 145 | 146 | /// \brief compose two rotations. 147 | AngleAxisTemplate operator*( 148 | const AngleAxisTemplate& rhs) const; 149 | 150 | /// \brief assignment operator. 151 | AngleAxisTemplate& operator=(const AngleAxisTemplate& rhs); 152 | 153 | /// \brief get the rotation matrix. 154 | RotationMatrix getRotationMatrix() const; 155 | 156 | private: 157 | Implementation C_A_B_; 158 | }; 159 | 160 | typedef AngleAxisTemplate AngleAxis; 161 | 162 | template 163 | std::ostream& operator<<(std::ostream& out, 164 | const AngleAxisTemplate& rhs); 165 | 166 | } // namespace minimal 167 | } // namespace kindr 168 | 169 | #include 170 | 171 | #endif /* KINDR_MIN_ROTATION_ANGLE_AXIS_HPP */ 172 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/quat-transformation.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MINIMAL_QUAT_TRANSFORMATION_H_ 26 | #define KINDR_MINIMAL_QUAT_TRANSFORMATION_H_ 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | namespace kindr { 34 | namespace minimal { 35 | 36 | /// \class QuatTransformation 37 | /// \brief A frame transformation built from a quaternion and a point 38 | /// 39 | /// This transformation takes points from frame B to frame A, written 40 | /// as \f${}_{A}\mathbf{p} = \mathbf{T}_{AB} {}_{B}\mathbf{p}\f$ 41 | /// 42 | /// In code, we write: 43 | /// 44 | /// \code{.cpp} 45 | /// A_p = T_A_B.transform(B_p); 46 | /// \endcode 47 | /// 48 | template 49 | class QuatTransformationTemplate { 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 52 | 53 | typedef Eigen::Matrix Vector3; 54 | typedef Eigen::Matrix Vector4; 55 | typedef Eigen::Matrix Vector6; 56 | 57 | typedef Eigen::Matrix Matrix3X; 58 | 59 | typedef PositionTemplate Position; 60 | typedef RotationQuaternionTemplate Rotation; 61 | typedef Eigen::Matrix RotationMatrix; 62 | typedef Eigen::Matrix TransformationMatrix; 63 | 64 | /// \brief Constructor of identity transformation. 65 | QuatTransformationTemplate(); 66 | QuatTransformationTemplate(const QuatTransformationTemplate& other) = default; 67 | 68 | explicit QuatTransformationTemplate( 69 | const RotationQuaternionTemplate& q_A_B, const Position& A_t_A_B); 70 | explicit QuatTransformationTemplate( 71 | const typename Rotation::Implementation& q_A_B, const Position& A_t_A_B); 72 | 73 | explicit QuatTransformationTemplate( 74 | const Position& A_t_A_B, const Rotation& q_A_B); 75 | explicit QuatTransformationTemplate( 76 | const Position& A_t_A_B, const typename Rotation::Implementation& q_A_B); 77 | 78 | explicit QuatTransformationTemplate(const TransformationMatrix& T); 79 | 80 | /// \brief a constructor based on the exponential map. 81 | /// translational part in the first 3 dimensions, 82 | /// rotational part in the last 3 dimensions. 83 | QuatTransformationTemplate(const Vector6& x_t_r); 84 | 85 | ~QuatTransformationTemplate(); 86 | 87 | void setIdentity(); 88 | 89 | /// \brief set to random transformation. 90 | QuatTransformationTemplate& setRandom(); 91 | 92 | /// \brief set to random transformation with a given translation norm. 93 | QuatTransformationTemplate& setRandom(Scalar norm_translation); 94 | 95 | /// \brief set to random transformation with a given translation norm and rotation angle. 96 | QuatTransformationTemplate& setRandom(Scalar norm_translation, Scalar angle_rad); 97 | 98 | /// \brief get the position component. 99 | Position& getPosition(); 100 | 101 | /// \brief get the position component. 102 | const Position& getPosition() const; 103 | 104 | /// \brief get the rotation component. 105 | Rotation& getRotation(); 106 | 107 | /// \brief get the rotation component. 108 | const Rotation& getRotation() const; 109 | 110 | /// \brief get the rotation component as an Eigen Quaternion directly. 111 | const Eigen::Quaternion& getEigenQuaternion() const; 112 | 113 | /// \brief get the transformation matrix. 114 | TransformationMatrix getTransformationMatrix() const; 115 | 116 | /// \brief get the rotation matrix. 117 | RotationMatrix getRotationMatrix() const; 118 | 119 | /// \brief get the quaternion of rotation and the position as a vector. 120 | /// [w x y z, x y z] 121 | Eigen::Matrix asVector() const; 122 | 123 | /// \brief compose two transformations. 124 | QuatTransformationTemplate operator*( 125 | const QuatTransformationTemplate& rhs) const; 126 | 127 | /// \brief transform a point. 128 | Vector3 operator*(const Vector3& rhs) const; 129 | 130 | /// \brief transform a point. 131 | Vector3 transform(const Vector3& rhs) const; 132 | 133 | /// \brief transform points. 134 | Matrix3X transformVectorized(const Matrix3X& rhs) const; 135 | 136 | /// \brief transform a point. 137 | Vector4 transform4(const Vector4& rhs) const; 138 | 139 | /// \brief transform a point by the inverse. 140 | Vector3 inverseTransform(const Vector3& rhs) const; 141 | 142 | /// \brief transform a point by the inverse. 143 | Vector4 inverseTransform4(const Vector4& rhs) const; 144 | 145 | /// \brief get the logarithmic map of the transformation 146 | /// note: this is the log map of SO(3)xR(3) and not SE(3) 147 | /// \return vector form of log map with first 3 components the translational 148 | /// part and the last three the rotational part. 149 | Vector6 log() const; 150 | 151 | /// \brief get the exponential map of the parameters, resulting in a valid 152 | /// transformation note: this is the exp map of SO(3)xR(3) and not SE(3) 153 | /// \param[in] vec vector form of log map with first 3 components the translational 154 | /// part and the last three the rotational part. 155 | /// \return The corresponding Transformation. 156 | static QuatTransformationTemplate exp(const Vector6& vec); 157 | 158 | /// \brief get the logarithmic map of the transformation 159 | /// note: this is the log map of SO(3)xR(3) and not SE(3) 160 | /// \return vector form of log map with first 3 components the translational 161 | /// part and the last three the rotational part. 162 | static Vector6 log(const QuatTransformationTemplate& vec); 163 | 164 | /// \brief return a copy of the transformation inverted. 165 | QuatTransformationTemplate inverse() const; 166 | 167 | /// \deprecated use inverse() instead. 168 | QuatTransformationTemplate inverted() const __attribute__((deprecated)); 169 | 170 | /// \brief check for binary equality. 171 | bool operator==(const QuatTransformationTemplate& rhs) const; 172 | 173 | /// \brief Factory to construct a QuatTransformTemplate from a transformation 174 | /// matrix with a near orthonormal rotation matrix. 175 | static QuatTransformationTemplate 176 | constructAndRenormalizeRotation(const TransformationMatrix& T); 177 | 178 | /// \brief cast scalar elements to another type 179 | template 180 | QuatTransformationTemplate cast() const; 181 | 182 | private: 183 | /// The quaternion that takes vectors from B to A. 184 | /// 185 | /// \code{.cpp} 186 | /// A_v = q_A_B_.rotate(B_v); 187 | /// \endcode 188 | Rotation q_A_B_; 189 | /// The vector from the origin of A to the origin of B 190 | /// expressed in A 191 | Position A_t_A_B_; 192 | }; 193 | 194 | typedef QuatTransformationTemplate QuatTransformation; 195 | 196 | template 197 | std::ostream & operator<<(std::ostream & out, 198 | const QuatTransformationTemplate& pose); 199 | 200 | // Exponential interpolation (i.e., Slerp) in SO(3) and linear interpolation in 201 | // R3. Lambda is in [0, 1], with 0 returning T_a, and 1 returning T_b. 202 | template 203 | inline QuatTransformationTemplate interpolateComponentwise( 204 | const QuatTransformationTemplate& T_a, 205 | const QuatTransformationTemplate& T_b, const double lambda); 206 | } // namespace minimal 207 | } // namespace kindr 208 | 209 | #include 210 | 211 | #endif // KINDR_MINIMAL_QUAT_TRANSFORMATION_H_ 212 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/rotation-quaternion.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MIN_ROTATION_QUATERNION_H_ 26 | #define KINDR_MIN_ROTATION_QUATERNION_H_ 27 | 28 | #include 29 | 30 | #include 31 | 32 | namespace kindr { 33 | namespace minimal { 34 | 35 | template 36 | class AngleAxisTemplate; 37 | 38 | /// \class RotationQuaternion 39 | /// \brief a minimal implementation of a passive Hamiltonian rotation 40 | /// (unit-length) quaternion 41 | /// 42 | /// This rotation takes vectors from frame B to frame A, written 43 | /// as \f${}_{A}\mathbf{v} = \mathbf{C}_{AB} {}_{B}\mathbf{v}\f$ 44 | /// 45 | /// In code, we write: 46 | /// 47 | /// \code{.cpp} 48 | /// A_v = q_A_B.rotate(B_v); 49 | /// \endcode 50 | /// 51 | template 52 | class RotationQuaternionTemplate { 53 | public: 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 55 | 56 | typedef Eigen::Matrix Vector3; 57 | 58 | typedef Vector3 Imaginary; 59 | 60 | typedef Eigen::Matrix Vector4; 61 | 62 | typedef Eigen::Matrix Matrix3X; 63 | 64 | typedef Eigen::Quaternion Implementation; 65 | 66 | typedef Eigen::Matrix RotationMatrix; 67 | 68 | /// \brief initialize to identity. 69 | RotationQuaternionTemplate(); 70 | RotationQuaternionTemplate(const RotationQuaternionTemplate& other) = default; 71 | 72 | /// \brief initialize from angle scaled axis. 73 | explicit RotationQuaternionTemplate(const Vector3& angle_scaled_axis); 74 | 75 | /// \brief initialize from real and imaginary components (real first). 76 | explicit RotationQuaternionTemplate(Scalar w, Scalar x, Scalar y, Scalar z); 77 | 78 | /// \brief initialize from real and imaginary components. 79 | explicit RotationQuaternionTemplate(Scalar real, const Imaginary& imaginary); 80 | 81 | /// \brief initialize from an Eigen quaternion. 82 | explicit RotationQuaternionTemplate(const Implementation& quaternion); 83 | 84 | /// \brief initialize from a rotation matrix. 85 | explicit RotationQuaternionTemplate(const RotationMatrix& matrix); 86 | 87 | /// \brief take an approximate rotation matrix, recover the closest matrix 88 | /// in SO(3) and construct. 89 | static RotationQuaternionTemplate fromApproximateRotationMatrix( 90 | const RotationMatrix& matrix); 91 | 92 | /// \brief initialize from an AngleAxis. 93 | explicit RotationQuaternionTemplate( 94 | const AngleAxisTemplate& angleAxis); 95 | 96 | ~RotationQuaternionTemplate(); 97 | 98 | /// \brief the real component of the quaternion. 99 | Scalar w() const; 100 | /// \brief the first imaginary component of the quaternion. 101 | Scalar x() const; 102 | /// \brief the second imaginary component of the quaternion. 103 | Scalar y() const; 104 | /// \brief the third imaginary component of the quaternion. 105 | Scalar z() const; 106 | 107 | /// \brief the imaginary components of the quaterion. 108 | Imaginary imaginary() const; 109 | 110 | /// \brief get the components of the quaternion as a vector (real first). 111 | Vector4 vector() const; 112 | 113 | /// \brief set the quaternion by its values (real, imaginary). 114 | void setValues(Scalar w, Scalar x, Scalar y, Scalar z); 115 | 116 | /// \brief set the quaternion by its real and imaginary parts. 117 | void setParts(Scalar real, const Imaginary& imag); 118 | 119 | /// \brief get a copy of the representation that is unique. 120 | RotationQuaternionTemplate getUnique() const; 121 | 122 | /// \brief set the quaternion to its unique representation. 123 | RotationQuaternionTemplate& setUnique(); 124 | 125 | /// \brief set the quaternion to identity. 126 | RotationQuaternionTemplate& setIdentity(); 127 | 128 | /// \brief set to random rotation. 129 | RotationQuaternionTemplate& setRandom(); 130 | 131 | /// \brief set to random rotation with a given angle. 132 | RotationQuaternionTemplate& setRandom(Scalar angle_rad); 133 | 134 | /// \brief get a copy of the quaternion inverted. 135 | RotationQuaternionTemplate inverse() const; 136 | 137 | /// \deprecated use inverse instead. 138 | RotationQuaternionTemplate inverted() const __attribute__((deprecated)); 139 | 140 | /// \brief get a copy of the conjugate of the quaternion. 141 | RotationQuaternionTemplate conjugated() const; 142 | 143 | /// \brief rotate a vector, v. 144 | Vector3 rotate(const Vector3& v) const; 145 | 146 | /// \brief rotate vectors v. 147 | Matrix3X rotateVectorized(const Matrix3X& v) const; 148 | 149 | /// \brief rotate a vector, v. 150 | Vector4 rotate4(const Vector4& v) const; 151 | 152 | /// \brief rotate a vector, v. 153 | Vector3 inverseRotate(const Vector3& v) const; 154 | 155 | /// \brief rotate a vector, v. 156 | Vector4 inverseRotate4(const Vector4& v) const; 157 | 158 | /// \brief cast to the implementation type. 159 | Implementation& toImplementation(); 160 | 161 | /// \brief cast to the implementation type. 162 | const Implementation& toImplementation() const; 163 | 164 | /// \brief get the norm of the quaternion. 165 | Scalar norm() const; 166 | 167 | /// \brief get the squared norm of the quaternion. 168 | Scalar squaredNorm() const; 169 | 170 | /// \brief get the angle [rad] between this and the other quaternion. 171 | Scalar getDisparityAngle(const RotationQuaternionTemplate& rhs) const; 172 | 173 | /// \brief get the angle [rad] between this and the angle axis. 174 | Scalar getDisparityAngle(const AngleAxisTemplate& rhs) const; 175 | 176 | /// \brief enforce the unit length constraint. 177 | RotationQuaternionTemplate& normalize(); 178 | 179 | /// \brief compose two quaternions. 180 | RotationQuaternionTemplate operator*( 181 | const RotationQuaternionTemplate& rhs) const; 182 | 183 | /// \brief compose quaternion and angle axis. 184 | RotationQuaternionTemplate operator*( 185 | const AngleAxisTemplate& rhs) const; 186 | 187 | /// \brief assignment operator. 188 | RotationQuaternionTemplate& operator=( 189 | const RotationQuaternionTemplate& rhs); 190 | 191 | /// \brief get the rotation matrix. 192 | RotationMatrix getRotationMatrix() const; 193 | 194 | /// \brief check for binary equality. 195 | bool operator==(const RotationQuaternionTemplate& rhs) const { 196 | return vector() == rhs.vector(); 197 | } 198 | 199 | // Compute the matrix log of the quaternion. 200 | static Vector3 log(const RotationQuaternionTemplate& q); 201 | 202 | // Compute the matrix exponential of the quaternion. 203 | static RotationQuaternionTemplate exp(const Vector3& dx); 204 | 205 | Vector3 log() const; 206 | 207 | /// \brief Check the validity of a rotation matrix. 208 | static bool isValidRotationMatrix(const RotationMatrix& matrix); 209 | static bool isValidRotationMatrix(const RotationMatrix& matrix, 210 | const Scalar threshold); 211 | 212 | /// \brief Factory to construct a RotationQuaternionTemplate from a near 213 | /// orthonormal rotation matrix. 214 | inline static RotationQuaternionTemplate constructAndRenormalize( 215 | const RotationMatrix& R) { 216 | return RotationQuaternionTemplate(Implementation(R).normalized()); 217 | } 218 | 219 | /// \brief cast scalar elements to another type 220 | template 221 | RotationQuaternionTemplate cast() const; 222 | 223 | private: 224 | void normalizationHelper(Implementation* quaternion) const; 225 | 226 | Implementation q_A_B_; 227 | }; 228 | 229 | typedef RotationQuaternionTemplate RotationQuaternion; 230 | 231 | template 232 | std::ostream& operator<<(std::ostream& out, 233 | const RotationQuaternionTemplate& rhs); 234 | 235 | } // namespace minimal 236 | } // namespace kindr 237 | 238 | #include 239 | 240 | #endif // KINDR_MIN_ROTATION_QUATERNION_H_ 241 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/implementation/angle-axis-inl.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MIN_ROTATION_ANGLE_AXIS_INL_H_ 26 | #define KINDR_MIN_ROTATION_ANGLE_AXIS_INL_H_ 27 | #include 28 | #include 29 | #include 30 | 31 | namespace kindr { 32 | namespace minimal { 33 | 34 | template 35 | AngleAxisTemplate::AngleAxisTemplate() : 36 | C_A_B_(Implementation::Identity()) { 37 | } 38 | 39 | template 40 | AngleAxisTemplate::AngleAxisTemplate( 41 | Scalar w, Scalar x, Scalar y, Scalar z) : C_A_B_(w, Vector3(x,y,z)) { 42 | CHECK_NEAR(Vector3(x,y,z).squaredNorm(), static_cast(1.0), 43 | static_cast(1e-4)); 44 | } 45 | 46 | template 47 | AngleAxisTemplate::AngleAxisTemplate( 48 | Scalar angle, const typename AngleAxisTemplate::Vector3& axis) : 49 | C_A_B_(angle, axis){ 50 | CHECK_NEAR(axis.squaredNorm(), static_cast(1.0), 51 | static_cast(1e-4)); 52 | } 53 | 54 | template 55 | AngleAxisTemplate::AngleAxisTemplate(const Implementation& angleAxis) : 56 | C_A_B_(angleAxis) { 57 | } 58 | 59 | template 60 | AngleAxisTemplate::AngleAxisTemplate(const RotationMatrix& matrix) : 61 | C_A_B_(matrix) { 62 | // \todo furgalep Check that the matrix was good... 63 | } 64 | 65 | template 66 | AngleAxisTemplate::AngleAxisTemplate(const Vector3& rotationVector) { 67 | const Scalar angle_rad = rotationVector.norm(); 68 | if (angle_rad < std::numeric_limits::epsilon()) { 69 | setIdentity(); 70 | } else { 71 | Vector3 axis = rotationVector; 72 | axis = axis / angle_rad; 73 | 74 | C_A_B_ = Implementation(angle_rad, axis); 75 | } 76 | } 77 | 78 | template 79 | AngleAxisTemplate::AngleAxisTemplate( 80 | const RotationQuaternionTemplate& quat) : 81 | C_A_B_(quat.toImplementation()) { 82 | } 83 | 84 | template 85 | AngleAxisTemplate::~AngleAxisTemplate() { } 86 | 87 | template 88 | AngleAxisTemplate& AngleAxisTemplate::operator=( 89 | const AngleAxisTemplate& rhs) { 90 | if(this != &rhs) { 91 | C_A_B_ = rhs.C_A_B_; 92 | } 93 | return *this; 94 | } 95 | 96 | template 97 | Scalar AngleAxisTemplate::angle() const{ 98 | return C_A_B_.angle(); 99 | } 100 | 101 | template 102 | void AngleAxisTemplate::setAngle(Scalar angle){ 103 | C_A_B_.angle() = angle; 104 | } 105 | 106 | template 107 | const typename AngleAxisTemplate::Vector3& 108 | AngleAxisTemplate::axis() const{ 109 | return C_A_B_.axis(); 110 | } 111 | 112 | template 113 | void AngleAxisTemplate::setAxis(const Vector3& axis){ 114 | CHECK_NEAR(axis.squaredNorm(), static_cast(1.0), 115 | static_cast(1e-4)); 116 | C_A_B_.axis() = axis; 117 | } 118 | 119 | template 120 | void AngleAxisTemplate::setAxis(Scalar v1, Scalar v2, Scalar v3){ 121 | C_A_B_.axis() = Vector3(v1,v2,v3); 122 | CHECK_NEAR(C_A_B_.axis().squaredNorm(), static_cast(1.0), 123 | static_cast(1e-4)); 124 | } 125 | 126 | template 127 | typename AngleAxisTemplate::Vector4 128 | AngleAxisTemplate::vector() const{ 129 | Vector4 vector; 130 | vector(0) = angle(); 131 | vector.template tail<3>() = C_A_B_.axis(); 132 | return vector; 133 | } 134 | 135 | template 136 | AngleAxisTemplate AngleAxisTemplate::getUnique() const { 137 | // first wraps angle into [-pi,pi) 138 | AngleAxisTemplate aa(fmod(angle()+M_PI, 2*M_PI)-M_PI, C_A_B_.axis()); 139 | if(aa.angle() > 0) { 140 | return aa; 141 | } else if(aa.angle() < 0) { 142 | if(aa.angle() != -M_PI) { 143 | return AngleAxisTemplate(-aa.angle(), -aa.axis()); 144 | } else { // angle == -pi, so axis must be viewed further, because -pi,axis 145 | // does the same as -pi,-axis. 146 | if(aa.axis()[0] < 0) { 147 | return AngleAxisTemplate(-aa.angle(), -aa.axis()); 148 | } else if(aa.axis()[0] > 0) { 149 | return AngleAxisTemplate(-aa.angle(), aa.axis()); 150 | } else { // v1 == 0 151 | if(aa.axis()[1] < 0) { 152 | return AngleAxisTemplate(-aa.angle(), -aa.axis()); 153 | } else if(aa.axis()[1] > 0) { 154 | return AngleAxisTemplate(-aa.angle(), aa.axis()); 155 | } else { // v2 == 0 156 | if(aa.axis()[2] < 0) { // v3 must be -1 or 1 157 | return AngleAxisTemplate(-aa.angle(), -aa.axis()); 158 | } else { 159 | return AngleAxisTemplate(-aa.angle(), aa.axis()); 160 | } 161 | } 162 | } 163 | } 164 | } else { // angle == 0 165 | return AngleAxisTemplate(); 166 | } 167 | } 168 | 169 | template 170 | AngleAxisTemplate& AngleAxisTemplate::setUnique() { 171 | *this = getUnique(); 172 | return *this; 173 | } 174 | 175 | template 176 | AngleAxisTemplate& AngleAxisTemplate::setIdentity() { 177 | C_A_B_ = C_A_B_.Identity(); 178 | return *this; 179 | } 180 | 181 | template 182 | AngleAxisTemplate AngleAxisTemplate::inverse() const { 183 | return AngleAxisTemplate(C_A_B_.inverse()); 184 | } 185 | 186 | template 187 | AngleAxisTemplate AngleAxisTemplate::inverted() const { 188 | return inverse(); 189 | } 190 | 191 | template 192 | typename AngleAxisTemplate::Vector3 AngleAxisTemplate::rotate( 193 | const AngleAxisTemplate::Vector3& v) const { 194 | return C_A_B_*v; 195 | } 196 | 197 | template 198 | typename AngleAxisTemplate::Vector4 199 | AngleAxisTemplate::rotate4( 200 | const AngleAxisTemplate::Vector4& v) const { 201 | AngleAxisTemplate::Vector4 vprime; 202 | vprime[3] = v[3]; 203 | vprime.template head<3>() = C_A_B_ * v.template head<3>(); 204 | return vprime; 205 | } 206 | 207 | template 208 | typename AngleAxisTemplate::Vector3 209 | AngleAxisTemplate::inverseRotate( 210 | const AngleAxisTemplate::Vector3& v) const { 211 | return C_A_B_.inverse() * v; 212 | } 213 | 214 | template 215 | typename AngleAxisTemplate::Vector4 216 | AngleAxisTemplate::inverseRotate4( 217 | const typename AngleAxisTemplate::Vector4& v) const { 218 | Eigen::Vector4d vprime; 219 | vprime[3] = v[3]; 220 | vprime.template head<3>() = C_A_B_.inverse() * v.template head<3>(); 221 | return vprime; 222 | } 223 | 224 | template 225 | typename AngleAxisTemplate::Implementation& 226 | AngleAxisTemplate::toImplementation() { 227 | return C_A_B_; 228 | } 229 | 230 | template 231 | const typename AngleAxisTemplate::Implementation& 232 | AngleAxisTemplate::toImplementation() const { 233 | return C_A_B_; 234 | } 235 | 236 | template 237 | AngleAxisTemplate& AngleAxisTemplate::normalize() { 238 | C_A_B_.axis().normalize(); 239 | return *this; 240 | } 241 | 242 | template 243 | AngleAxisTemplate AngleAxisTemplate::operator*( 244 | const AngleAxisTemplate& rhs) const { 245 | return AngleAxisTemplate(Implementation(C_A_B_ * rhs.C_A_B_)); 246 | } 247 | 248 | template 249 | Scalar AngleAxisTemplate::getDisparityAngle( 250 | const AngleAxisTemplate& rhs) const { 251 | return (rhs * this->inverse()).getUnique().angle(); 252 | } 253 | 254 | template 255 | std::ostream& operator<<(std::ostream& out, 256 | const AngleAxisTemplate& rhs) { 257 | out << rhs.vector().transpose(); 258 | return out; 259 | } 260 | 261 | template 262 | typename AngleAxisTemplate::RotationMatrix 263 | AngleAxisTemplate::getRotationMatrix() const { 264 | return C_A_B_.matrix(); 265 | } 266 | 267 | } // namespace minimal 268 | } // namespace kindr 269 | #endif // KINDR_MIN_ROTATION_ANGLE_AXIS_INL_H_ 270 | -------------------------------------------------------------------------------- /minkindr/test/test-rotation.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #ifndef TEST 32 | #define TEST(a, b) int Test_##a##_##b() 33 | #endif 34 | 35 | TEST(MinKindrTests,testQuatAxisAngle) { 36 | using namespace kindr::minimal; 37 | Eigen::Vector4d v(0.64491714, 0.26382416, 0.51605132, 0.49816637); 38 | Eigen::Matrix3d C; 39 | 40 | // This is data generated from a trusted python implementation. 41 | C << -0.02895739, -0.37025845, 0.92847733, 42 | 0.91484567, 0.36445417, 0.17386938, 43 | -0.40276404, 0.85444827, 0.3281757; 44 | 45 | Eigen::Vector4d aax(1.7397629325686206, 0.34520549, 0.67523668, 0.65183479); 46 | 47 | RotationQuaternion q1(v[0], v[1], v[2], v[3]); 48 | RotationQuaternion q2(C); 49 | RotationQuaternion q3(v[0],v.tail<3>()); 50 | RotationQuaternion q4(v[0], v[1], v[2], v[3]); 51 | RotationQuaternion q5(-v[0], -v[1], -v[2], -v[3]); 52 | 53 | 54 | AngleAxis a1(aax[0],aax.tail<3>()); 55 | AngleAxis a2(aax[0] + 2*M_PI,aax.tail<3>()); 56 | AngleAxis a3(aax[0] + 4*M_PI,aax.tail<3>()); 57 | AngleAxis a4(q1); 58 | Eigen::Vector3d rotation_vector = aax[0]*aax.tail<3>(); 59 | AngleAxis a5(rotation_vector); 60 | 61 | RotationQuaternion q6(a1); 62 | 63 | EXPECT_NEAR(q1.getDisparityAngle(q2), 0.0, 1e-3); 64 | EXPECT_NEAR(q1.getDisparityAngle(q3), 0.0, 1e-3); 65 | EXPECT_NEAR(q1.getDisparityAngle(q4), 0.0, 1e-3); 66 | EXPECT_NEAR(q1.getDisparityAngle(q5), 0.0, 1e-3); 67 | EXPECT_NEAR(q1.getDisparityAngle(q6), 0.0, 1e-3); 68 | EXPECT_NEAR(q1.getDisparityAngle(a1), 0.0, 1e-3); 69 | EXPECT_NEAR(q1.getDisparityAngle(a2), 0.0, 1e-3); 70 | EXPECT_NEAR(q1.getDisparityAngle(a2), 0.0, 1e-3); 71 | EXPECT_NEAR(q1.getDisparityAngle(a3), 0.0, 1e-3); 72 | EXPECT_NEAR(q1.getDisparityAngle(a4), 0.0, 1e-3); 73 | EXPECT_NEAR(a1.getDisparityAngle(q2), 0.0, 1e-3); 74 | EXPECT_NEAR(a1.getDisparityAngle(q3), 0.0, 1e-3); 75 | EXPECT_NEAR(a1.getDisparityAngle(q4), 0.0, 1e-3); 76 | EXPECT_NEAR(a1.getDisparityAngle(q5), 0.0, 1e-3); 77 | EXPECT_NEAR(a1.getDisparityAngle(q6), 0.0, 1e-3); 78 | EXPECT_NEAR(a1.getDisparityAngle(q1), 0.0, 1e-3); 79 | EXPECT_NEAR(a1.getDisparityAngle(a2), 0.0, 1e-3); 80 | EXPECT_NEAR(a1.getDisparityAngle(a2), 0.0, 1e-3); 81 | EXPECT_NEAR(a1.getDisparityAngle(a3), 0.0, 1e-3); 82 | EXPECT_NEAR(a1.getDisparityAngle(a4), 0.0, 1e-3); 83 | EXPECT_NEAR(a1.getDisparityAngle(a5), 0.0, 1e-3); 84 | 85 | EXPECT_NEAR(q1.inverse().getDisparityAngle(q2.inverse()), 0.0, 1e-3); 86 | EXPECT_NEAR(q1.inverse().getDisparityAngle(q3.inverse()), 0.0, 1e-3); 87 | EXPECT_NEAR(q1.inverse().getDisparityAngle(q4.inverse()), 0.0, 1e-3); 88 | EXPECT_NEAR(q1.inverse().getDisparityAngle(q5.inverse()), 0.0, 1e-3); 89 | EXPECT_NEAR(q1.inverse().getDisparityAngle(q6.inverse()), 0.0, 1e-3); 90 | EXPECT_NEAR(q1.inverse().getDisparityAngle(a1.inverse()), 0.0, 1e-3); 91 | EXPECT_NEAR(q1.inverse().getDisparityAngle(a2.inverse()), 0.0, 1e-3); 92 | EXPECT_NEAR(q1.inverse().getDisparityAngle(a2.inverse()), 0.0, 1e-3); 93 | EXPECT_NEAR(q1.inverse().getDisparityAngle(a3.inverse()), 0.0, 1e-3); 94 | EXPECT_NEAR(q1.inverse().getDisparityAngle(a4.inverse()), 0.0, 1e-3); 95 | EXPECT_NEAR(a1.inverse().getDisparityAngle(q2.inverse()), 0.0, 1e-3); 96 | EXPECT_NEAR(a1.inverse().getDisparityAngle(q3.inverse()), 0.0, 1e-3); 97 | EXPECT_NEAR(a1.inverse().getDisparityAngle(q4.inverse()), 0.0, 1e-3); 98 | EXPECT_NEAR(a1.inverse().getDisparityAngle(q5.inverse()), 0.0, 1e-3); 99 | EXPECT_NEAR(a1.inverse().getDisparityAngle(q6.inverse()), 0.0, 1e-3); 100 | EXPECT_NEAR(a1.inverse().getDisparityAngle(q1.inverse()), 0.0, 1e-3); 101 | EXPECT_NEAR(a1.inverse().getDisparityAngle(a2.inverse()), 0.0, 1e-3); 102 | EXPECT_NEAR(a1.inverse().getDisparityAngle(a2.inverse()), 0.0, 1e-3); 103 | EXPECT_NEAR(a1.inverse().getDisparityAngle(a3.inverse()), 0.0, 1e-3); 104 | EXPECT_NEAR(a1.inverse().getDisparityAngle(a4.inverse()), 0.0, 1e-3); 105 | EXPECT_NEAR(a1.inverse().getDisparityAngle(a5.inverse()), 0.0, 1e-3); 106 | 107 | 108 | } 109 | 110 | TEST(MinKindrTests,testComposition) { 111 | using namespace kindr::minimal; 112 | Eigen::Vector4d v(0.64491714, 0.26382416, 0.51605132, 0.49816637); 113 | Eigen::Matrix3d C, Csquared; 114 | 115 | // This is data generated from a trusted python implementation. 116 | C << -0.02895739, -0.37025845, 0.92847733, 117 | 0.91484567, 0.36445417, 0.17386938, 118 | -0.40276404, 0.85444827, 0.3281757; 119 | 120 | Csquared << -0.71184809, 0.66911533, 0.21344081, 121 | 0.23689944, -0.05734011, 0.96984059, 122 | 0.66117392, 0.74094318, -0.1176956; 123 | 124 | Eigen::Vector4d aax(1.7397629325686206, 0.34520549, 0.67523668, 0.65183479); 125 | 126 | 127 | RotationQuaternion q1(v[0], v[1], v[2], v[3]); 128 | AngleAxis a1(aax[0],aax.tail<3>()); 129 | 130 | 131 | RotationQuaternion qsquared(Csquared); 132 | 133 | EXPECT_NEAR((q1*q1).getDisparityAngle(qsquared), 0.0, 1e-3); 134 | EXPECT_NEAR((q1*a1).getDisparityAngle(qsquared), 0.0, 1e-3); 135 | EXPECT_NEAR((a1*q1).getDisparityAngle(qsquared), 0.0, 1e-3); 136 | EXPECT_NEAR((a1*a1).getDisparityAngle(qsquared), 0.0, 1e-3); 137 | EXPECT_NEAR((a1.inverse()*a1.inverse()).getDisparityAngle(qsquared.inverse()), 0.0, 1e-3); 138 | EXPECT_NEAR((q1.inverse()*a1.inverse()).getDisparityAngle(qsquared.inverse()), 0.0, 1e-3); 139 | EXPECT_NEAR((a1.inverse()*q1.inverse()).getDisparityAngle(qsquared.inverse()), 0.0, 1e-3); 140 | EXPECT_NEAR((q1.inverse()*q1.inverse()).getDisparityAngle(qsquared.inverse()), 0.0, 1e-3); 141 | 142 | } 143 | 144 | TEST(MinKindrTests, testQuaternionInitialization) { 145 | Eigen::Vector4d q_coeffs; 146 | q_coeffs << 1, 0, 0, 0; 147 | kindr::minimal::RotationQuaternionTemplate q_from_coeffs( 148 | q_coeffs[0], q_coeffs[1], q_coeffs[2], q_coeffs[3]); 149 | Eigen::Quaterniond q; 150 | q.setIdentity(); 151 | 152 | for(int i = 0; i < 4; ++i) { 153 | EXPECT_NEAR(q_from_coeffs.toImplementation().coeffs()[i], 154 | q.coeffs()[i], 1e-10); 155 | } 156 | } 157 | 158 | TEST(MinKindrTests, testRotate) { 159 | using namespace kindr::minimal; 160 | Eigen::Vector4d q(0.64491714, 0.26382416, 0.51605132, 0.49816637); 161 | Eigen::Matrix3d C, Csquared; 162 | 163 | // This is data generated from a trusted python implementation. 164 | C << -0.02895739, -0.37025845, 0.92847733, 165 | 0.91484567, 0.36445417, 0.17386938, 166 | -0.40276404, 0.85444827, 0.3281757; 167 | 168 | Eigen::Vector4d aax(1.7397629325686206, 0.34520549, 0.67523668, 0.65183479); 169 | 170 | 171 | RotationQuaternion q1(q[0], q[1], q[2], q[3]); 172 | AngleAxis a1(aax[0],aax.tail<3>()); 173 | 174 | Eigen::Vector3d v(4.67833851, 8.52053031, 6.71796159); 175 | Eigen::Vector3d Cv(2.94720425, 8.55334831, 7.60075758); 176 | Eigen::Vector3d Ctv(4.95374446, 7.11329907, 8.02986227); 177 | Eigen::Vector3d Cv1 = q1.rotate(v); 178 | Eigen::Vector3d Cv2 = a1.rotate(v); 179 | Eigen::Vector3d Ctv1 = q1.inverseRotate(v); 180 | Eigen::Vector3d Ctv2 = a1.inverseRotate(v); 181 | Eigen::Vector3d Ctv3 = q1.inverse().rotate(v); 182 | Eigen::Vector3d Ctv4 = a1.inverse().rotate(v); 183 | 184 | 185 | for(int i = 0; i < 3; ++i) { 186 | EXPECT_NEAR(Cv[i], Cv1[i], 1e-4); 187 | EXPECT_NEAR(Cv[i], Cv2[i], 1e-4); 188 | EXPECT_NEAR(Ctv[i], Ctv1[i], 1e-4); 189 | EXPECT_NEAR(Ctv[i], Ctv2[i], 1e-4); 190 | EXPECT_NEAR(Ctv[i], Ctv3[i], 1e-4); 191 | EXPECT_NEAR(Ctv[i], Ctv4[i], 1e-4); 192 | } 193 | } 194 | 195 | TEST(MinKindrTests, testRotationExpLog) { 196 | using namespace kindr::minimal; 197 | for(int i = 0; i < 10; ++i) { 198 | RotationQuaternion C1; 199 | C1.setRandom(); 200 | RotationQuaternion::Vector3 v = C1.log(); 201 | RotationQuaternion C2 = RotationQuaternion::exp(v); 202 | Eigen::Matrix3d CC1 = C1.getRotationMatrix(); 203 | Eigen::Matrix3d CC2 = C2.getRotationMatrix(); 204 | for(int r = 0; r < 3; ++r) { 205 | for(int c = 0; c < 3; ++c) { 206 | EXPECT_NEAR(CC1(r,c), CC2(r,c), 1e-6) << "Failed at (" << r << "," << c << ")"; 207 | } 208 | } 209 | } 210 | 211 | RotationQuaternion::Vector3 axis; 212 | axis << 0.0,0.0,1.0; 213 | axis /= axis.norm(); 214 | for (double angle = -M_PI; angle <= M_PI; angle+=M_PI/100) { 215 | RotationQuaternion::Vector3 axisAngle = axis*angle; 216 | RotationQuaternion C2 = RotationQuaternion::exp(axisAngle); 217 | RotationQuaternion::Vector3 v = C2.log(); 218 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(v,axisAngle,1e-6)); 219 | } 220 | 221 | Eigen::Matrix q; 222 | for(int i=0; i < 1000; ++i) { 223 | RotationQuaternion C1; 224 | C1.setRandom(); 225 | RotationQuaternion::Vector3 v1 = C1.log(); 226 | C1.setParts(-C1.w(), -C1.imaginary()); 227 | RotationQuaternion::Vector3 v2 = C1.log(); 228 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(v1,v2,1e-6)); 229 | } 230 | } 231 | -------------------------------------------------------------------------------- /minkindr/test/test-transformation.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #ifndef TEST 33 | #define TEST(a, b) int Test_##a##_##b() 34 | #endif 35 | 36 | typedef kindr::minimal::QuatTransformation Transformation; 37 | 38 | Eigen::Vector3d fromHomogeneous(const Eigen::Vector4d& v) { 39 | return v.head<3>() / v[3]; 40 | } 41 | 42 | 43 | TEST(MinKindrTests, testTransform) { 44 | using namespace kindr::minimal; 45 | Eigen::Vector4d q(0.64491714, 0.26382416, 0.51605132, 0.49816637); 46 | RotationQuaternion q1(q[0], q[1], q[2], q[3]); 47 | Eigen::Vector3d t( 4.67833851, 8.52053031, 6.71796159 ); 48 | 49 | Transformation T(q1,t); 50 | Transformation invT = T.inverse(); 51 | Eigen::Vector3d v(6.26257419, 1.58356548, 6.05772983); 52 | Eigen::Vector4d vh(6.26257419, 1.58356548, 6.05772983, 1.0); 53 | Eigen::Vector4d vh2 = vh * 0.2; 54 | Eigen::Vector4d vh3 = -vh * 0.2; 55 | 56 | Eigen::Matrix3Xd vv(3, 2); 57 | vv.block<3, 1>(0, 0) = v; 58 | vv.block<3, 1>(0, 1) = v; 59 | Eigen::Matrix3Xd vempty(3, 0); 60 | 61 | Eigen::Vector3d Tv( 9.53512701, 15.88020996, 7.53669644); 62 | Eigen::Vector4d Tvh(9.53512701, 15.88020996, 7.53669644, 1.0); 63 | Eigen::Vector3d invTv(-6.12620997, -3.67891623, 0.04812912); 64 | Eigen::Vector3d Cv(4.8567885 , 7.35967965, 0.81873485); 65 | Eigen::Vector3d invCv(-1.17246549, 3.4343828 , 8.07799141); 66 | 67 | Eigen::Matrix4d Tmx; 68 | Tmx << 0.3281757 , -0.17386938, 0.92847733, 2.7527055, 69 | 0.85444827, -0.36445416, -0.37025845, 0.7790679, 70 | 0.40276403, 0.91484567, 0.02895739, 4.1696795, 71 | 0. , 0. , 0. , 1. ; 72 | 73 | 74 | Eigen::Vector3d Tv1 = T.transform(v); 75 | Eigen::Vector4d Tvh2 = T.transform4(vh); 76 | Eigen::Vector4d Tvh3 = T.transform4(vh2); 77 | Eigen::Vector4d Tvh4 = T.transform4(vh3); 78 | Eigen::Vector3d Tv2 = fromHomogeneous(Tvh2); 79 | Eigen::Vector3d Tv3 = fromHomogeneous(Tvh3); 80 | Eigen::Vector3d Tv4 = fromHomogeneous(Tvh4); 81 | 82 | Eigen::Matrix3Xd Tvv1 = T.transformVectorized(vv); 83 | EXPECT_DEATH(T.transform(vempty), "^"); 84 | 85 | for(int i = 0; i < 3; ++i) { 86 | EXPECT_NEAR(Tv1[i], Tv[i], 1e-4); 87 | EXPECT_NEAR(Tv2[i], Tv[i], 1e-4); 88 | EXPECT_NEAR(Tv3[i], Tv[i], 1e-4); 89 | EXPECT_NEAR(Tv4[i], Tv[i], 1e-4); 90 | 91 | EXPECT_NEAR(Tvv1(i, 0), Tv[i], 1e-4); 92 | EXPECT_NEAR(Tvv1(i, 1), Tv[i], 1e-4); 93 | } 94 | 95 | { 96 | Eigen::Vector3d invTv1 = T.inverse().transform(v); 97 | Eigen::Vector4d invTvh2 = T.inverse().transform4(vh); 98 | Eigen::Vector4d invTvh3 = T.inverse().transform4(vh2); 99 | Eigen::Vector4d invTvh4 = T.inverse().transform4(vh3); 100 | Eigen::Vector3d invTv2 = fromHomogeneous(invTvh2); 101 | Eigen::Vector3d invTv3 = fromHomogeneous(invTvh3); 102 | Eigen::Vector3d invTv4 = fromHomogeneous(invTvh4); 103 | 104 | for(int i = 0; i < 3; ++i) { 105 | EXPECT_NEAR(invTv1[i], invTv[i], 1e-4); 106 | EXPECT_NEAR(invTv2[i], invTv[i], 1e-4); 107 | EXPECT_NEAR(invTv3[i], invTv[i], 1e-4); 108 | EXPECT_NEAR(invTv4[i], invTv[i], 1e-4); 109 | } 110 | } 111 | 112 | { 113 | Eigen::Vector3d invTv1 = invT.transform(v); 114 | Eigen::Vector4d invTvh2 = invT.transform4(vh); 115 | Eigen::Vector4d invTvh3 = invT.transform4(vh2); 116 | Eigen::Vector4d invTvh4 = invT.transform4(vh3); 117 | Eigen::Vector3d invTv2 = fromHomogeneous(invTvh2); 118 | Eigen::Vector3d invTv3 = fromHomogeneous(invTvh3); 119 | Eigen::Vector3d invTv4 = fromHomogeneous(invTvh4); 120 | 121 | for(int i = 0; i < 3; ++i) { 122 | EXPECT_NEAR(invTv1[i], invTv[i], 1e-4); 123 | EXPECT_NEAR(invTv2[i], invTv[i], 1e-4); 124 | EXPECT_NEAR(invTv3[i], invTv[i], 1e-4); 125 | EXPECT_NEAR(invTv4[i], invTv[i], 1e-4); 126 | } 127 | } 128 | 129 | { 130 | Eigen::Vector3d invTv1 = T.inverseTransform(v); 131 | Eigen::Vector4d invTvh2 = T.inverseTransform4(vh); 132 | Eigen::Vector4d invTvh3 = T.inverseTransform4(vh2); 133 | Eigen::Vector4d invTvh4 = T.inverseTransform4(vh3); 134 | Eigen::Vector3d invTv2 = fromHomogeneous(invTvh2); 135 | Eigen::Vector3d invTv3 = fromHomogeneous(invTvh3); 136 | Eigen::Vector3d invTv4 = fromHomogeneous(invTvh4); 137 | 138 | for(int i = 0; i < 3; ++i) { 139 | EXPECT_NEAR(invTv1[i], invTv[i], 1e-4); 140 | EXPECT_NEAR(invTv2[i], invTv[i], 1e-4); 141 | EXPECT_NEAR(invTv3[i], invTv[i], 1e-4); 142 | EXPECT_NEAR(invTv4[i], invTv[i], 1e-4); 143 | } 144 | } 145 | } 146 | 147 | TEST(MinKindrTests, testCompose) { 148 | using namespace kindr::minimal; 149 | Eigen::Vector4d q(0.64491714, 0.26382416, 0.51605132, 0.49816637); 150 | RotationQuaternion q1(q[0], q[1], q[2], q[3]); 151 | Eigen::Vector3d t( 4.67833851, 8.52053031, 6.71796159 ); 152 | 153 | Transformation T(q1,t); 154 | Transformation invT = T.inverse(); 155 | Eigen::Vector3d v(6.26257419, 1.58356548, 6.05772983); 156 | Eigen::Vector3d invTTv(-8.16137069, -6.14469052, -14.34176544); 157 | Eigen::Vector3d TTv(5.52009598, 24.34170933, 18.9197339); 158 | 159 | Transformation TT = T * T; 160 | Transformation Id1 = T * T.inverse(); 161 | Transformation Id2 = T.inverse() * T; 162 | Transformation iTiT1 = T.inverse() * T.inverse(); 163 | Transformation iTiT2 = TT.inverse(); 164 | 165 | Eigen::Vector3d TTv1 = TT.transform(v); 166 | for(int i = 0; i < 3; ++i) { 167 | EXPECT_NEAR(TTv1[i], TTv[i],1e-4); 168 | } 169 | 170 | Eigen::Vector3d v1 = Id1.transform(v); 171 | Eigen::Vector3d v2 = Id2.transform(v); 172 | for(int i = 0; i < 3; ++i) { 173 | EXPECT_NEAR(v1[i], v[i],1e-4); 174 | EXPECT_NEAR(v2[i], v[i],1e-4); 175 | } 176 | 177 | Eigen::Vector3d iTTv1 = iTiT1.transform(v); 178 | Eigen::Vector3d iTTv2 = iTiT2.transform(v); 179 | for(int i = 0; i < 3; ++i) { 180 | EXPECT_NEAR(iTTv1[i], invTTv[i],1e-4); 181 | EXPECT_NEAR(iTTv2[i], invTTv[i],1e-4); 182 | } 183 | } 184 | 185 | TEST(MinKindrTests, testSetRandom) { 186 | using namespace kindr::minimal; 187 | Transformation T; 188 | T.setRandom(); 189 | Eigen::Matrix3d R = T.getRotation().getRotationMatrix(); 190 | 191 | // Check if orthonormal 192 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(R*R.transpose(), Eigen::Matrix3d::Identity(), 1e-6)); 193 | } 194 | 195 | TEST(MinKindrTests, testSetRandomWithNorm) { 196 | using namespace kindr::minimal; 197 | Transformation T; 198 | const double kTranslationNorm = 2.0; 199 | T.setRandom(kTranslationNorm); 200 | Eigen::Matrix3d R = T.getRotation().getRotationMatrix(); 201 | Eigen::Vector3d p = T.getPosition(); 202 | 203 | // Check if orthonormal and translation norm 204 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(R*R.transpose(), Eigen::Matrix3d::Identity(), 1e-6)); 205 | EXPECT_NEAR(p.norm(), kTranslationNorm, 1e-8); 206 | } 207 | 208 | TEST(MinKindrTests, testSetRandomWithAngleAndNorm) { 209 | using namespace kindr::minimal; 210 | Transformation T; 211 | const double kTranslationNorm = 2.0; 212 | const double KRotationAngleRad = 10.0 / 180.0 * M_PI; 213 | 214 | T.setRandom(kTranslationNorm, KRotationAngleRad); 215 | Eigen::Matrix3d R = T.getRotation().getRotationMatrix(); 216 | Eigen::Vector3d p = T.getPosition(); 217 | 218 | // Check if orthonormal, translation norm and rotation angle 219 | EXPECT_TRUE(EIGEN_MATRIX_NEAR(R*R.transpose(), Eigen::Matrix3d::Identity(), 1e-6)); 220 | EXPECT_NEAR(p.norm(), kTranslationNorm, 1e-8); 221 | EXPECT_NEAR(AngleAxis(R).angle(), KRotationAngleRad, 1e-8); 222 | } 223 | 224 | TEST(MinKindrTests, testExpLog) { 225 | using namespace kindr::minimal; 226 | for(int i = 0; i < 10; ++i) { 227 | Transformation T1; 228 | T1.setRandom(); 229 | Transformation::Vector6 v = T1.log(); 230 | Transformation T2 = Transformation::exp(v); 231 | Eigen::Matrix4d TT1 = T1.getTransformationMatrix(); 232 | Eigen::Matrix4d TT2 = T2.getTransformationMatrix(); 233 | for(int r = 0; r < 4; ++r) { 234 | for(int c = 0; c < 4; ++c) { 235 | EXPECT_NEAR(TT1(r,c), TT2(r,c), 1e-6) << "Failed at (" << r << "," << c << ")"; 236 | } 237 | } 238 | } 239 | } 240 | 241 | TEST(MinKindrTests, testLinearInterpolation) { 242 | using namespace kindr::minimal; 243 | Transformation T_a; 244 | T_a.setRandom(); 245 | 246 | Transformation T_b; 247 | T_b.setRandom(); 248 | 249 | EXPECT_DEATH(interpolateComponentwise( 250 | T_a, T_b, 1.0 + std::numeric_limits::epsilon()), ""); 251 | EXPECT_DEATH(interpolateComponentwise( 252 | T_a, T_b, 0.0 - std::numeric_limits::epsilon()), ""); 253 | 254 | Eigen::Matrix3d R_a; 255 | R_a << 0.0, -1.0, 0.0, 256 | 1.0, 0.0, 0.0, 257 | 0.0, 0.0, 1.0; 258 | Eigen::Vector3d p_a(1.0, 2.0, 3.0); 259 | T_a = Transformation(RotationQuaternion(R_a), p_a); 260 | 261 | Eigen::Matrix3d R_b; 262 | R_b << 0.0, 1.0, 0.0, 263 | -1.0, 0.0, 0.0, 264 | 0.0, 0.0, 1.0; 265 | Eigen::Vector3d p_b(4.0, 5.0, 6.0); 266 | T_b = Transformation(RotationQuaternion(R_b), p_b); 267 | 268 | Transformation T_int = interpolateComponentwise(T_a, T_b, 0.0); 269 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 270 | T_int.getRotationMatrix(), R_a, 1e-6)); 271 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 272 | T_int.getPosition(), p_a, 1e-6)); 273 | 274 | T_int = interpolateComponentwise(T_a, T_b, 1.0); 275 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 276 | T_int.getRotationMatrix(), R_b, 1e-6)); 277 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 278 | T_int.getPosition(), p_b, 1e-6)); 279 | 280 | T_int = interpolateComponentwise(T_a, T_b, 0.5); 281 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 282 | T_int.getRotationMatrix(), Eigen::Matrix3d::Identity(), 1e-6)); 283 | EXPECT_TRUE(EIGEN_MATRIX_NEAR( 284 | T_int.getPosition(), Eigen::Vector3d(2.5, 3.5, 4.5), 1e-6)); 285 | } 286 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/implementation/quat-transformation-inl.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MINIMAL_QUAT_TRANSFORMATION_H_INL_ 26 | #define KINDR_MINIMAL_QUAT_TRANSFORMATION_H_INL_ 27 | #include 28 | 29 | #include 30 | 31 | namespace kindr { 32 | namespace minimal { 33 | 34 | template 35 | QuatTransformationTemplate::QuatTransformationTemplate() { 36 | setIdentity(); 37 | } 38 | 39 | template 40 | QuatTransformationTemplate::QuatTransformationTemplate( 41 | const RotationQuaternionTemplate& q_A_B, const Position& A_t_A_B) : 42 | q_A_B_(q_A_B), 43 | A_t_A_B_(A_t_A_B) { 44 | 45 | } 46 | 47 | template 48 | QuatTransformationTemplate::QuatTransformationTemplate( 49 | const typename Rotation::Implementation& q_A_B, 50 | const Position& A_t_A_B) : 51 | q_A_B_(q_A_B), 52 | A_t_A_B_(A_t_A_B) { 53 | 54 | } 55 | 56 | template 57 | QuatTransformationTemplate::QuatTransformationTemplate( 58 | const Position& A_t_A_B, const RotationQuaternionTemplate& q_A_B) : 59 | q_A_B_(q_A_B), 60 | A_t_A_B_(A_t_A_B) { 61 | 62 | } 63 | 64 | template 65 | QuatTransformationTemplate::QuatTransformationTemplate( 66 | const Position& A_t_A_B, const typename Rotation::Implementation& q_A_B) : 67 | q_A_B_(q_A_B), 68 | A_t_A_B_(A_t_A_B) { 69 | 70 | } 71 | 72 | template 73 | QuatTransformationTemplate::QuatTransformationTemplate( 74 | const TransformationMatrix& T) : 75 | q_A_B_(T.template topLeftCorner<3,3>().eval()), 76 | A_t_A_B_(T.template topRightCorner<3,1>().eval()) { 77 | } 78 | 79 | template 80 | QuatTransformationTemplate::QuatTransformationTemplate( 81 | const QuatTransformationTemplate::Vector6& x_t_r) : 82 | q_A_B_(x_t_r.template tail<3>().eval()), 83 | A_t_A_B_(x_t_r.template head<3>().eval()) { 84 | } 85 | 86 | template 87 | QuatTransformationTemplate::~QuatTransformationTemplate() { 88 | 89 | } 90 | 91 | template 92 | void QuatTransformationTemplate::setIdentity() { 93 | q_A_B_.setIdentity(); 94 | A_t_A_B_.setZero(); 95 | } 96 | 97 | template 98 | typename QuatTransformationTemplate::Position& 99 | QuatTransformationTemplate::getPosition() { 100 | return A_t_A_B_; 101 | } 102 | 103 | template 104 | const typename QuatTransformationTemplate::Position& 105 | QuatTransformationTemplate::getPosition() const { 106 | return A_t_A_B_; 107 | } 108 | 109 | template 110 | typename QuatTransformationTemplate::Rotation& 111 | QuatTransformationTemplate::getRotation() { 112 | return q_A_B_; 113 | } 114 | 115 | template 116 | const typename QuatTransformationTemplate::Rotation& 117 | QuatTransformationTemplate::getRotation() const { 118 | return q_A_B_; 119 | } 120 | 121 | template 122 | const Eigen::Quaternion& 123 | QuatTransformationTemplate::getEigenQuaternion() const { 124 | return getRotation().toImplementation(); 125 | } 126 | 127 | template 128 | typename QuatTransformationTemplate::TransformationMatrix 129 | QuatTransformationTemplate::getTransformationMatrix() const { 130 | TransformationMatrix transformation_matrix; 131 | transformation_matrix.setIdentity(); 132 | transformation_matrix.template topLeftCorner<3,3>() = 133 | q_A_B_.getRotationMatrix(); 134 | transformation_matrix.template topRightCorner<3,1>() = A_t_A_B_; 135 | return transformation_matrix; 136 | } 137 | 138 | template 139 | Eigen::Matrix 140 | QuatTransformationTemplate::asVector() const { 141 | return (Eigen::Matrix() << 142 | q_A_B_.w(), q_A_B_.x(), q_A_B_.y(), q_A_B_.z(), A_t_A_B_).finished(); 143 | } 144 | 145 | template 146 | typename QuatTransformationTemplate::RotationMatrix 147 | QuatTransformationTemplate::getRotationMatrix() const { 148 | return q_A_B_.getRotationMatrix(); 149 | } 150 | 151 | template 152 | QuatTransformationTemplate 153 | QuatTransformationTemplate::operator*( 154 | const QuatTransformationTemplate& rhs) const { 155 | return QuatTransformationTemplate(q_A_B_ * rhs.q_A_B_, A_t_A_B_ + 156 | q_A_B_.rotate(rhs.A_t_A_B_)); 157 | } 158 | 159 | template 160 | typename QuatTransformationTemplate::Vector3 161 | QuatTransformationTemplate::transform( 162 | const typename QuatTransformationTemplate::Vector3& rhs) const { 163 | return q_A_B_.rotate(rhs) + A_t_A_B_; 164 | } 165 | 166 | template 167 | typename QuatTransformationTemplate::Matrix3X 168 | QuatTransformationTemplate::transformVectorized( 169 | const typename QuatTransformationTemplate::Matrix3X& rhs) const { 170 | CHECK_GT(rhs.cols(), 0); 171 | return q_A_B_.rotateVectorized(rhs).colwise() + A_t_A_B_; 172 | } 173 | 174 | template 175 | typename QuatTransformationTemplate::Vector3 176 | QuatTransformationTemplate::operator*( 177 | const typename QuatTransformationTemplate::Vector3& rhs) const { 178 | return transform(rhs); 179 | } 180 | 181 | template 182 | typename QuatTransformationTemplate::Vector4 183 | QuatTransformationTemplate::transform4( 184 | const typename QuatTransformationTemplate::Vector4& rhs) const { 185 | QuatTransformationTemplate::Vector4 rval; 186 | rval[3] = rhs[3]; 187 | rval.template head<3>() = 188 | q_A_B_.rotate(rhs.template head<3>()) + rhs[3]*A_t_A_B_; 189 | return rval; 190 | } 191 | 192 | template 193 | typename QuatTransformationTemplate::Vector3 194 | QuatTransformationTemplate::inverseTransform( 195 | const Vector3& rhs) const { 196 | return q_A_B_.inverseRotate(rhs - A_t_A_B_); 197 | } 198 | 199 | template 200 | typename QuatTransformationTemplate::Vector4 201 | QuatTransformationTemplate::inverseTransform4( 202 | const typename QuatTransformationTemplate::Vector4& rhs) const { 203 | typename QuatTransformationTemplate::Vector4 rval; 204 | rval.template head<3>() = q_A_B_.inverseRotate(rhs.template head<3>() - 205 | A_t_A_B_*rhs[3]); 206 | rval[3] = rhs[3]; 207 | return rval; 208 | } 209 | 210 | template 211 | QuatTransformationTemplate 212 | QuatTransformationTemplate::inverse() const { 213 | return QuatTransformationTemplate(q_A_B_.inverse(), -q_A_B_.inverseRotate(A_t_A_B_)); 214 | } 215 | 216 | template 217 | QuatTransformationTemplate 218 | QuatTransformationTemplate::inverted() const { 219 | return inverse(); 220 | } 221 | 222 | template 223 | typename QuatTransformationTemplate::Vector6 224 | QuatTransformationTemplate::log() const { 225 | return log(*this); 226 | } 227 | 228 | template 229 | QuatTransformationTemplate QuatTransformationTemplate::exp(const Vector6& vec) { 230 | return QuatTransformationTemplate(vec); 231 | } 232 | 233 | template 234 | typename QuatTransformationTemplate::Vector6 235 | QuatTransformationTemplate::log(const QuatTransformationTemplate& T) { 236 | AngleAxisTemplate angleaxis(T.q_A_B_); 237 | return (Vector6() << T.A_t_A_B_, T.q_A_B_.log()).finished(); 238 | } 239 | 240 | template 241 | QuatTransformationTemplate& 242 | QuatTransformationTemplate::setRandom() { 243 | q_A_B_.setRandom(); 244 | A_t_A_B_.setRandom(); 245 | return *this; 246 | } 247 | 248 | template 249 | QuatTransformationTemplate& 250 | QuatTransformationTemplate::setRandom(Scalar norm_translation) { 251 | setRandom(); 252 | A_t_A_B_.normalize(); 253 | A_t_A_B_ *= norm_translation; 254 | return *this; 255 | } 256 | 257 | template 258 | QuatTransformationTemplate& 259 | QuatTransformationTemplate::setRandom(Scalar norm_translation, Scalar angle_rad) { 260 | q_A_B_.setRandom(angle_rad); 261 | A_t_A_B_.setRandom().normalize(); 262 | A_t_A_B_ *= norm_translation; 263 | return *this; 264 | } 265 | 266 | template 267 | std::ostream & operator<<(std::ostream & out, 268 | const QuatTransformationTemplate& pose) { 269 | out << pose.getTransformationMatrix(); 270 | return out; 271 | } 272 | 273 | template 274 | bool QuatTransformationTemplate::operator==( 275 | const QuatTransformationTemplate& rhs) const { 276 | return q_A_B_ == rhs.q_A_B_ && A_t_A_B_ == rhs.A_t_A_B_; 277 | } 278 | 279 | template 280 | QuatTransformationTemplate QuatTransformationTemplate< 281 | Scalar>::constructAndRenormalizeRotation(const TransformationMatrix& T) { 282 | return QuatTransformationTemplate( 283 | Rotation::constructAndRenormalize( 284 | T.template topLeftCorner<3, 3>().eval()), 285 | T.template topRightCorner<3, 1>().eval()); 286 | } 287 | 288 | template 289 | template 290 | QuatTransformationTemplate 291 | QuatTransformationTemplate::cast() const { 292 | return QuatTransformationTemplate( 293 | getRotation().template cast(), 294 | getPosition().template cast()); 295 | } 296 | 297 | template 298 | QuatTransformationTemplate interpolateComponentwise( 299 | const QuatTransformationTemplate& T_a, 300 | const QuatTransformationTemplate& T_b, const double lambda) { 301 | CHECK_GE(lambda, 0.0); 302 | CHECK_LE(lambda, 1.0); 303 | const PositionTemplate p_int = 304 | T_a.getPosition() + lambda * (T_b.getPosition() - T_a.getPosition()); 305 | const Eigen::Quaternion q_int = 306 | T_a.getEigenQuaternion().slerp(lambda, T_b.getEigenQuaternion()); 307 | return QuatTransformationTemplate(q_int, p_int); 308 | } 309 | 310 | } // namespace minimal 311 | } // namespace kindr 312 | #endif // KINDR_MINIMAL_QUAT_TRANSFORMATION_H_INL_ 313 | -------------------------------------------------------------------------------- /minkindr/include/kindr/minimal/implementation/rotation-quaternion-inl.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MIN_ROTATION_QUATERNION_INL_H_ 26 | #define KINDR_MIN_ROTATION_QUATERNION_INL_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace kindr { 33 | namespace minimal { 34 | 35 | template 36 | struct EPS { 37 | static constexpr Scalar value() { return static_cast(1.0e-5); } 38 | static constexpr Scalar normalization_value() { 39 | return static_cast(1.0e-4); 40 | } 41 | }; 42 | template <> 43 | struct EPS { 44 | static constexpr double value() { return 1.0e-8; } 45 | static constexpr double normalization_value() { return 1.0e-4; } 46 | }; 47 | template <> 48 | struct EPS { 49 | static constexpr float value() { return 1.0e-5f; } 50 | static constexpr float normalization_value() { return 1.0e-4f; } 51 | }; 52 | 53 | /// \brief initialize to identity 54 | template 55 | RotationQuaternionTemplate::RotationQuaternionTemplate() 56 | : q_A_B_(Implementation::Identity()) {} 57 | 58 | /// \brief initialize from real and imaginary components (real first) 59 | template 60 | RotationQuaternionTemplate::RotationQuaternionTemplate( 61 | Scalar w, Scalar x, Scalar y, Scalar z) : 62 | q_A_B_(w,x,y,z) { 63 | CHECK_NEAR(squaredNorm(), static_cast(1.0), 64 | EPS::normalization_value()); 65 | } 66 | 67 | /// \brief initialize from real and imaginary components 68 | template 69 | RotationQuaternionTemplate::RotationQuaternionTemplate( 70 | Scalar real, 71 | const typename RotationQuaternionTemplate::Vector3& imaginary) : 72 | q_A_B_(real, imaginary[0], imaginary[1], imaginary[2]){ 73 | CHECK_NEAR(squaredNorm(), static_cast(1.0), 74 | EPS::normalization_value()); 75 | } 76 | 77 | /// \brief initialize from an Eigen quaternion 78 | template 79 | RotationQuaternionTemplate::RotationQuaternionTemplate( 80 | const Implementation& quaternion) : 81 | q_A_B_(quaternion){ 82 | CHECK_NEAR(squaredNorm(), static_cast(1.0), 83 | EPS::normalization_value()); 84 | } 85 | 86 | namespace detail { 87 | 88 | template 89 | inline bool isLessThenEpsilons4thRoot(Scalar_ x){ 90 | static const Scalar_ epsilon4thRoot = pow(std::numeric_limits::epsilon(), 1.0/4.0); 91 | return x < epsilon4thRoot; 92 | } 93 | 94 | template 95 | inline Scalar_ arcSinXOverX(Scalar_ x) { 96 | if(isLessThenEpsilons4thRoot(fabs(x))){ 97 | return Scalar_(1.0) + x * x * Scalar_(1.0/6.0); 98 | } 99 | return asin(x) / x; 100 | } 101 | } // namespace detail 102 | 103 | /// \brief initialize from axis-scaled angle vector 104 | template 105 | RotationQuaternionTemplate::RotationQuaternionTemplate( 106 | const Vector3& axis_scaled_angle) { 107 | *this = exp(axis_scaled_angle); 108 | } 109 | 110 | /// \brief initialize from a rotation matrix 111 | template 112 | RotationQuaternionTemplate::RotationQuaternionTemplate( 113 | const RotationMatrix& matrix) : 114 | q_A_B_(matrix) { 115 | CHECK(isValidRotationMatrix(matrix)) << matrix; 116 | } 117 | 118 | template 119 | RotationQuaternionTemplate 120 | RotationQuaternionTemplate::fromApproximateRotationMatrix( 121 | const RotationMatrix& matrix) { 122 | // We still want the input matrix to resemble a rotation matrix to avoid 123 | // bug hiding. 124 | CHECK(isValidRotationMatrix( 125 | matrix, static_cast(EPS::normalization_value()))); 126 | // http://people.csail.mit.edu/bkph/articles/Nearest_Orthonormal_Matrix.pdf 127 | // as discussed in https://github.com/ethz-asl/kindr/issues/55 , 128 | // code by Philipp Krüsi. 129 | Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullV); 130 | 131 | RotationMatrix correction = 132 | svd.matrixV().col(0) * svd.matrixV().col(0).transpose() / 133 | svd.singularValues()(0) + 134 | svd.matrixV().col(1) * svd.matrixV().col(1).transpose() / 135 | svd.singularValues()(1) + 136 | svd.matrixV().col(2) * svd.matrixV().col(2).transpose() / 137 | svd.singularValues()(2); 138 | 139 | return RotationQuaternionTemplate( 140 | RotationMatrix(matrix * correction)); 141 | } 142 | 143 | template 144 | RotationQuaternionTemplate::RotationQuaternionTemplate( 145 | const AngleAxisTemplate& angleAxis) : 146 | q_A_B_(angleAxis.toImplementation()){ 147 | 148 | } 149 | 150 | 151 | template 152 | RotationQuaternionTemplate::~RotationQuaternionTemplate() { 153 | 154 | } 155 | 156 | 157 | /// \brief the real component of the quaternion 158 | template 159 | Scalar RotationQuaternionTemplate::w() const { 160 | return q_A_B_.w(); 161 | } 162 | 163 | /// \brief the first imaginary component of the quaternion 164 | template 165 | Scalar RotationQuaternionTemplate::x() const { 166 | return q_A_B_.x(); 167 | } 168 | 169 | /// \brief the second imaginary component of the quaternion 170 | template 171 | Scalar RotationQuaternionTemplate::y() const { 172 | return q_A_B_.y(); 173 | } 174 | 175 | /// \brief the third imaginary component of the quaternion 176 | template 177 | Scalar RotationQuaternionTemplate::z() const { 178 | return q_A_B_.z(); 179 | } 180 | 181 | /// \brief assignment operator 182 | template 183 | RotationQuaternionTemplate& 184 | RotationQuaternionTemplate::operator=( 185 | const RotationQuaternionTemplate& rhs) { 186 | if(this != &rhs) { 187 | q_A_B_ = rhs.q_A_B_; 188 | } 189 | return *this; 190 | } 191 | 192 | /// \brief the imaginary components of the quaterion. 193 | template 194 | typename RotationQuaternionTemplate::Imaginary 195 | RotationQuaternionTemplate::imaginary() const { 196 | return Imaginary(q_A_B_.x(),q_A_B_.y(),q_A_B_.z()); 197 | } 198 | 199 | /// \brief get the components of the quaternion as a vector (real first) 200 | template 201 | typename RotationQuaternionTemplate::Vector4 202 | RotationQuaternionTemplate::vector() const { 203 | return Vector4(q_A_B_.w(), q_A_B_.x(),q_A_B_.y(),q_A_B_.z()); 204 | } 205 | 206 | /// \brief set the quaternion by its values (real, imaginary) 207 | template 208 | void RotationQuaternionTemplate::setValues(Scalar w, Scalar x, 209 | Scalar y, Scalar z) { 210 | q_A_B_ = Implementation(w,x,y,z); 211 | CHECK_NEAR(squaredNorm(), static_cast(1.0), 212 | static_cast(1e-4)); 213 | } 214 | 215 | /// \brief set the quaternion by its real and imaginary parts 216 | template 217 | void RotationQuaternionTemplate::setParts(Scalar real, 218 | const Imaginary& imag) { 219 | q_A_B_ = Implementation(real, imag[0], imag[1], imag[2]); 220 | CHECK_NEAR(squaredNorm(), static_cast(1.0), 221 | static_cast(1e-4)); 222 | } 223 | 224 | 225 | /// \brief get a copy of the representation that is unique 226 | template 227 | RotationQuaternionTemplate 228 | RotationQuaternionTemplate::getUnique() const { 229 | if(this->w() > 0) { 230 | return *this; 231 | } else if (this->w() < 0){ 232 | return RotationQuaternionTemplate( 233 | -this->w(),-this->x(),-this->y(),-this->z()); 234 | } 235 | // w == 0 236 | if(this->x() > 0) { 237 | return *this; 238 | } else if (this->x() < 0){ 239 | return RotationQuaternionTemplate( 240 | -this->w(),-this->x(),-this->y(),-this->z()); 241 | } 242 | // x == 0 243 | if(this->y() > 0) { 244 | return *this; 245 | } else if (this->y() < 0){ 246 | return RotationQuaternionTemplate( 247 | -this->w(),-this->x(),-this->y(),-this->z()); 248 | } 249 | // y == 0 250 | if(this->z() > 0) { // z must be either -1 or 1 in this case 251 | return *this; 252 | } else { 253 | return RotationQuaternionTemplate( 254 | -this->w(),-this->x(),-this->y(),-this->z()); 255 | } 256 | } 257 | 258 | /// \brief set the quaternion to its unique representation 259 | template 260 | RotationQuaternionTemplate& 261 | RotationQuaternionTemplate::setUnique() { 262 | *this = getUnique(); 263 | return *this; 264 | } 265 | 266 | /// \brief set the quaternion to identity 267 | template 268 | RotationQuaternionTemplate& 269 | RotationQuaternionTemplate::setIdentity() { 270 | q_A_B_.setIdentity(); 271 | return *this; 272 | } 273 | 274 | /// \brief set to random rotation 275 | template 276 | RotationQuaternionTemplate& 277 | RotationQuaternionTemplate::setRandom() { 278 | Vector4 coeffs; 279 | coeffs.setRandom().normalize(); 280 | q_A_B_ = Implementation(coeffs(0), coeffs(1), coeffs(2), coeffs(3)); 281 | this->setUnique(); 282 | return *this; 283 | } 284 | 285 | /// \brief set to random rotation with a given angle 286 | template 287 | RotationQuaternionTemplate& 288 | RotationQuaternionTemplate::setRandom(Scalar angle_rad) { 289 | Vector3 rotation_axis; 290 | rotation_axis.setRandom().normalize(); 291 | q_A_B_ = Implementation(Eigen::AngleAxis(angle_rad, rotation_axis)); 292 | return *this; 293 | } 294 | 295 | /// \brief get a copy of the quaternion inverted. 296 | template 297 | RotationQuaternionTemplate 298 | RotationQuaternionTemplate::inverse() const { 299 | return conjugated(); 300 | } 301 | 302 | /// \brief get a copy of the quaternion inverted. 303 | template 304 | RotationQuaternionTemplate 305 | RotationQuaternionTemplate::inverted() const { 306 | return inverse(); 307 | } 308 | 309 | /// \brief get a copy of the conjugate of the quaternion. 310 | template 311 | RotationQuaternionTemplate 312 | RotationQuaternionTemplate::conjugated() const { 313 | // Own implementation since Eigen::conjugate does not use the correct 314 | // scalar type for the greater than zero comparison. 315 | return RotationQuaternionTemplate( 316 | Implementation(q_A_B_.w(),-q_A_B_.x(),-q_A_B_.y(),-q_A_B_.z())); 317 | } 318 | 319 | 320 | /// \brief rotate a vector, v 321 | template 322 | typename RotationQuaternionTemplate::Vector3 323 | RotationQuaternionTemplate::rotate( 324 | const typename RotationQuaternionTemplate::Vector3& v) const { 325 | return q_A_B_*v; 326 | } 327 | 328 | /// \brief rotate vectors, v 329 | template 330 | typename RotationQuaternionTemplate::Matrix3X 331 | RotationQuaternionTemplate::rotateVectorized( 332 | const typename RotationQuaternionTemplate::Matrix3X& v) const { 333 | CHECK_GT(v.cols(), 0); 334 | return q_A_B_.toRotationMatrix() * v; 335 | } 336 | 337 | /// \brief rotate a vector, v 338 | template 339 | typename RotationQuaternionTemplate::Vector4 340 | RotationQuaternionTemplate::rotate4( 341 | const typename RotationQuaternionTemplate::Vector4& v) const { 342 | typename RotationQuaternionTemplate::Vector4 vprime; 343 | vprime[3] = v[3]; 344 | vprime.template head<3>() = q_A_B_*v.template head<3>(); 345 | return vprime; 346 | } 347 | 348 | 349 | /// \brief rotate a vector, v 350 | template 351 | typename RotationQuaternionTemplate::Vector3 352 | RotationQuaternionTemplate::inverseRotate( 353 | const typename RotationQuaternionTemplate::Vector3& v) const { 354 | return q_A_B_.inverse()*v; 355 | } 356 | 357 | 358 | /// \brief rotate a vector, v 359 | template 360 | typename RotationQuaternionTemplate::Vector4 361 | RotationQuaternionTemplate::inverseRotate4( 362 | const typename RotationQuaternionTemplate::Vector4& v) const { 363 | typename RotationQuaternionTemplate::Vector4 vprime; 364 | vprime[3] = v[3]; 365 | vprime.template head<3>() = q_A_B_.inverse()*v.template head<3>(); 366 | return vprime; 367 | } 368 | 369 | 370 | /// \brief cast to the implementation type 371 | template 372 | typename RotationQuaternionTemplate::Implementation& 373 | RotationQuaternionTemplate::toImplementation() { 374 | return q_A_B_; 375 | } 376 | 377 | /// \brief cast to the implementation type 378 | template 379 | const typename RotationQuaternionTemplate::Implementation& 380 | RotationQuaternionTemplate::toImplementation() const { 381 | return q_A_B_; 382 | } 383 | 384 | /// \brief get the norm of the quaternion 385 | template 386 | Scalar RotationQuaternionTemplate::norm() const { 387 | return q_A_B_.norm(); 388 | } 389 | 390 | /// \brief get the squared norm of the quaternion 391 | template 392 | Scalar RotationQuaternionTemplate::squaredNorm() const { 393 | return q_A_B_.squaredNorm(); 394 | } 395 | 396 | /// \brief enforce the unit length constraint 397 | template 398 | RotationQuaternionTemplate& 399 | RotationQuaternionTemplate::normalize() { 400 | q_A_B_.normalize(); 401 | return *this; 402 | } 403 | 404 | template 405 | RotationQuaternionTemplate 406 | RotationQuaternionTemplate::operator*( 407 | const RotationQuaternionTemplate& rhs) const { 408 | CHECK(!std::is_arithmetic::value) << "Please provide a specialized " 409 | "function for this specific arithmetic type. This function is only a " 410 | "workaround for non-arithmetic types."; 411 | Implementation result = q_A_B_ * rhs.q_A_B_; 412 | 413 | // Check if the multiplication has resulted in the quaternion no longer being 414 | // approximately normalized. 415 | // Cover the case of non-arithmetic types that may not provide an 416 | // implementation of std::abs. 417 | Scalar signed_norm_diff = result.squaredNorm() - static_cast(1.0); 418 | if ((signed_norm_diff > EPS::normalization_value()) || 419 | (signed_norm_diff < - EPS::normalization_value())) { 420 | // renormalize 421 | result.normalize(); 422 | } 423 | return RotationQuaternionTemplate(result); 424 | } 425 | 426 | template<> inline 427 | RotationQuaternionTemplate 428 | RotationQuaternionTemplate::operator*( 429 | const RotationQuaternionTemplate& rhs) const { 430 | Implementation result = q_A_B_ * rhs.q_A_B_; 431 | normalizationHelper(&result); 432 | return RotationQuaternionTemplate(result); 433 | } 434 | 435 | template<> inline 436 | RotationQuaternionTemplate 437 | RotationQuaternionTemplate::operator*( 438 | const RotationQuaternionTemplate& rhs) const { 439 | Implementation result = q_A_B_ * rhs.q_A_B_; 440 | normalizationHelper(&result); 441 | return RotationQuaternionTemplate(result); 442 | } 443 | 444 | template 445 | RotationQuaternionTemplate 446 | RotationQuaternionTemplate::operator*( 447 | const AngleAxisTemplate& rhs) const { 448 | return *this * RotationQuaternionTemplate(rhs); 449 | } 450 | 451 | template 452 | std::ostream& operator<<(std::ostream& out, 453 | const RotationQuaternionTemplate& rhs) { 454 | out << rhs.vector(); 455 | return out; 456 | } 457 | 458 | template 459 | typename RotationQuaternionTemplate::RotationMatrix 460 | RotationQuaternionTemplate::getRotationMatrix() const { 461 | return q_A_B_.matrix(); 462 | } 463 | 464 | template 465 | Scalar RotationQuaternionTemplate::getDisparityAngle( 466 | const RotationQuaternionTemplate& rhs) const{ 467 | return AngleAxisTemplate(rhs * this->inverse()).getUnique().angle(); 468 | } 469 | 470 | template 471 | Scalar RotationQuaternionTemplate::getDisparityAngle( 472 | const AngleAxisTemplate& rhs) const{ 473 | return AngleAxisTemplate(rhs * this->inverse()).getUnique().angle(); 474 | } 475 | 476 | template 477 | typename RotationQuaternionTemplate::Vector3 478 | RotationQuaternionTemplate::log(const RotationQuaternionTemplate& q) { 479 | const Eigen::Matrix a = q.imaginary(); 480 | const Scalar na = a.norm(); 481 | const Scalar eta = q.w(); 482 | Scalar scale; 483 | if(fabs(eta) < na){ // use eta because it is more precise than na to calculate the scale. No singularities here. 484 | // check sign of eta so that we can be sure that log(-q) = log(q) 485 | if (eta >= 0) { 486 | scale = acos(eta) / na; 487 | } else { 488 | scale = -acos(-eta) / na; 489 | } 490 | } else { 491 | /* 492 | * In this case more precision is in na than in eta so lets use na only to calculate the scale: 493 | * 494 | * assume first eta > 0 and 1 > na > 0. 495 | * u = asin (na) / na (this implies u in [1, pi/2], because na i in [0, 1] 496 | * sin (u * na) = na 497 | * sin^2 (u * na) = na^2 498 | * cos^2 (u * na) = 1 - na^2 499 | * (1 = ||q|| = eta^2 + na^2) 500 | * cos^2 (u * na) = eta^2 501 | * (eta > 0, u * na = asin(na) in [0, pi/2] => cos(u * na) >= 0 ) 502 | * cos (u * na) = eta 503 | * (u * na in [ 0, pi/2] ) 504 | * u = acos (eta) / na 505 | * 506 | * So the for eta > 0 it is acos(eta) / na == asin(na) / na. 507 | * From some geometric considerations (mirror the setting at the hyper plane q==0) it follows for eta < 0 that (pi - asin(na)) / na = acos(eta) / na. 508 | */ 509 | if(eta > 0) { 510 | // For asin(na)/ na the singularity na == 0 can be removed. We can ask (e.g. Wolfram alpha) for its series expansion at na = 0. And that is done in the following function. 511 | scale = detail::arcSinXOverX(na); 512 | } else { 513 | // the negative is here so that log(-q) == log(q) 514 | scale = -detail::arcSinXOverX(na); 515 | } 516 | } 517 | return a * (Scalar(2.0) * scale); 518 | } 519 | 520 | template 521 | RotationQuaternionTemplate 522 | RotationQuaternionTemplate::exp(const Vector3& dx) { 523 | // Method of implementing this function that is accurate to numerical precision from 524 | // Grassia, F. S. (1998). Practical parameterization of rotations using the exponential map. journal of graphics, gpu, and game tools, 3(3):29–48. 525 | double theta = dx.norm(); 526 | // na is 1/theta sin(theta/2) 527 | double na; 528 | if(detail::isLessThenEpsilons4thRoot(theta)){ 529 | static const double one_over_48 = 1.0/48.0; 530 | na = 0.5 + (theta * theta) * one_over_48; 531 | } else { 532 | na = sin(theta*0.5) / theta; 533 | } 534 | double ct = cos(theta*0.5); 535 | return RotationQuaternionTemplate(ct, 536 | dx[0]*na, 537 | dx[1]*na, 538 | dx[2]*na); 539 | } 540 | 541 | template 542 | typename RotationQuaternionTemplate::Vector3 543 | RotationQuaternionTemplate::log() const { 544 | return log(*this); 545 | } 546 | 547 | template 548 | bool RotationQuaternionTemplate::isValidRotationMatrix( 549 | const RotationMatrix& matrix) { 550 | return isValidRotationMatrix(matrix, EPS::value()); 551 | } 552 | 553 | template 554 | bool RotationQuaternionTemplate::isValidRotationMatrix( 555 | const RotationMatrix& matrix, const Scalar threshold) { 556 | if (std::fabs(matrix.determinant() - static_cast(1.0)) > threshold) { 557 | VLOG(200) << matrix.determinant(); 558 | VLOG(200) << matrix.determinant() - static_cast(1.0); 559 | return false; 560 | } 561 | if ((matrix * matrix.transpose() - 562 | RotationMatrix::Identity()).cwiseAbs().maxCoeff() > threshold) { 563 | VLOG(200) << matrix * matrix.transpose(); 564 | VLOG(200) << matrix * matrix.transpose() - RotationMatrix::Identity(); 565 | return false; 566 | } 567 | return true; 568 | } 569 | 570 | template 571 | template 572 | RotationQuaternionTemplate 573 | RotationQuaternionTemplate::cast() const { 574 | // renormalization needed to allow casting to increased precision 575 | return RotationQuaternionTemplate::constructAndRenormalize( 576 | getRotationMatrix().template cast()); 577 | } 578 | 579 | template 580 | void RotationQuaternionTemplate::normalizationHelper(Implementation* quaternion) const { 581 | CHECK_NOTNULL(quaternion); 582 | // check if the multiplication has resulted in the quaternion no longer being 583 | // approximately normalized. 584 | if (std::abs(quaternion->squaredNorm() - static_cast(1)) > 585 | EPS::normalization_value()) { 586 | // renormalize 587 | quaternion->normalize(); 588 | } 589 | } 590 | 591 | } // namespace minimal 592 | } // namespace kindr 593 | #endif // KINDR_MIN_ROTATION_QUATERNION_INL_H_ 594 | --------------------------------------------------------------------------------