mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 04:33:10 +08:00
consume PX4/Matrix repository and preserve history
This commit is contained in:
Daniel Agar
@@ -0,0 +1,9 @@
|
||||
Checks: 'clang-diagnostic-*,clang-analyzer-*,*,
|
||||
,-cppcoreguidelines-pro-type-vararg
|
||||
,-cppcoreguidelines-pro-bounds-array-to-pointer-decay
|
||||
,-cppcoreguidelines-pro-bounds-constant-array-index
|
||||
,-cppcoreguidelines-pro-bounds-pointer-arithmetic
|
||||
'
|
||||
WarningsAsErrors: '*'
|
||||
HeaderFilterRegex: '*.h, *.hpp, *.hh, *.hxx'
|
||||
|
||||
+53
@@ -0,0 +1,53 @@
|
||||
name: Tests
|
||||
|
||||
on:
|
||||
push:
|
||||
branches:
|
||||
- 'master'
|
||||
pull_request:
|
||||
branches:
|
||||
- '*'
|
||||
|
||||
jobs:
|
||||
build:
|
||||
runs-on: ubuntu-latest
|
||||
strategy:
|
||||
fail-fast: false # don't cancel if a job from the matrix fails
|
||||
matrix:
|
||||
compiler:
|
||||
- { compiler: GNU, CC: gcc, CXX: g++, FORMAT: ON }
|
||||
- { compiler: LLVM, CC: clang, CXX: clang++, FORMAT: OFF }
|
||||
#flavor: [ Release, Coverage ] # TODO
|
||||
flavor: [ Release ]
|
||||
exclude:
|
||||
- compiler: { compiler: LLVM, CC: clang, CXX: clang++ }
|
||||
flavor: Coverage
|
||||
|
||||
steps:
|
||||
- uses: actions/checkout@v2
|
||||
with:
|
||||
submodules: 'recursive'
|
||||
- name: Install Dependencies
|
||||
run: |
|
||||
sudo apt install -y clang cmake g++ gcc lcov
|
||||
- name: Fetch coveralls
|
||||
if: matrix.flavor == 'Coverage'
|
||||
run: |
|
||||
pip install --user cpp-coveralls
|
||||
|
||||
- name: Build & Run
|
||||
run: |
|
||||
cmake -DCMAKE_BUILD_TYPE=${{ matrix.flavor }} -DSUPPORT_STDIOSTREAM=ON -DTESTING=ON -DFORMAT=${{ matrix.compiler.FORMAT }} .
|
||||
make check
|
||||
|
||||
# TODO: enable
|
||||
# - name: Coveralls build
|
||||
# if: matrix.flavor == 'Coverage'
|
||||
# run: |
|
||||
# cpp-coveralls -i matrix
|
||||
# - name: Coveralls
|
||||
# if: matrix.flavor == 'Coverage'
|
||||
# uses: coverallsapp/github-action@master
|
||||
# with:
|
||||
# github-token: ${{ secrets.GITHUB_TOKEN }}
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
*.orig
|
||||
*.swp
|
||||
*~
|
||||
astyle/
|
||||
build*/
|
||||
cmake_install.cmake
|
||||
CMakeCache.txt
|
||||
CMakeFiles/
|
||||
compile_commands.json
|
||||
CPackConfig.cmake
|
||||
CPackSourceConfig.cmake
|
||||
CTestTestfile.cmake
|
||||
Makefile
|
||||
test/attitude
|
||||
test/cmake_install.cmake
|
||||
test/CMakeFiles/
|
||||
test/copyto
|
||||
test/coverage.info
|
||||
test/CTestTestfile.cmake
|
||||
test/filter
|
||||
test/hatvee
|
||||
test/helper
|
||||
test/integration
|
||||
test/inverse
|
||||
test/Makefile
|
||||
test/matrixAssignment
|
||||
test/matrixMult
|
||||
test/matrixScalarMult
|
||||
test/out/
|
||||
test/pseudoinverse
|
||||
test/setIdentity
|
||||
test/slice
|
||||
test/squareMatrix
|
||||
test/transpose
|
||||
test/vector
|
||||
test/vector2
|
||||
test/vector3
|
||||
test/vectorAssignment
|
||||
Testing/
|
||||
@@ -0,0 +1,180 @@
|
||||
cmake_minimum_required(VERSION 2.8)
|
||||
|
||||
set(VERSION_MAJOR "1")
|
||||
set(VERSION_MINOR "0")
|
||||
set(VERSION_PATCH "2")
|
||||
|
||||
project(matrix CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
if (NOT CMAKE_BUILD_TYPE)
|
||||
if(TESTING)
|
||||
set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type" FORCE)
|
||||
else()
|
||||
set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
|
||||
endif()
|
||||
message(STATUS "set build type to ${CMAKE_BUILD_TYPE}")
|
||||
endif()
|
||||
|
||||
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage")
|
||||
|
||||
option(SUPPORT_STDIOSTREAM "If enabled provides support for << operator (as used with std::cout)" OFF)
|
||||
option(TESTING "Enable testing" OFF)
|
||||
option(FORMAT "Enable formatting" OFF)
|
||||
option(COV_HTML "Display html for coverage" OFF)
|
||||
option(ASAN "Enable address sanitizer" OFF)
|
||||
option(UBSAN "Enable undefined behaviour sanitizer" OFF)
|
||||
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
include_directories(${CMAKE_SOURCE_DIR})
|
||||
|
||||
if(SUPPORT_STDIOSTREAM)
|
||||
add_definitions(-DSUPPORT_STDIOSTREAM)
|
||||
endif()
|
||||
|
||||
if (("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") OR ("${CMAKE_CXX_COMPILER_ID}" MATCHES "AppleClang"))
|
||||
set(CMAKE_CXX_FLAGS_COVERAGE
|
||||
"--coverage -ftest-coverage -fdiagnostics-absolute-paths -O0 -fprofile-arcs -fno-inline-functions -fno-elide-constructors"
|
||||
CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE)
|
||||
set(CMAKE_EXE_LINKER_FLAGS_COVERAGE
|
||||
"-ftest-coverage -fdiagnostics-absolute-paths"
|
||||
CACHE STRING "Flags used for linking binaries during coverage builds" FORCE)
|
||||
else()
|
||||
set(CMAKE_CXX_FLAGS_COVERAGE
|
||||
# Add when GCC 9 or later is available as part of the default toolchain: -fprofile-abs-path
|
||||
"--coverage -fprofile-arcs -ftest-coverage -O0 -fno-default-inline -fno-inline -fno-inline-small-functions -fno-elide-constructors"
|
||||
CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE)
|
||||
set(CMAKE_EXE_LINKER_FLAGS_COVERAGE
|
||||
"--coverage -ftest-coverage -lgcov"
|
||||
CACHE STRING "Flags used for linking binaries during coverage builds" FORCE)
|
||||
endif()
|
||||
mark_as_advanced(CMAKE_CXX_FLAGS_COVERAGE CMAKE_C_FLAGS_COVERAGE CMAKE_EXE_LINKER_FLAGS_COVERAGE)
|
||||
|
||||
add_compile_options(
|
||||
-pedantic
|
||||
-Wall
|
||||
-Warray-bounds
|
||||
-Wcast-align
|
||||
-Wcast-qual
|
||||
-Wconversion
|
||||
-Wctor-dtor-privacy
|
||||
-Wdisabled-optimization
|
||||
-Werror
|
||||
-Wextra
|
||||
-Wfloat-equal
|
||||
-Wformat-security
|
||||
-Wformat=2
|
||||
-Winit-self
|
||||
-Wlogical-op
|
||||
-Wmissing-declarations
|
||||
-Wmissing-include-dirs
|
||||
-Wno-sign-compare
|
||||
-Wno-unused
|
||||
-Wno-unused-parameter
|
||||
-Wnoexcept
|
||||
-Wold-style-cast
|
||||
-Woverloaded-virtual
|
||||
-Wpointer-arith
|
||||
-Wredundant-decls
|
||||
-Wreorder
|
||||
-Wshadow
|
||||
-Wsign-conversion
|
||||
-Wsign-promo
|
||||
-Wstrict-null-sentinel
|
||||
-Wswitch-default
|
||||
-Wundef
|
||||
-Wuninitialized
|
||||
-Wunused-variable
|
||||
)
|
||||
|
||||
# clang tolerate unknown gcc options
|
||||
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
|
||||
add_compile_options(
|
||||
-Wno-error=unused-command-line-argument-hard-error-in-future
|
||||
-Wno-unknown-warning-option
|
||||
)
|
||||
else()
|
||||
add_compile_options(
|
||||
-Wstrict-overflow=5
|
||||
)
|
||||
endif()
|
||||
|
||||
# santiziers (ASAN, UBSAN)
|
||||
if(ASAN)
|
||||
message(STATUS "address sanitizer enabled")
|
||||
|
||||
# environment variables
|
||||
# ASAN_OPTIONS=detect_stack_use_after_return=1
|
||||
# ASAN_OPTIONS=check_initialization_order=1
|
||||
|
||||
add_compile_options(
|
||||
-fsanitize=address
|
||||
-g3
|
||||
-O1
|
||||
-fno-omit-frame-pointer
|
||||
)
|
||||
|
||||
set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address)
|
||||
|
||||
elseif(UBSAN)
|
||||
message(STATUS "undefined behaviour sanitizer enabled")
|
||||
|
||||
add_compile_options(
|
||||
-fsanitize=undefined
|
||||
-fsanitize=integer
|
||||
-g3
|
||||
-O1
|
||||
-fno-omit-frame-pointer
|
||||
)
|
||||
|
||||
set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=undefined)
|
||||
endif()
|
||||
|
||||
|
||||
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
|
||||
|
||||
if(TESTING)
|
||||
enable_testing()
|
||||
add_subdirectory(test)
|
||||
add_dependencies(check test_build)
|
||||
|
||||
add_custom_target(clang-tidy COMMAND clang-tidy -p . ${CMAKE_SOURCE_DIR}/test/*.cpp)
|
||||
add_dependencies(clang-tidy test_build)
|
||||
endif()
|
||||
|
||||
if(FORMAT)
|
||||
set(astyle_exe ${CMAKE_BINARY_DIR}/astyle/src/bin/astyle)
|
||||
add_custom_command(OUTPUT ${astyle_exe}
|
||||
COMMAND wget http://sourceforge.net/projects/astyle/files/astyle/astyle%202.06/astyle_2.06_linux.tar.gz -O /tmp/astyle.tar.gz
|
||||
COMMAND tar -xvf /tmp/astyle.tar.gz
|
||||
COMMAND cd astyle/src && make -f ../build/gcc/Makefile
|
||||
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
|
||||
)
|
||||
|
||||
add_custom_target(check_format
|
||||
COMMAND scripts/format.sh ${astyle_exe} 0
|
||||
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
|
||||
DEPENDS ${astyle_exe}
|
||||
VERBATIM
|
||||
)
|
||||
|
||||
add_custom_target(format
|
||||
COMMAND scripts/format.sh ${astyle_exe} 1
|
||||
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
|
||||
VERBATIM
|
||||
DEPENDS ${astyle_exe}
|
||||
)
|
||||
|
||||
add_dependencies(check check_format)
|
||||
endif()
|
||||
|
||||
set(CPACK_PACKAGE_VERSION_MAJOR ${VERSION_MAJOR})
|
||||
set(CPACK_PACKAGE_VERSION_MINOR ${VERSION_MINOR})
|
||||
set(CPACK_PACKAGE_VERSION_PATCH ${VERSION_PATCH})
|
||||
set(CPACK_PACKAGE_CONTACT "james.goppert@gmail.com")
|
||||
include(CPack)
|
||||
|
||||
# vim: set et fenc=utf-8 ft=cmake ff=unix sts=0 sw=4 ts=4 :
|
||||
@@ -0,0 +1,29 @@
|
||||
BSD 3-Clause License
|
||||
|
||||
Copyright (c) 2015, PX4 Pro Drone Autopilot
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
* Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
@@ -0,0 +1,146 @@
|
||||
# matrix [](https://travis-ci.org/PX4/Matrix) [](https://coveralls.io/github/PX4/Matrix?branch=master)
|
||||
|
||||
A simple and efficient template based matrix library.
|
||||
|
||||
## License
|
||||
* (BSD) The Matrix library is licensed under a permissive 3-clause BSD license. Contributions must be made under the same license.
|
||||
|
||||
## Features
|
||||
* Compile time size checking.
|
||||
* Euler angle (321), DCM, Quaternion conversion through constructors.
|
||||
* High testing coverage.
|
||||
|
||||
## Limitations
|
||||
* No dynamically sized matrices.
|
||||
|
||||
## Library Overview
|
||||
|
||||
* matrix/math.hpp : Provides matrix math routines.
|
||||
* Matrix (MxN)
|
||||
* Square Matrix (MxM, has inverse)
|
||||
* Vector (Mx1)
|
||||
* Scalar (1x1)
|
||||
* Quaternion
|
||||
* Dcm
|
||||
* Euler (Body 321)
|
||||
* Axis Angle
|
||||
|
||||
* matrix/filter.hpp : Provides filtering routines.
|
||||
* kalman_correct
|
||||
|
||||
* matrix/integrate.hpp : Provides integration routines.
|
||||
* integrate_rk4 (Runge-Kutta 4th order)
|
||||
|
||||
## Testing
|
||||
The tests can be executed as following:
|
||||
```
|
||||
mkdir build
|
||||
cd build
|
||||
cmake -DTESTING=ON ..
|
||||
make check
|
||||
```
|
||||
|
||||
## Formatting
|
||||
The format can be checked as following:
|
||||
```
|
||||
mkdir build
|
||||
cd build
|
||||
cmake -DFORMAT=ON ..
|
||||
make check_format
|
||||
```
|
||||
|
||||
|
||||
## Example
|
||||
|
||||
See the test directory for detailed examples. Some simple examples are included below:
|
||||
|
||||
```c++
|
||||
// define an euler angle (Body 3(yaw)-2(pitch)-1(roll) rotation)
|
||||
float roll = 0.1f;
|
||||
float pitch = 0.2f;
|
||||
float yaw = 0.3f;
|
||||
Eulerf euler(roll, pitch, yaw);
|
||||
|
||||
// convert to quaternion from euler
|
||||
Quatf q_nb(euler);
|
||||
|
||||
// convert to DCM from quaternion
|
||||
Dcmf dcm(q_nb);
|
||||
|
||||
// you can assign a rotation instance that already exist to another rotation instance, e.g.
|
||||
dcm = euler;
|
||||
|
||||
// you can also directly create a DCM instance from euler angles like this
|
||||
dcm = Eulerf(roll, pitch, yaw);
|
||||
|
||||
// create an axis angle representation from euler angles
|
||||
AxisAngle<float> axis_angle = euler;
|
||||
|
||||
// use axis angle to initialize a DCM
|
||||
Dcmf dcm2(AxisAngle(1, 2, 3));
|
||||
|
||||
// use axis angle with axis/angle separated to init DCM
|
||||
Dcmf dcm3(AxisAngle(Vector3f(1, 0, 0), 0.2));
|
||||
|
||||
// do some kalman filtering
|
||||
const size_t n_x = 5;
|
||||
const size_t n_y = 3;
|
||||
|
||||
// define matrix sizes
|
||||
SquareMatrix<float, n_x> P;
|
||||
Vector<float, n_x> x;
|
||||
Vector<float, n_y> y;
|
||||
Matrix<float, n_y, n_x> C;
|
||||
SquareMatrix<float, n_y> R;
|
||||
SquareMatrix<float, n_y> S;
|
||||
Matrix<float, n_x, n_y> K;
|
||||
|
||||
// define measurement matrix
|
||||
C = zero<float, n_y, n_x>(); // or C.setZero()
|
||||
C(0,0) = 1;
|
||||
C(1,1) = 2;
|
||||
C(2,2) = 3;
|
||||
|
||||
// set x to zero
|
||||
x = zero<float, n_x, 1>(); // or x.setZero()
|
||||
|
||||
// set P to identity * 0.01
|
||||
P = eye<float, n_x>()*0.01;
|
||||
|
||||
// set R to identity * 0.1
|
||||
R = eye<float, n_y>()*0.1;
|
||||
|
||||
// measurement
|
||||
y(0) = 1;
|
||||
y(1) = 2;
|
||||
y(2) = 3;
|
||||
|
||||
// innovation
|
||||
r = y - C*x;
|
||||
|
||||
// innovations variance
|
||||
S = C*P*C.T() + R;
|
||||
|
||||
// Kalman gain matrix
|
||||
K = P*C.T()*S.I();
|
||||
// S.I() is the inverse, defined for SquareMatrix
|
||||
|
||||
// correction
|
||||
x += K*r;
|
||||
|
||||
// slicing
|
||||
float data[9] = {0, 2, 3,
|
||||
4, 5, 6,
|
||||
7, 8, 10
|
||||
};
|
||||
SquareMatrix<float, 3> A(data);
|
||||
|
||||
// Slice a 3,3 matrix starting at row 1, col 0,
|
||||
// with size 2 x 3, warning, no size checking
|
||||
Matrix<float, 2, 3> B(A.slice<2, 3>(1, 0));
|
||||
|
||||
// this results in:
|
||||
// 4, 5, 6
|
||||
// 7, 8, 10
|
||||
```
|
||||
<!-- vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
|
||||
@@ -0,0 +1 @@
|
||||
.ipynb_checkpoints/
|
||||
File diff suppressed because one or more lines are too long
Binary file not shown.
@@ -0,0 +1,160 @@
|
||||
/**
|
||||
* @file AxisAngle.hpp
|
||||
*
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template <typename Type>
|
||||
class Dcm;
|
||||
|
||||
template <typename Type>
|
||||
class Euler;
|
||||
|
||||
template <typename Type>
|
||||
class AxisAngle;
|
||||
|
||||
/**
|
||||
* AxisAngle class
|
||||
*
|
||||
* The rotation between two coordinate frames is
|
||||
* described by this class.
|
||||
*/
|
||||
template<typename Type>
|
||||
class AxisAngle : public Vector<Type, 3>
|
||||
{
|
||||
public:
|
||||
using Matrix31 = Matrix<Type, 3, 1>;
|
||||
|
||||
/**
|
||||
* Constructor from array
|
||||
*
|
||||
* @param data_ array
|
||||
*/
|
||||
explicit AxisAngle(const Type data_[3]) :
|
||||
Vector<Type, 3>(data_)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Standard constructor
|
||||
*/
|
||||
AxisAngle() = default;
|
||||
|
||||
/**
|
||||
* Constructor from Matrix31
|
||||
*
|
||||
* @param other Matrix31 to copy
|
||||
*/
|
||||
AxisAngle(const Matrix31 &other) :
|
||||
Vector<Type, 3>(other)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from quaternion
|
||||
*
|
||||
* This sets the instance from a quaternion representing coordinate transformation from
|
||||
* frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
|
||||
* by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
|
||||
*
|
||||
* @param q quaternion
|
||||
*/
|
||||
AxisAngle(const Quaternion<Type> &q)
|
||||
{
|
||||
AxisAngle &v = *this;
|
||||
Type mag = q.imag().norm();
|
||||
if (fabs(mag) >= Type(1e-10)) {
|
||||
v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
|
||||
} else {
|
||||
v = q.imag() * Type(Type(2) * Type(sign(q(0))));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from dcm
|
||||
*
|
||||
* Instance is initialized from a dcm representing coordinate transformation
|
||||
* from frame 2 to frame 1.
|
||||
*
|
||||
* @param dcm dcm to set quaternion to
|
||||
*/
|
||||
AxisAngle(const Dcm<Type> &dcm)
|
||||
{
|
||||
AxisAngle &v = *this;
|
||||
v = AxisAngle<Type>(Quaternion<Type>(dcm));
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from euler angles
|
||||
*
|
||||
* This sets the instance to a quaternion representing coordinate transformation from
|
||||
* frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
|
||||
* by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
|
||||
*
|
||||
* @param euler euler angle instance
|
||||
*/
|
||||
AxisAngle(const Euler<Type> &euler)
|
||||
{
|
||||
AxisAngle &v = *this;
|
||||
v = AxisAngle<Type>(Quaternion<Type>(euler));
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from 3 axis angle values (unit vector * angle)
|
||||
*
|
||||
* @param x r_x*angle
|
||||
* @param y r_y*angle
|
||||
* @param z r_z*angle
|
||||
*/
|
||||
AxisAngle(Type x, Type y, Type z)
|
||||
{
|
||||
AxisAngle &v = *this;
|
||||
v(0) = x;
|
||||
v(1) = y;
|
||||
v(2) = z;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from axis and angle
|
||||
*
|
||||
* @param axis An axis of rotation, normalized if not unit length
|
||||
* @param angle The amount to rotate
|
||||
*/
|
||||
AxisAngle(const Matrix31 & axis_, Type angle_)
|
||||
{
|
||||
AxisAngle &v = *this;
|
||||
// make sure axis is a unit vector
|
||||
Vector<Type, 3> a = axis_;
|
||||
a = a.unit();
|
||||
v(0) = a(0)*angle_;
|
||||
v(1) = a(1)*angle_;
|
||||
v(2) = a(2)*angle_;
|
||||
}
|
||||
|
||||
|
||||
Vector<Type, 3> axis() {
|
||||
if (Vector<Type, 3>::norm() > 0) {
|
||||
return Vector<Type, 3>::unit();
|
||||
} else {
|
||||
return Vector3<Type>(1, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
Type angle() {
|
||||
return Vector<Type, 3>::norm();
|
||||
}
|
||||
};
|
||||
|
||||
using AxisAnglef = AxisAngle<float>;
|
||||
using AxisAngled = AxisAngle<double>;
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,187 @@
|
||||
/**
|
||||
* @file Dcm.hpp
|
||||
*
|
||||
* A direction cosine matrix class.
|
||||
* All rotations and axis systems follow the right-hand rule.
|
||||
*
|
||||
* This library uses the convention that premultiplying a three dimensional
|
||||
* vector represented in coordinate system 1 will apply a rotation from coordinate system
|
||||
* 1 to coordinate system 2 to the vector.
|
||||
* Likewise, a matrix instance of this class also represents a coordinate transformation
|
||||
* from frame 2 to frame 1.
|
||||
*
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template<typename Type>
|
||||
class Quaternion;
|
||||
|
||||
template<typename Type>
|
||||
class Euler;
|
||||
|
||||
template<typename Type>
|
||||
class AxisAngle;
|
||||
|
||||
/**
|
||||
* Direction cosine matrix class
|
||||
*
|
||||
* The rotation between two coordinate frames is
|
||||
* described by this class.
|
||||
*/
|
||||
template<typename Type>
|
||||
class Dcm : public SquareMatrix<Type, 3>
|
||||
{
|
||||
public:
|
||||
using Vector3 = Matrix<Type, 3, 1>;
|
||||
|
||||
/**
|
||||
* Standard constructor
|
||||
*
|
||||
* Initializes to identity
|
||||
*/
|
||||
Dcm() : SquareMatrix<Type, 3>(eye<Type, 3>()) {}
|
||||
|
||||
/**
|
||||
* Constructor from array
|
||||
*
|
||||
* @param _data pointer to array
|
||||
*/
|
||||
explicit Dcm(const Type data_[3][3]) : SquareMatrix<Type, 3>(data_)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from array
|
||||
*
|
||||
* @param _data pointer to array
|
||||
*/
|
||||
explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*
|
||||
* @param other Matrix33 to set dcm to
|
||||
*/
|
||||
Dcm(const Matrix<Type, 3, 3> &other) : SquareMatrix<Type, 3>(other)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from quaternion
|
||||
*
|
||||
* Instance is initialized from quaternion representing
|
||||
* coordinate transformation from frame 2 to frame 1.
|
||||
*
|
||||
* @param q quaternion to set dcm to
|
||||
*/
|
||||
Dcm(const Quaternion<Type> &q)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
const Type a = q(0);
|
||||
const Type b = q(1);
|
||||
const Type c = q(2);
|
||||
const Type d = q(3);
|
||||
const Type aa = a * a;
|
||||
const Type ab = a * b;
|
||||
const Type ac = a * c;
|
||||
const Type ad = a * d;
|
||||
const Type bb = b * b;
|
||||
const Type bc = b * c;
|
||||
const Type bd = b * d;
|
||||
const Type cc = c * c;
|
||||
const Type cd = c * d;
|
||||
const Type dd = d * d;
|
||||
dcm(0, 0) = aa + bb - cc - dd;
|
||||
dcm(0, 1) = Type(2) * (bc - ad);
|
||||
dcm(0, 2) = Type(2) * (ac + bd);
|
||||
dcm(1, 0) = Type(2) * (bc + ad);
|
||||
dcm(1, 1) = aa - bb + cc - dd;
|
||||
dcm(1, 2) = Type(2) * (cd - ab);
|
||||
dcm(2, 0) = Type(2) * (bd - ac);
|
||||
dcm(2, 1) = Type(2) * (ab + cd);
|
||||
dcm(2, 2) = aa - bb - cc + dd;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from euler angles
|
||||
*
|
||||
* This sets the transformation matrix from frame 2 to frame 1 where the rotation
|
||||
* from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
|
||||
*
|
||||
*
|
||||
* @param euler euler angle instance
|
||||
*/
|
||||
Dcm(const Euler<Type> &euler)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
Type cosPhi = Type(cos(euler.phi()));
|
||||
Type sinPhi = Type(sin(euler.phi()));
|
||||
Type cosThe = Type(cos(euler.theta()));
|
||||
Type sinThe = Type(sin(euler.theta()));
|
||||
Type cosPsi = Type(cos(euler.psi()));
|
||||
Type sinPsi = Type(sin(euler.psi()));
|
||||
|
||||
dcm(0, 0) = cosThe * cosPsi;
|
||||
dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
|
||||
dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
|
||||
|
||||
dcm(1, 0) = cosThe * sinPsi;
|
||||
dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
|
||||
dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
|
||||
|
||||
dcm(2, 0) = -sinThe;
|
||||
dcm(2, 1) = sinPhi * cosThe;
|
||||
dcm(2, 2) = cosPhi * cosThe;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Constructor from axis angle
|
||||
*
|
||||
* This sets the transformation matrix from frame 2 to frame 1 where the rotation
|
||||
* from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
|
||||
*
|
||||
*
|
||||
* @param euler euler angle instance
|
||||
*/
|
||||
Dcm(const AxisAngle<Type> &aa)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
dcm = Quaternion<Type>(aa);
|
||||
}
|
||||
|
||||
Vector<Type, 3> vee() const // inverse to Vector.hat() operation
|
||||
{
|
||||
const Dcm &A(*this);
|
||||
Vector<Type, 3> v;
|
||||
v(0) = -A(1, 2);
|
||||
v(1) = A(0, 2);
|
||||
v(2) = -A(0, 1);
|
||||
return v;
|
||||
}
|
||||
|
||||
void renormalize()
|
||||
{
|
||||
/* renormalize rows */
|
||||
for (size_t r = 0; r < 3; r++) {
|
||||
matrix::Vector3<Type> rvec(Matrix<Type,1,3>(this->Matrix<Type,3,3>::row(r)).transpose());
|
||||
this->Matrix<Type,3,3>::row(r) = rvec.normalized();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
using Dcmf = Dcm<float>;
|
||||
using Dcmd = Dcm<double>;
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,373 @@
|
||||
/**
|
||||
* @file Dual.hpp
|
||||
*
|
||||
* This is a dual number type for calculating automatic derivatives.
|
||||
*
|
||||
* Based roughly on the methods described in:
|
||||
* Automatic Differentiation, C++ Templates and Photogrammetry, by Dan Piponi
|
||||
* and
|
||||
* Ceres Solver's excellent Jet.h
|
||||
*
|
||||
* @author Julian Kent <julian@auterion.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template <typename Type, size_t M>
|
||||
class Vector;
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
struct Dual
|
||||
{
|
||||
static constexpr size_t WIDTH = N;
|
||||
|
||||
Scalar value {};
|
||||
Vector<Scalar, N> derivative;
|
||||
|
||||
Dual() = default;
|
||||
|
||||
explicit Dual(Scalar v, size_t inputDimension = 65535)
|
||||
{
|
||||
value = v;
|
||||
if (inputDimension < N) {
|
||||
derivative(inputDimension) = Scalar(1);
|
||||
}
|
||||
}
|
||||
|
||||
explicit Dual(Scalar v, const Vector<Scalar, N>& d) :
|
||||
value(v), derivative(d)
|
||||
{}
|
||||
|
||||
Dual<Scalar, N>& operator=(const Scalar& a)
|
||||
{
|
||||
derivative.setZero();
|
||||
value = a;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Dual<Scalar, N>& operator +=(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return (*this = *this + a);
|
||||
}
|
||||
|
||||
Dual<Scalar, N>& operator *=(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return (*this = *this * a);
|
||||
}
|
||||
|
||||
Dual<Scalar, N>& operator -=(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return (*this = *this - a);
|
||||
}
|
||||
|
||||
Dual<Scalar, N>& operator /=(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return (*this = *this / a);
|
||||
}
|
||||
|
||||
Dual<Scalar, N>& operator +=(Scalar a)
|
||||
{
|
||||
return (*this = *this + a);
|
||||
}
|
||||
|
||||
Dual<Scalar, N>& operator -=(Scalar a)
|
||||
{
|
||||
return (*this = *this - a);
|
||||
}
|
||||
|
||||
Dual<Scalar, N>& operator *=(Scalar a)
|
||||
{
|
||||
return (*this = *this * a);
|
||||
}
|
||||
|
||||
Dual<Scalar, N>& operator /=(Scalar a)
|
||||
{
|
||||
return (*this = *this / a);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
// operators
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return a;
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return Dual<Scalar, N>(-a.value, -a.derivative);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
return Dual<Scalar, N>(a.value + b.value, a.derivative + b.derivative);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
return a + (-b);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, Scalar b)
|
||||
{
|
||||
return Dual<Scalar, N>(a.value + b, a.derivative);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, Scalar b)
|
||||
{
|
||||
return a + (-b);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator+(Scalar a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
return Dual<Scalar, N>(a + b.value, b.derivative);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator-(Scalar a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
return a + (-b);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
return Dual<Scalar, N>(a.value * b.value, a.value * b.derivative + b.value * a.derivative);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, Scalar b)
|
||||
{
|
||||
return Dual<Scalar, N>(a.value * b, a.derivative * b);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator*(Scalar a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
return b * a;
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
const Scalar inv_b_real = Scalar(1) / b.value;
|
||||
return Dual<Scalar, N>(a.value * inv_b_real, a.derivative * inv_b_real -
|
||||
a.value * b.derivative * inv_b_real * inv_b_real);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, Scalar b)
|
||||
{
|
||||
return a * (Scalar(1) / b);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> operator/(Scalar a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
const Scalar inv_b_real = Scalar(1) / b.value;
|
||||
return Dual<Scalar, N>(a * inv_b_real, (-inv_b_real * a * inv_b_real) * b.derivative);
|
||||
}
|
||||
|
||||
// basic math
|
||||
|
||||
// sqrt
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> sqrt(const Dual<Scalar, N>& a)
|
||||
{
|
||||
Scalar real = sqrt(a.value);
|
||||
return Dual<Scalar, N>(real, a.derivative * (Scalar(1) / (Scalar(2) * real)));
|
||||
}
|
||||
|
||||
// abs
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> abs(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return a.value >= Scalar(0) ? a : -a;
|
||||
}
|
||||
|
||||
// ceil
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> ceil(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return Dual<Scalar, N>(ceil(a.value));
|
||||
}
|
||||
|
||||
// floor
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> floor(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return Dual<Scalar, N>(floor(a.value));
|
||||
}
|
||||
|
||||
// fmod
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> fmod(const Dual<Scalar, N>& a, Scalar mod)
|
||||
{
|
||||
return Dual<Scalar, N>(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative);
|
||||
}
|
||||
|
||||
// max
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> max(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
return a.value >= b.value ? a : b;
|
||||
}
|
||||
|
||||
// min
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> min(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
return a.value < b.value ? a : b;
|
||||
}
|
||||
|
||||
// isnan
|
||||
template <typename Scalar>
|
||||
bool IsNan(Scalar a)
|
||||
{
|
||||
return isnan(a);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
bool IsNan(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return IsNan(a.value);
|
||||
}
|
||||
|
||||
// isfinite
|
||||
template <typename Scalar>
|
||||
bool IsFinite(Scalar a)
|
||||
{
|
||||
return isfinite(a);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
bool IsFinite(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return IsFinite(a.value);
|
||||
}
|
||||
|
||||
// isinf
|
||||
template <typename Scalar>
|
||||
bool IsInf(Scalar a)
|
||||
{
|
||||
return isinf(a);
|
||||
}
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
bool IsInf(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return IsInf(a.value);
|
||||
}
|
||||
|
||||
// trig
|
||||
|
||||
// sin
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> sin(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return Dual<Scalar, N>(sin(a.value), cos(a.value) * a.derivative);
|
||||
}
|
||||
|
||||
// cos
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> cos(const Dual<Scalar, N>& a)
|
||||
{
|
||||
return Dual<Scalar, N>(cos(a.value), -sin(a.value) * a.derivative);
|
||||
}
|
||||
|
||||
// tan
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> tan(const Dual<Scalar, N>& a)
|
||||
{
|
||||
Scalar real = tan(a.value);
|
||||
return Dual<Scalar, N>(real, (Scalar(1) + real * real) * a.derivative);
|
||||
}
|
||||
|
||||
// asin
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> asin(const Dual<Scalar, N>& a)
|
||||
{
|
||||
Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
|
||||
return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative);
|
||||
}
|
||||
|
||||
// acos
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> acos(const Dual<Scalar, N>& a)
|
||||
{
|
||||
Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
|
||||
return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative);
|
||||
}
|
||||
|
||||
// atan
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> atan(const Dual<Scalar, N>& a)
|
||||
{
|
||||
Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value);
|
||||
return Dual<Scalar, N>(atan(a.value), atan_d * a.derivative);
|
||||
}
|
||||
|
||||
// atan2
|
||||
template <typename Scalar, size_t N>
|
||||
Dual<Scalar, N> atan2(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
|
||||
{
|
||||
// derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify
|
||||
Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value);
|
||||
return Dual<Scalar, N>(atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d);
|
||||
}
|
||||
|
||||
// retrieve the derivative elements of a vector of Duals into a matrix
|
||||
template <typename Scalar, size_t M, size_t N>
|
||||
Matrix<Scalar, M, N> collectDerivatives(const Matrix<Dual<Scalar, N>, M, 1>& input)
|
||||
{
|
||||
Matrix<Scalar, M, N> jac;
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
jac.row(i) = input(i, 0).derivative;
|
||||
}
|
||||
return jac;
|
||||
}
|
||||
|
||||
// retrieve the real (non-derivative) elements of a matrix of Duals into an equally sized matrix
|
||||
template <typename Scalar, size_t M, size_t N, size_t D>
|
||||
Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N>& input)
|
||||
{
|
||||
Matrix<Scalar, M, N> r;
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
r(i,j) = input(i,j).value;
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
#if defined(SUPPORT_STDIOSTREAM)
|
||||
template<typename Type, size_t N>
|
||||
std::ostream& operator<<(std::ostream& os,
|
||||
const matrix::Dual<Type, N>& dual)
|
||||
{
|
||||
os << "[";
|
||||
os << std::setw(10) << dual.value << ";";
|
||||
for (size_t j = 0; j < N; ++j) {
|
||||
os << "\t";
|
||||
os << std::setw(10) << static_cast<double>(dual.derivative(j));
|
||||
}
|
||||
os << "]";
|
||||
return os;
|
||||
}
|
||||
#endif // defined(SUPPORT_STDIOSTREAM)
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,157 @@
|
||||
/**
|
||||
* @file Euler.hpp
|
||||
*
|
||||
* All rotations and axis systems follow the right-hand rule
|
||||
*
|
||||
* An instance of this class defines a rotation from coordinate frame 1 to coordinate frame 2.
|
||||
* It follows the convention of a 3-2-1 intrinsic Tait-Bryan rotation sequence.
|
||||
* In order to go from frame 1 to frame 2 we apply the following rotations consecutively.
|
||||
* 1) We rotate about our initial Z axis by an angle of _psi.
|
||||
* 2) We rotate about the newly created Y' axis by an angle of _theta.
|
||||
* 3) We rotate about the newly created X'' axis by an angle of _phi.
|
||||
*
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template <typename Type>
|
||||
class Dcm;
|
||||
|
||||
template <typename Type>
|
||||
class Quaternion;
|
||||
|
||||
/**
|
||||
* Euler angles class
|
||||
*
|
||||
* This class describes the rotation from frame 1
|
||||
* to frame 2 via 3-2-1 intrinsic Tait-Bryan rotation sequence.
|
||||
*/
|
||||
template<typename Type>
|
||||
class Euler : public Vector<Type, 3>
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Standard constructor
|
||||
*/
|
||||
Euler() = default;
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*
|
||||
* @param other vector to copy
|
||||
*/
|
||||
Euler(const Vector<Type, 3> &other) :
|
||||
Vector<Type, 3>(other)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from Matrix31
|
||||
*
|
||||
* @param other Matrix31 to copy
|
||||
*/
|
||||
Euler(const Matrix<Type, 3, 1> &other) :
|
||||
Vector<Type, 3>(other)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from euler angles
|
||||
*
|
||||
* Instance is initialized from an 3-2-1 intrinsic Tait-Bryan
|
||||
* rotation sequence representing transformation from frame 1
|
||||
* to frame 2.
|
||||
*
|
||||
* @param phi_ rotation angle about X axis
|
||||
* @param theta_ rotation angle about Y axis
|
||||
* @param psi_ rotation angle about Z axis
|
||||
*/
|
||||
Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
|
||||
{
|
||||
phi() = phi_;
|
||||
theta() = theta_;
|
||||
psi() = psi_;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from DCM matrix
|
||||
*
|
||||
* Instance is set from Dcm representing transformation from
|
||||
* frame 2 to frame 1.
|
||||
* This instance will hold the angles defining the 3-2-1 intrinsic
|
||||
* Tait-Bryan rotation sequence from frame 1 to frame 2.
|
||||
*
|
||||
* @param dcm Direction cosine matrix
|
||||
*/
|
||||
Euler(const Dcm<Type> &dcm)
|
||||
{
|
||||
theta() = asin(-dcm(2, 0));
|
||||
|
||||
if ((fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
|
||||
phi() = 0;
|
||||
psi() = atan2(dcm(1, 2), dcm(0, 2));
|
||||
|
||||
} else if ((fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
|
||||
phi() = 0;
|
||||
psi() = atan2(-dcm(1, 2), -dcm(0, 2));
|
||||
|
||||
} else {
|
||||
phi() = atan2(dcm(2, 1), dcm(2, 2));
|
||||
psi() = atan2(dcm(1, 0), dcm(0, 0));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor from quaternion instance.
|
||||
*
|
||||
* Instance is set from a quaternion representing transformation
|
||||
* from frame 2 to frame 1.
|
||||
* This instance will hold the angles defining the 3-2-1 intrinsic
|
||||
* Tait-Bryan rotation sequence from frame 1 to frame 2.
|
||||
*
|
||||
* @param q quaternion
|
||||
*/
|
||||
Euler(const Quaternion<Type> &q) : Vector<Type, 3>(Euler(Dcm<Type>(q)))
|
||||
{
|
||||
}
|
||||
|
||||
inline Type phi() const
|
||||
{
|
||||
return (*this)(0);
|
||||
}
|
||||
inline Type theta() const
|
||||
{
|
||||
return (*this)(1);
|
||||
}
|
||||
inline Type psi() const
|
||||
{
|
||||
return (*this)(2);
|
||||
}
|
||||
|
||||
inline Type &phi()
|
||||
{
|
||||
return (*this)(0);
|
||||
}
|
||||
inline Type &theta()
|
||||
{
|
||||
return (*this)(1);
|
||||
}
|
||||
inline Type &psi()
|
||||
{
|
||||
return (*this)(2);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
using Eulerf = Euler<float>;
|
||||
using Eulerd = Euler<double>;
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,146 @@
|
||||
/**
|
||||
* @file LeastSquaresSolver.hpp
|
||||
*
|
||||
* Least Squares Solver using QR householder decomposition.
|
||||
* It calculates x for Ax = b.
|
||||
* A = Q*R
|
||||
* where R is an upper triangular matrix.
|
||||
*
|
||||
* R*x = Q^T*b
|
||||
* This is efficiently solved for x because of the upper triangular property of R.
|
||||
*
|
||||
* @author Bart Slinger <bartslinger@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix {
|
||||
|
||||
template<typename Type, size_t M, size_t N>
|
||||
class LeastSquaresSolver
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @brief Class calculates QR decomposition which can be used for linear
|
||||
* least squares
|
||||
* @param A Matrix of size MxN
|
||||
*
|
||||
* Initialize the class with a MxN matrix. The constructor starts the
|
||||
* QR decomposition. This class does not check the rank of the matrix.
|
||||
* The user needs to make sure that rank(A) = N and M >= N.
|
||||
*/
|
||||
LeastSquaresSolver(const Matrix<Type, M, N> &A)
|
||||
{
|
||||
static_assert(M >= N, "Matrix dimension should be M >= N");
|
||||
|
||||
// Copy contentents of matrix A
|
||||
_A = A;
|
||||
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
Type normx = Type(0);
|
||||
for (size_t i = j; i < M; i++) {
|
||||
normx += _A(i,j) * _A(i,j);
|
||||
}
|
||||
normx = sqrt(normx);
|
||||
Type s = _A(j,j) > 0 ? Type(-1) : Type(1);
|
||||
Type u1 = _A(j,j) - s*normx;
|
||||
// prevent divide by zero
|
||||
// also covers u1. normx is never negative
|
||||
if (normx < Type(1e-8)) {
|
||||
break;
|
||||
}
|
||||
Type w[M] = {};
|
||||
w[0] = Type(1);
|
||||
for (size_t i = j+1; i < M; i++) {
|
||||
w[i-j] = _A(i,j) / u1;
|
||||
_A(i,j) = w[i-j];
|
||||
}
|
||||
_A(j,j) = s*normx;
|
||||
_tau(j) = -s*u1/normx;
|
||||
|
||||
for (size_t k = j+1; k < N; k++) {
|
||||
Type tmp = Type(0);
|
||||
for (size_t i = j; i < M; i++) {
|
||||
tmp += w[i-j] * _A(i,k);
|
||||
}
|
||||
for (size_t i = j; i < M; i++) {
|
||||
_A(i,k) -= _tau(j) * w[i-j] * tmp;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief qtb Calculate Q^T * b
|
||||
* @param b
|
||||
* @return Q^T*b
|
||||
*
|
||||
* This function calculates Q^T * b. This is useful for the solver
|
||||
* because R*x = Q^T*b.
|
||||
*/
|
||||
Vector<Type, M> qtb(const Vector<Type, M> &b) {
|
||||
Vector<Type, M> qtbv = b;
|
||||
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
Type w[M];
|
||||
w[0] = Type(1);
|
||||
// fill vector w
|
||||
for (size_t i = j+1; i < M; i++) {
|
||||
w[i-j] = _A(i,j);
|
||||
}
|
||||
Type tmp = Type(0);
|
||||
for (size_t i = j; i < M; i++) {
|
||||
tmp += w[i-j] * qtbv(i);
|
||||
}
|
||||
|
||||
for (size_t i = j; i < M; i++) {
|
||||
qtbv(i) -= _tau(j) * w[i-j] * tmp;
|
||||
}
|
||||
}
|
||||
return qtbv;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Solve Ax=b for x
|
||||
* @param b
|
||||
* @return Vector x
|
||||
*
|
||||
* Find x in the equation Ax = b.
|
||||
* A is provided in the initializer of the class.
|
||||
*/
|
||||
Vector<Type, N> solve(const Vector<Type, M> &b) {
|
||||
Vector<Type, M> qtbv = qtb(b);
|
||||
Vector<Type, N> x;
|
||||
|
||||
// size_t is unsigned and wraps i = 0 - 1 to i > N
|
||||
for (size_t i = N - 1; i < N; i--) {
|
||||
printf("i %d\n", static_cast<int>(i));
|
||||
x(i) = qtbv(i);
|
||||
for (size_t r = i+1; r < N; r++) {
|
||||
x(i) -= _A(i,r) * x(r);
|
||||
}
|
||||
// divide by zero, return vector of zeros
|
||||
if (isEqualF(_A(i,i), Type(0), Type(1e-8))) {
|
||||
for (size_t z = 0; z < N; z++) {
|
||||
x(z) = Type(0);
|
||||
}
|
||||
break;
|
||||
}
|
||||
x(i) /= _A(i,i);
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
private:
|
||||
Matrix<Type, M, N> _A;
|
||||
Vector<Type, N> _tau;
|
||||
|
||||
};
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,119 @@
|
||||
/**
|
||||
* @file PseudoInverse.hpp
|
||||
*
|
||||
* Implementation of matrix pseudo inverse
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
* @author Julian Kent <julian@auterion.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
/**
|
||||
* Geninv
|
||||
* Fast pseudoinverse based on full rank cholesky factorisation
|
||||
*
|
||||
* Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809
|
||||
*/
|
||||
template<typename Type, size_t M, size_t N>
|
||||
bool geninv(const Matrix<Type, M, N> & G, Matrix<Type, N, M>& res)
|
||||
{
|
||||
size_t rank;
|
||||
if (M <= N) {
|
||||
SquareMatrix<Type, M> A = G * G.transpose();
|
||||
SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
|
||||
|
||||
A = L.transpose() * L;
|
||||
SquareMatrix<Type, M> X;
|
||||
if (!inv(A, X, rank)) {
|
||||
res = Matrix<Type, N, M>();
|
||||
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
|
||||
}
|
||||
// doing an intermediate assignment reduces stack usage
|
||||
A = X * X * L.transpose();
|
||||
res = G.transpose() * (L * A);
|
||||
|
||||
} else {
|
||||
SquareMatrix<Type, N> A = G.transpose() * G;
|
||||
SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
|
||||
|
||||
A = L.transpose() * L;
|
||||
SquareMatrix<Type, N> X;
|
||||
if(!inv(A, X, rank)) {
|
||||
res = Matrix<Type, N, M>();
|
||||
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
|
||||
}
|
||||
// doing an intermediate assignment reduces stack usage
|
||||
A = X * X * L.transpose();
|
||||
res = (L * A) * G.transpose();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
template<typename Type>
|
||||
Type typeEpsilon();
|
||||
|
||||
template<> inline
|
||||
float typeEpsilon<float>()
|
||||
{
|
||||
return FLT_EPSILON;
|
||||
}
|
||||
|
||||
/**
|
||||
* Full rank Cholesky factorization of A
|
||||
*/
|
||||
template<typename Type, size_t N>
|
||||
SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
|
||||
size_t& rank)
|
||||
{
|
||||
// Loses one ulp accuracy per row of diag, relative to largest magnitude
|
||||
const Type tol = N * typeEpsilon<Type>() * A.diag().max();
|
||||
|
||||
Matrix<Type, N, N> L;
|
||||
|
||||
size_t r = 0;
|
||||
for (size_t k = 0; k < N; k++) {
|
||||
|
||||
if (r == 0) {
|
||||
for (size_t i = k; i < N; i++) {
|
||||
L(i, r) = A(i, k);
|
||||
}
|
||||
|
||||
} else {
|
||||
for (size_t i = k; i < N; i++) {
|
||||
// Compute LL = L[k:n, :r] * L[k, :r].T
|
||||
Type LL = Type();
|
||||
for (size_t j = 0; j < r; j++) {
|
||||
LL += L(i, j) * L(k, j);
|
||||
}
|
||||
L(i, r) = A(i, k) - LL;
|
||||
}
|
||||
}
|
||||
if (L(k, r) > tol) {
|
||||
L(k, r) = sqrt(L(k, r));
|
||||
|
||||
if (k < N - 1) {
|
||||
for (size_t i = k + 1; i < N; i++) {
|
||||
L(i, r) = L(i, r) / L(k, r);
|
||||
}
|
||||
}
|
||||
|
||||
r = r + 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Return rank
|
||||
rank = r;
|
||||
|
||||
return L;
|
||||
}
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,58 @@
|
||||
/**
|
||||
* @file Scalar.hpp
|
||||
*
|
||||
* Defines conversion of matrix to scalar.
|
||||
*
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template<typename Type>
|
||||
class Scalar
|
||||
{
|
||||
public:
|
||||
Scalar() = delete;
|
||||
|
||||
Scalar(const Matrix<Type, 1, 1> & other) :
|
||||
_value{other(0,0)}
|
||||
{
|
||||
}
|
||||
|
||||
Scalar(Type other) : _value(other)
|
||||
{
|
||||
}
|
||||
|
||||
operator const Type &()
|
||||
{
|
||||
return _value;
|
||||
}
|
||||
|
||||
operator Matrix<Type, 1, 1>() const {
|
||||
Matrix<Type, 1, 1> m;
|
||||
m(0, 0) = _value;
|
||||
return m;
|
||||
}
|
||||
|
||||
operator Vector<Type, 1>() const {
|
||||
Vector<Type, 1> m;
|
||||
m(0) = _value;
|
||||
return m;
|
||||
}
|
||||
|
||||
private:
|
||||
const Type _value;
|
||||
|
||||
};
|
||||
|
||||
using Scalarf = Scalar<float>;
|
||||
using Scalard = Scalar<double>;
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,304 @@
|
||||
/**
|
||||
* @file Slice.hpp
|
||||
*
|
||||
* A simple matrix template library.
|
||||
*
|
||||
* @author Julian Kent < julian@auterion.com >
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
|
||||
namespace matrix {
|
||||
|
||||
template<typename Type, size_t M, size_t N>
|
||||
class Matrix;
|
||||
|
||||
template<typename Type, size_t M>
|
||||
class Vector;
|
||||
|
||||
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
|
||||
class Slice {
|
||||
public:
|
||||
Slice(size_t x0, size_t y0, const Matrix<Type, M, N>* data) :
|
||||
_x0(x0),
|
||||
_y0(y0),
|
||||
_data(const_cast<Matrix<Type, M, N>*>(data)) {
|
||||
static_assert(P <= M, "Slice rows bigger than backing matrix");
|
||||
static_assert(Q <= N, "Slice cols bigger than backing matrix");
|
||||
assert(x0 + P <= M);
|
||||
assert(y0 + Q <= N);
|
||||
}
|
||||
|
||||
const Type &operator()(size_t i, size_t j) const
|
||||
{
|
||||
assert(i < P);
|
||||
assert(j < Q);
|
||||
|
||||
return (*_data)(_x0 + i, _y0 + j);
|
||||
}
|
||||
|
||||
Type &operator()(size_t i, size_t j)
|
||||
|
||||
{
|
||||
assert(i < P);
|
||||
assert(j < Q);
|
||||
|
||||
return (*_data)(_x0 + i, _y0 + j);
|
||||
}
|
||||
|
||||
template<size_t MM, size_t NN>
|
||||
Slice<Type, P, Q, M, N>& operator=(const Slice<Type, P, Q, MM, NN>& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) = other(i, j);
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
Slice<Type, P, Q, M, N>& operator=(const Matrix<Type, P, Q>& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) = other(i, j);
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
Slice<Type, P, Q, M, N>& operator=(const Type& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) = other;
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
// allow assigning vectors to a slice that are in the axis
|
||||
template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
|
||||
Slice<Type, 1, Q, M, N>& operator=(const Vector<Type, Q>& other)
|
||||
{
|
||||
Slice<Type, 1, Q, M, N>& self = *this;
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(0, j) = other(j);
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
template<size_t MM, size_t NN>
|
||||
Slice<Type, P, Q, M, N>& operator+=(const Slice<Type, P, Q, MM, NN>& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) += other(i, j);
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
Slice<Type, P, Q, M, N>& operator+=(const Matrix<Type, P, Q>& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) += other(i, j);
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
Slice<Type, P, Q, M, N>& operator+=(const Type& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) += other;
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
template<size_t MM, size_t NN>
|
||||
Slice<Type, P, Q, M, N>& operator-=(const Slice<Type, P, Q, MM, NN>& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) -= other(i, j);
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
Slice<Type, P, Q, M, N>& operator-=(const Matrix<Type, P, Q>& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) -= other(i, j);
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
Slice<Type, P, Q, M, N>& operator-=(const Type& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) -= other;
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
Slice<Type, P, Q, M, N>& operator*=(const Type& other)
|
||||
{
|
||||
Slice<Type, P, Q, M, N>& self = *this;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
self(i, j) *= other;
|
||||
}
|
||||
}
|
||||
return self;
|
||||
}
|
||||
|
||||
Slice<Type, P, Q, M, N>& operator/=(const Type& other)
|
||||
{
|
||||
return operator*=(Type(1) / other);
|
||||
}
|
||||
|
||||
Matrix<Type, P, Q> operator*(const Type& other) const
|
||||
{
|
||||
const Slice<Type, P, Q, M, N>& self = *this;
|
||||
Matrix<Type, P, Q> res;
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
res(i, j) = self(i, j) * other;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix<Type, P, Q> operator/(const Type& other) const
|
||||
{
|
||||
const Slice<Type, P, Q, M, N>& self = *this;
|
||||
return self * (Type(1) / other);
|
||||
}
|
||||
|
||||
template<size_t R, size_t S>
|
||||
const Slice<Type, R, S, M, N> slice(size_t x0, size_t y0) const
|
||||
{
|
||||
return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
|
||||
}
|
||||
|
||||
template<size_t R, size_t S>
|
||||
Slice<Type, R, S, M, N> slice(size_t x0, size_t y0)
|
||||
{
|
||||
return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
|
||||
}
|
||||
|
||||
void copyTo(Type dst[P*Q]) const
|
||||
{
|
||||
const Slice<Type, P, Q, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
dst[i*N+j] = self(i, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void copyToColumnMajor(Type dst[P*Q]) const
|
||||
{
|
||||
const Slice<Type, P, Q, M, N> &self = *this;
|
||||
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
dst[i+(j*M)] = self(i, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Vector<Type, P<Q?P:Q> diag() const
|
||||
{
|
||||
const Slice<Type, P, Q, M, N>& self = *this;
|
||||
Vector<Type,P<Q?P:Q> res;
|
||||
for (size_t j = 0; j < (P<Q?P:Q); j++) {
|
||||
res(j) = self(j,j);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
Type norm_squared() const
|
||||
{
|
||||
const Slice<Type, P, Q, M, N>& self = *this;
|
||||
Type accum(0);
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
accum += self(i, j)*self(i, j);
|
||||
}
|
||||
}
|
||||
return accum;
|
||||
}
|
||||
|
||||
Type norm() const
|
||||
{
|
||||
return matrix::sqrt(norm_squared());
|
||||
}
|
||||
|
||||
bool longerThan(Type testVal) const
|
||||
{
|
||||
return norm_squared() > testVal*testVal;
|
||||
}
|
||||
|
||||
Type max() const
|
||||
{
|
||||
Type max_val = (*this)(0,0);
|
||||
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
Type val = (*this)(i,j);
|
||||
|
||||
if (val > max_val) {
|
||||
max_val = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return max_val;
|
||||
}
|
||||
|
||||
Type min() const
|
||||
{
|
||||
Type min_val = (*this)(0,0);
|
||||
|
||||
for (size_t i = 0; i < P; i++) {
|
||||
for (size_t j = 0; j < Q; j++) {
|
||||
Type val = (*this)(i,j);
|
||||
|
||||
if (val < min_val) {
|
||||
min_val = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return min_val;
|
||||
}
|
||||
|
||||
private:
|
||||
size_t _x0, _y0;
|
||||
Matrix<Type,M,N>* _data;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,189 @@
|
||||
/**
|
||||
* @file SparseVector.hpp
|
||||
*
|
||||
* SparseVector class.
|
||||
*
|
||||
* @author Kamil Ritz <kritz@ethz.ch>
|
||||
* @author Julian Kent <julian@auterion.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix {
|
||||
template<int N> struct force_constexpr_eval {
|
||||
static const int value = N;
|
||||
};
|
||||
|
||||
// Vector that only store nonzero elements,
|
||||
// which indices are specified as parameter pack
|
||||
template<typename Type, size_t M, size_t... Idxs>
|
||||
class SparseVector {
|
||||
private:
|
||||
static constexpr size_t N = sizeof...(Idxs);
|
||||
static constexpr size_t _indices[N] {Idxs...};
|
||||
|
||||
static constexpr bool duplicateIndices() {
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
for (size_t j = 0; j < i; j++) {
|
||||
if (_indices[i] == _indices[j]) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
static constexpr size_t findMaxIndex() {
|
||||
size_t maxIndex = 0;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (maxIndex < _indices[i]) {
|
||||
maxIndex = _indices[i];
|
||||
}
|
||||
}
|
||||
return maxIndex;
|
||||
}
|
||||
|
||||
static_assert(!duplicateIndices(), "Duplicate indices");
|
||||
static_assert(N < M, "More entries than elements, use a dense vector");
|
||||
static_assert(N > 0, "A sparse vector needs at least one element");
|
||||
static_assert(findMaxIndex() < M, "Largest entry doesn't fit in sparse vector");
|
||||
|
||||
Type _data[N] {};
|
||||
|
||||
static constexpr int findCompressedIndex(size_t index) {
|
||||
int compressedIndex = -1;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (index == _indices[i]) {
|
||||
compressedIndex = static_cast<int>(i);
|
||||
}
|
||||
}
|
||||
return compressedIndex;
|
||||
}
|
||||
|
||||
public:
|
||||
constexpr size_t non_zeros() const {
|
||||
return N;
|
||||
}
|
||||
|
||||
constexpr size_t index(size_t i) const {
|
||||
return SparseVector::_indices[i];
|
||||
}
|
||||
|
||||
SparseVector() = default;
|
||||
|
||||
SparseVector(const matrix::Vector<Type, M>& data) {
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
_data[i] = data(_indices[i]);
|
||||
}
|
||||
}
|
||||
|
||||
explicit SparseVector(const Type data[N]) {
|
||||
memcpy(_data, data, sizeof(_data));
|
||||
}
|
||||
|
||||
template <size_t i>
|
||||
inline Type at() const {
|
||||
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
|
||||
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
|
||||
return _data[compressed_index];
|
||||
}
|
||||
|
||||
template <size_t i>
|
||||
inline Type& at() {
|
||||
static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
|
||||
static_assert(compressed_index >= 0, "cannot access unpopulated indices");
|
||||
return _data[compressed_index];
|
||||
}
|
||||
|
||||
inline Type atCompressedIndex(size_t i) const {
|
||||
assert(i < N);
|
||||
return _data[i];
|
||||
}
|
||||
|
||||
inline Type& atCompressedIndex(size_t i) {
|
||||
assert(i < N);
|
||||
return _data[i];
|
||||
}
|
||||
|
||||
void setZero() {
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
_data[i] = Type(0);
|
||||
}
|
||||
}
|
||||
|
||||
Type dot(const matrix::Vector<Type, M>& other) const {
|
||||
Type accum (0);
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
accum += _data[i] * other(_indices[i]);
|
||||
}
|
||||
return accum;
|
||||
}
|
||||
|
||||
matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M>& other) const {
|
||||
matrix::Vector<Type, M> vec = other;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
vec(_indices[i]) += _data[i];
|
||||
}
|
||||
return vec;
|
||||
}
|
||||
|
||||
SparseVector& operator+=(Type t) {
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
_data[i] += t;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
Type norm_squared() const
|
||||
{
|
||||
Type accum(0);
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
accum += _data[i] * _data[i];
|
||||
}
|
||||
return accum;
|
||||
}
|
||||
|
||||
Type norm() const
|
||||
{
|
||||
return matrix::sqrt(norm_squared());
|
||||
}
|
||||
|
||||
bool longerThan(Type testVal) const
|
||||
{
|
||||
return norm_squared() > testVal*testVal;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename Type, size_t Q, size_t M, size_t ... Idxs>
|
||||
matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M>& mat, const matrix::SparseVector<Type, M, Idxs...>& vec) {
|
||||
matrix::Vector<Type, Q> res;
|
||||
for (size_t i = 0; i < Q; i++) {
|
||||
const Vector<Type, M> row = mat.row(i);
|
||||
res(i) = vec.dot(row);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
// returns x.T * A * x
|
||||
template<typename Type, size_t M, size_t ... Idxs>
|
||||
Type quadraticForm(const matrix::SquareMatrix<Type, M>& A, const matrix::SparseVector<Type, M, Idxs...>& x) {
|
||||
Type res = Type(0);
|
||||
for (size_t i = 0; i < x.non_zeros(); i++) {
|
||||
Type tmp = Type(0);
|
||||
for (size_t j = 0; j < x.non_zeros(); j++) {
|
||||
tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
|
||||
}
|
||||
res += x.atCompressedIndex(i) * tmp;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename Type,size_t M, size_t... Idxs>
|
||||
constexpr size_t SparseVector<Type, M, Idxs...>::_indices[SparseVector<Type, M, Idxs...>::N];
|
||||
|
||||
template<size_t M, size_t ... Idxs>
|
||||
using SparseVectorf = SparseVector<float, M, Idxs...>;
|
||||
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,136 @@
|
||||
/**
|
||||
* @file Vector.hpp
|
||||
*
|
||||
* Vector class.
|
||||
*
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template <typename Type, size_t M, size_t N>
|
||||
class Matrix;
|
||||
|
||||
template<typename Type, size_t M>
|
||||
class Vector : public Matrix<Type, M, 1>
|
||||
{
|
||||
public:
|
||||
using MatrixM1 = Matrix<Type, M, 1>;
|
||||
|
||||
Vector() = default;
|
||||
|
||||
Vector(const MatrixM1 & other) :
|
||||
MatrixM1(other)
|
||||
{
|
||||
}
|
||||
|
||||
explicit Vector(const Type data_[M]) :
|
||||
MatrixM1(data_)
|
||||
{
|
||||
}
|
||||
|
||||
template<size_t P, size_t Q>
|
||||
Vector(const Slice<Type, M, 1, P, Q>& slice_in) :
|
||||
Matrix<Type, M, 1>(slice_in)
|
||||
{
|
||||
}
|
||||
|
||||
template<size_t P, size_t Q, size_t DUMMY = 1>
|
||||
Vector(const Slice<Type, 1, M, P, Q>& slice_in)
|
||||
{
|
||||
Vector &self(*this);
|
||||
for (size_t i = 0; i<M; i++) {
|
||||
self(i) = slice_in(0, i);
|
||||
}
|
||||
}
|
||||
|
||||
inline const Type &operator()(size_t i) const
|
||||
{
|
||||
assert(i < M);
|
||||
|
||||
const MatrixM1 &v = *this;
|
||||
return v(i, 0);
|
||||
}
|
||||
|
||||
inline Type &operator()(size_t i)
|
||||
{
|
||||
assert(i < M);
|
||||
|
||||
MatrixM1 &v = *this;
|
||||
return v(i, 0);
|
||||
}
|
||||
|
||||
Type dot(const MatrixM1 & b) const {
|
||||
const Vector &a(*this);
|
||||
Type r(0);
|
||||
for (size_t i = 0; i<M; i++) {
|
||||
r += a(i)*b(i,0);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
inline Type operator*(const MatrixM1 & b) const {
|
||||
const Vector &a(*this);
|
||||
return a.dot(b);
|
||||
}
|
||||
|
||||
inline Vector operator*(Type b) const {
|
||||
return Vector(MatrixM1::operator*(b));
|
||||
}
|
||||
|
||||
Type norm() const {
|
||||
const Vector &a(*this);
|
||||
return Type(matrix::sqrt(a.dot(a)));
|
||||
}
|
||||
|
||||
Type norm_squared() const {
|
||||
const Vector &a(*this);
|
||||
return a.dot(a);
|
||||
}
|
||||
|
||||
inline Type length() const {
|
||||
return norm();
|
||||
}
|
||||
|
||||
inline void normalize() {
|
||||
(*this) /= norm();
|
||||
}
|
||||
|
||||
Vector unit() const {
|
||||
return (*this) / norm();
|
||||
}
|
||||
|
||||
Vector unit_or_zero(const Type eps = Type(1e-5)) const {
|
||||
const Type n = norm();
|
||||
if (n > eps) {
|
||||
return (*this) / n;
|
||||
}
|
||||
return Vector();
|
||||
}
|
||||
|
||||
inline Vector normalized() const {
|
||||
return unit();
|
||||
}
|
||||
|
||||
bool longerThan(Type testVal) const {
|
||||
return norm_squared() > testVal*testVal;
|
||||
}
|
||||
|
||||
Vector sqrt() const {
|
||||
const Vector &a(*this);
|
||||
Vector r;
|
||||
for (size_t i = 0; i<M; i++) {
|
||||
r(i) = Type(matrix::sqrt(a(i)));
|
||||
}
|
||||
return r;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,80 @@
|
||||
/**
|
||||
* @file Vector2.hpp
|
||||
*
|
||||
* 2D vector class.
|
||||
*
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template <typename Type, size_t M>
|
||||
class Vector;
|
||||
|
||||
template<typename Type>
|
||||
class Vector2 : public Vector<Type, 2>
|
||||
{
|
||||
public:
|
||||
|
||||
using Matrix21 = Matrix<Type, 2, 1>;
|
||||
using Vector3 = Vector<Type, 3>;
|
||||
|
||||
Vector2() = default;
|
||||
|
||||
Vector2(const Matrix21 & other) :
|
||||
Vector<Type, 2>(other)
|
||||
{
|
||||
}
|
||||
|
||||
explicit Vector2(const Type data_[2]) :
|
||||
Vector<Type, 2>(data_)
|
||||
{
|
||||
}
|
||||
|
||||
Vector2(Type x, Type y)
|
||||
{
|
||||
Vector2 &v(*this);
|
||||
v(0) = x;
|
||||
v(1) = y;
|
||||
}
|
||||
|
||||
template<size_t P, size_t Q>
|
||||
Vector2(const Slice<Type, 2, 1, P, Q>& slice_in) : Vector<Type, 2>(slice_in)
|
||||
{
|
||||
}
|
||||
|
||||
template<size_t P, size_t Q>
|
||||
Vector2(const Slice<Type, 1, 2, P, Q>& slice_in) : Vector<Type, 2>(slice_in)
|
||||
{
|
||||
}
|
||||
|
||||
explicit Vector2(const Vector3 & other)
|
||||
{
|
||||
Vector2 &v(*this);
|
||||
v(0) = other(0);
|
||||
v(1) = other(1);
|
||||
}
|
||||
|
||||
Type cross(const Matrix21 & b) const {
|
||||
const Vector2 &a(*this);
|
||||
return a(0)*b(1, 0) - a(1)*b(0, 0);
|
||||
}
|
||||
|
||||
Type operator%(const Matrix21 & b) const {
|
||||
return (*this).cross(b);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
using Vector2f = Vector2<float>;
|
||||
using Vector2d = Vector2<double>;
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,156 @@
|
||||
/**
|
||||
* @file Vector3.hpp
|
||||
*
|
||||
* 3D vector class.
|
||||
*
|
||||
* @author James Goppert <james.goppert@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template <typename Type, size_t M, size_t N>
|
||||
class Matrix;
|
||||
|
||||
template <typename Type, size_t M>
|
||||
class Vector;
|
||||
|
||||
template <typename Type>
|
||||
class Dcm;
|
||||
|
||||
template <typename Type>
|
||||
class Vector2;
|
||||
|
||||
template<typename Type>
|
||||
class Vector3 : public Vector<Type, 3>
|
||||
{
|
||||
public:
|
||||
|
||||
using Matrix31 = Matrix<Type, 3, 1>;
|
||||
|
||||
Vector3() = default;
|
||||
|
||||
Vector3(const Matrix31 & other) :
|
||||
Vector<Type, 3>(other)
|
||||
{
|
||||
}
|
||||
|
||||
explicit Vector3(const Type data_[3]) :
|
||||
Vector<Type, 3>(data_)
|
||||
{
|
||||
}
|
||||
|
||||
Vector3(Type x, Type y, Type z) {
|
||||
Vector3 &v(*this);
|
||||
v(0) = x;
|
||||
v(1) = y;
|
||||
v(2) = z;
|
||||
}
|
||||
|
||||
template<size_t P, size_t Q>
|
||||
Vector3(const Slice<Type, 3, 1, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
|
||||
{
|
||||
}
|
||||
|
||||
template<size_t P, size_t Q>
|
||||
Vector3(const Slice<Type, 1, 3, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
|
||||
{
|
||||
}
|
||||
|
||||
Vector3 cross(const Matrix31 & b) const {
|
||||
const Vector3 &a(*this);
|
||||
return {a(1)*b(2,0) - a(2)*b(1,0), -a(0)*b(2,0) + a(2)*b(0,0), a(0)*b(1,0) - a(1)*b(0,0)};
|
||||
}
|
||||
|
||||
/**
|
||||
* Override matrix ops so Vector3 type is returned
|
||||
*/
|
||||
|
||||
inline Vector3 operator+(Vector3 other) const
|
||||
{
|
||||
return Matrix31::operator+(other);
|
||||
}
|
||||
|
||||
inline Vector3 operator+(Type scalar) const
|
||||
{
|
||||
return Matrix31::operator+(scalar);
|
||||
}
|
||||
|
||||
inline Vector3 operator-(Vector3 other) const
|
||||
{
|
||||
return Matrix31::operator-(other);
|
||||
}
|
||||
|
||||
inline Vector3 operator-(Type scalar) const
|
||||
{
|
||||
return Matrix31::operator-(scalar);
|
||||
}
|
||||
|
||||
inline Vector3 operator-() const
|
||||
{
|
||||
return Matrix31::operator-();
|
||||
}
|
||||
|
||||
inline Vector3 operator*(Type scalar) const
|
||||
{
|
||||
return Matrix31::operator*(scalar);
|
||||
}
|
||||
|
||||
inline Type operator*(Vector3 b) const
|
||||
{
|
||||
return Vector<Type, 3>::operator*(b);
|
||||
}
|
||||
|
||||
inline Vector3 operator%(const Matrix31 & b) const {
|
||||
return (*this).cross(b);
|
||||
}
|
||||
|
||||
/**
|
||||
* Override vector ops so Vector3 type is returned
|
||||
*/
|
||||
inline Vector3 unit() const {
|
||||
return Vector3(Vector<Type, 3>::unit());
|
||||
}
|
||||
|
||||
inline Vector3 normalized() const {
|
||||
return unit();
|
||||
}
|
||||
|
||||
const Slice<Type, 2, 1, 3, 1> xy() const
|
||||
{
|
||||
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
|
||||
}
|
||||
|
||||
Slice<Type, 2, 1, 3, 1> xy()
|
||||
{
|
||||
return Slice<Type, 2, 1, 3, 1>(0, 0, this);
|
||||
}
|
||||
|
||||
|
||||
Dcm<Type> hat() const { // inverse to Dcm.vee() operation
|
||||
const Vector3 &v(*this);
|
||||
Dcm<Type> A;
|
||||
A(0,0) = 0;
|
||||
A(0,1) = -v(2);
|
||||
A(0,2) = v(1);
|
||||
A(1,0) = v(2);
|
||||
A(1,1) = 0;
|
||||
A(1,2) = -v(0);
|
||||
A(2,0) = -v(1);
|
||||
A(2,1) = v(0);
|
||||
A(2,2) = 0;
|
||||
return A;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
using Vector3f = Vector3<float>;
|
||||
using Vector3d = Vector3<double>;
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,26 @@
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix {
|
||||
|
||||
template<typename Type, size_t M, size_t N>
|
||||
int kalman_correct(
|
||||
const Matrix<Type, M, M> & P,
|
||||
const Matrix<Type, N, M> & C,
|
||||
const Matrix<Type, N, N> & R,
|
||||
const Matrix<Type, N, 1> &r,
|
||||
Matrix<Type, M, 1> & dx,
|
||||
Matrix<Type, M, M> & dP,
|
||||
Type & beta
|
||||
)
|
||||
{
|
||||
SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
|
||||
Matrix<Type, M, N> K = P*C.T()*S_I;
|
||||
dx = K*r;
|
||||
beta = Scalar<Type>(r.T()*S_I*r);
|
||||
dP = K*C*P*(-1);
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace matrix
|
||||
@@ -0,0 +1,127 @@
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
|
||||
#include <px4_defines.h>
|
||||
#endif
|
||||
|
||||
namespace matrix
|
||||
{
|
||||
|
||||
template<typename Type>
|
||||
bool is_finite(Type x) {
|
||||
#if defined (__PX4_NUTTX)
|
||||
return PX4_ISFINITE(x);
|
||||
#elif defined (__PX4_QURT)
|
||||
return __builtin_isfinite(x);
|
||||
#else
|
||||
return std::isfinite(x);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Compare if two floating point numbers are equal
|
||||
*
|
||||
* NAN is considered equal to NAN and -NAN
|
||||
* INFINITY is considered equal INFINITY but not -INFINITY
|
||||
*
|
||||
* @param x right side of equality check
|
||||
* @param y left side of equality check
|
||||
* @param eps numerical tolerance of the check
|
||||
* @return true if the two values are considered equal, false otherwise
|
||||
*/
|
||||
template<typename Type>
|
||||
bool isEqualF(const Type x, const Type y, const Type eps = Type(1e-4f))
|
||||
{
|
||||
return (matrix::fabs(x - y) <= eps)
|
||||
|| (isnan(x) && isnan(y))
|
||||
|| (isinf(x) && isinf(y) && isnan(x - y));
|
||||
}
|
||||
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template<typename Floating>
|
||||
Floating wrap_floating(Floating x, Floating low, Floating high) {
|
||||
// already in range
|
||||
if (low <= x && x < high) {
|
||||
return x;
|
||||
}
|
||||
|
||||
const auto range = high - low;
|
||||
const auto inv_range = Floating(1) / range; // should evaluate at compile time, multiplies below at runtime
|
||||
const auto num_wraps = floor((x - low) * inv_range);
|
||||
return x - range * num_wraps;
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
/**
|
||||
* Wrap single precision floating point value to stay in range [low, high)
|
||||
*
|
||||
* @param x input possibly outside of the range
|
||||
* @param low lower limit of the allowed range
|
||||
* @param high upper limit of the allowed range
|
||||
* @return wrapped value inside the range
|
||||
*/
|
||||
inline float wrap(float x, float low, float high) {
|
||||
return matrix::detail::wrap_floating(x, low, high);
|
||||
}
|
||||
|
||||
/**
|
||||
* Wrap double precision floating point value to stay in range [low, high)
|
||||
*
|
||||
* @param x input possibly outside of the range
|
||||
* @param low lower limit of the allowed range
|
||||
* @param high upper limit of the allowed range
|
||||
* @return wrapped value inside the range
|
||||
*/
|
||||
inline double wrap(double x, double low, double high) {
|
||||
return matrix::detail::wrap_floating(x, low, high);
|
||||
}
|
||||
|
||||
/**
|
||||
* Wrap integer value to stay in range [low, high)
|
||||
*
|
||||
* @param x input possibly outside of the range
|
||||
* @param low lower limit of the allowed range
|
||||
* @param high upper limit of the allowed range
|
||||
* @return wrapped value inside the range
|
||||
*/
|
||||
template<typename Integer>
|
||||
Integer wrap(Integer x, Integer low, Integer high) {
|
||||
const auto range = high - low;
|
||||
|
||||
if (x < low) {
|
||||
x += range * ((low - x) / range + 1);
|
||||
}
|
||||
|
||||
return low + (x - low) % range;
|
||||
}
|
||||
|
||||
/**
|
||||
* Wrap value in range [-π, π)
|
||||
*/
|
||||
template<typename Type>
|
||||
Type wrap_pi(Type x)
|
||||
{
|
||||
return wrap(x, Type(-M_PI), Type(M_PI));
|
||||
}
|
||||
|
||||
/**
|
||||
* Wrap value in range [0, 2π)
|
||||
*/
|
||||
template<typename Type>
|
||||
Type wrap_2pi(Type x)
|
||||
{
|
||||
return wrap(x, Type(0), Type(M_TWOPI));
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
int sign(T val)
|
||||
{
|
||||
return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON));
|
||||
}
|
||||
|
||||
} // namespace matrix
|
||||
@@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
|
||||
#include "math.hpp"
|
||||
|
||||
namespace matrix {
|
||||
|
||||
template<typename Type, size_t M, size_t N>
|
||||
int integrate_rk4(
|
||||
Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> & u),
|
||||
const Matrix<Type, M, 1> & y0,
|
||||
const Matrix<Type, N, 1> & u,
|
||||
Type t0,
|
||||
Type tf,
|
||||
Type h0,
|
||||
Matrix<Type, M, 1> & y1
|
||||
)
|
||||
{
|
||||
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
|
||||
Type t1 = t0;
|
||||
y1 = y0;
|
||||
Type h = h0;
|
||||
Vector<Type, M> k1, k2, k3, k4;
|
||||
if (tf < t0) return -1; // make sure t1 > t0
|
||||
while (t1 < tf) {
|
||||
if (t1 + h0 < tf) {
|
||||
h = h0;
|
||||
} else {
|
||||
h = tf - t1;
|
||||
}
|
||||
k1 = f(t1, y1, u);
|
||||
k2 = f(t1 + h/2, y1 + k1*h/2, u);
|
||||
k3 = f(t1 + h/2, y1 + k2*h/2, u);
|
||||
k4 = f(t1 + h, y1 + k3*h, u);
|
||||
y1 += (k1 + k2*2 + k3*2 + k4)*(h/6);
|
||||
t1 += h;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace matrix
|
||||
|
||||
// vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :
|
||||
@@ -0,0 +1,23 @@
|
||||
#pragma once
|
||||
|
||||
#include <assert.h>
|
||||
#include "stdlib_imports.hpp"
|
||||
#ifdef __PX4_QURT
|
||||
#include "dspal_math.h"
|
||||
#endif
|
||||
#include "helper_functions.hpp"
|
||||
#include "Matrix.hpp"
|
||||
#include "SquareMatrix.hpp"
|
||||
#include "Slice.hpp"
|
||||
#include "Vector.hpp"
|
||||
#include "Vector2.hpp"
|
||||
#include "Vector3.hpp"
|
||||
#include "Euler.hpp"
|
||||
#include "Dcm.hpp"
|
||||
#include "Scalar.hpp"
|
||||
#include "Quaternion.hpp"
|
||||
#include "AxisAngle.hpp"
|
||||
#include "LeastSquaresSolver.hpp"
|
||||
#include "Dual.hpp"
|
||||
#include "PseudoInverse.hpp"
|
||||
#include "SparseVector.hpp"
|
||||
@@ -0,0 +1,139 @@
|
||||
/**
|
||||
* @file stdlib_imports.hpp
|
||||
*
|
||||
* This file is needed to shadow the C standard library math functions with ones provided by the C++ standard library.
|
||||
* This way we can guarantee that unwanted functions from the C library will never creep back in unexpectedly.
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@zubax.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <inttypes.h>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
#ifndef M_TWOPI
|
||||
#define M_TWOPI (M_PI * 2.0)
|
||||
#endif
|
||||
|
||||
namespace matrix {
|
||||
|
||||
#if !defined(FLT_EPSILON)
|
||||
#define FLT_EPSILON __FLT_EPSILON__
|
||||
#endif
|
||||
|
||||
#if defined(__PX4_NUTTX)
|
||||
/*
|
||||
* NuttX has no usable C++ math library, so we need to provide the needed definitions here manually.
|
||||
*/
|
||||
#define MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(name) \
|
||||
inline float name(float x) { return ::name##f(x); } \
|
||||
inline double name(double x) { return ::name(x); } \
|
||||
inline long double name(long double x) { return ::name##l(x); }
|
||||
|
||||
#define MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(name) \
|
||||
inline float name(float x, float y) { return ::name##f(x, y); } \
|
||||
inline double name(double x, double y) { return ::name(x, y); } \
|
||||
inline long double name(long double x, long double y) { return ::name##l(x, y); }
|
||||
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(fabs)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(log)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(log10)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(exp)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sqrt)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sin)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(cos)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(tan)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(asin)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(acos)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(atan)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sinh)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(cosh)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(tanh)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(ceil)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(floor)
|
||||
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(pow)
|
||||
MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(atan2)
|
||||
|
||||
#else // Not NuttX, using the C++ standard library
|
||||
|
||||
using std::abs;
|
||||
using std::div;
|
||||
using std::fabs;
|
||||
using std::fmod;
|
||||
using std::exp;
|
||||
using std::log;
|
||||
using std::log10;
|
||||
using std::pow;
|
||||
using std::sqrt;
|
||||
using std::sin;
|
||||
using std::cos;
|
||||
using std::tan;
|
||||
using std::asin;
|
||||
using std::acos;
|
||||
using std::atan;
|
||||
using std::atan2;
|
||||
using std::sinh;
|
||||
using std::cosh;
|
||||
using std::tanh;
|
||||
using std::ceil;
|
||||
using std::floor;
|
||||
using std::frexp;
|
||||
using std::ldexp;
|
||||
using std::modf;
|
||||
|
||||
# if (__cplusplus >= 201103L)
|
||||
|
||||
using std::remainder;
|
||||
using std::remquo;
|
||||
using std::fma;
|
||||
using std::fmax;
|
||||
using std::fmin;
|
||||
using std::fdim;
|
||||
using std::nan;
|
||||
using std::nanf;
|
||||
using std::nanl;
|
||||
using std::exp2;
|
||||
using std::expm1;
|
||||
using std::log2;
|
||||
using std::log1p;
|
||||
using std::cbrt;
|
||||
using std::hypot;
|
||||
using std::asinh;
|
||||
using std::acosh;
|
||||
using std::atanh;
|
||||
using std::erf;
|
||||
using std::erfc;
|
||||
using std::tgamma;
|
||||
using std::lgamma;
|
||||
using std::trunc;
|
||||
using std::round;
|
||||
using std::nearbyint;
|
||||
using std::rint;
|
||||
using std::scalbn;
|
||||
using std::ilogb;
|
||||
using std::logb;
|
||||
using std::nextafter;
|
||||
using std::copysign;
|
||||
using std::fpclassify;
|
||||
using std::isfinite;
|
||||
using std::isinf;
|
||||
using std::isnan;
|
||||
using std::isnormal;
|
||||
using std::signbit;
|
||||
using std::isgreater;
|
||||
using std::isgreaterequal;
|
||||
using std::isless;
|
||||
using std::islessequal;
|
||||
using std::islessgreater;
|
||||
using std::isunordered;
|
||||
|
||||
# endif
|
||||
#endif
|
||||
|
||||
}
|
||||
Executable
+30
@@ -0,0 +1,30 @@
|
||||
#!/bin/bash
|
||||
echo pwd:$PWD
|
||||
astyle=$1
|
||||
format=$2
|
||||
format_wildcards="""
|
||||
./matrix/*.*pp
|
||||
./test/*.*pp
|
||||
"""
|
||||
|
||||
#echo astyle: $astyle
|
||||
#echo format: $format
|
||||
|
||||
if [[ $format -eq 1 ]]
|
||||
then
|
||||
echo formatting
|
||||
$astyle ${format_wildcards}
|
||||
else
|
||||
echo checking format...
|
||||
$astyle --dry-run ${format_wildcards} | grep Formatted &>/dev/null
|
||||
if [[ $? -eq 0 ]]
|
||||
then
|
||||
echo Error: need to format
|
||||
echo "From cmake build directory run: 'make format'"
|
||||
exit 1
|
||||
fi
|
||||
echo no formatting needed
|
||||
exit 0
|
||||
fi
|
||||
|
||||
# vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :
|
||||
@@ -0,0 +1,67 @@
|
||||
include(gtest.cmake)
|
||||
|
||||
set(tests
|
||||
setIdentity
|
||||
inverse
|
||||
slice
|
||||
matrixMult
|
||||
vectorAssignment
|
||||
matrixAssignment
|
||||
matrixScalarMult
|
||||
transpose
|
||||
vector
|
||||
vector2
|
||||
vector3
|
||||
attitude
|
||||
filter
|
||||
integration
|
||||
sparseVector
|
||||
squareMatrix
|
||||
helper
|
||||
hatvee
|
||||
copyto
|
||||
least_squares
|
||||
upperRightTriangle
|
||||
dual
|
||||
pseudoInverse
|
||||
)
|
||||
|
||||
add_custom_target(test_build)
|
||||
foreach(test_name ${tests})
|
||||
add_executable(${test_name}
|
||||
${test_name}.cpp)
|
||||
add_test(test_${test_name} ${test_name})
|
||||
add_dependencies(test_build ${test_name})
|
||||
endforeach()
|
||||
target_link_libraries(sparseVector gtest_main)
|
||||
|
||||
if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage")
|
||||
|
||||
add_custom_target(coverage_build
|
||||
COMMAND ${CMAKE_CTEST_COMMAND}
|
||||
COMMAND lcov --capture --directory . --output-file coverage.info --rc lcov_branch_coverage=1
|
||||
COMMAND lcov --rc lcov_branch_coverage=1 --summary coverage.info
|
||||
WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
|
||||
DEPENDS test_build
|
||||
)
|
||||
|
||||
add_custom_target(coverage_html
|
||||
COMMAND genhtml coverage.info --output-directory out --branch-coverage
|
||||
COMMAND x-www-browser out/index.html
|
||||
WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
|
||||
DEPENDS coverage_build
|
||||
)
|
||||
|
||||
set(coverage_deps
|
||||
coverage_build)
|
||||
|
||||
if (COV_HTML)
|
||||
list(APPEND coverage_deps coverage_html)
|
||||
endif()
|
||||
|
||||
add_custom_target(coverage
|
||||
DEPENDS ${coverage_deps}
|
||||
)
|
||||
endif()
|
||||
|
||||
# vim: set et fenc=utf-8 ft=cmake ff=unix sts=0 sw=4 ts=4 :
|
||||
@@ -0,0 +1,18 @@
|
||||
cmake_minimum_required(VERSION 2.8.4)
|
||||
|
||||
project(googletest-download NONE)
|
||||
|
||||
include(ExternalProject)
|
||||
ExternalProject_Add(googletest
|
||||
URL https://github.com/google/googletest/archive/e2239ee6043f73722e7aa812a459f54a28552929.zip
|
||||
SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
|
||||
BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
|
||||
CONFIGURE_COMMAND ""
|
||||
BUILD_COMMAND ""
|
||||
INSTALL_COMMAND ""
|
||||
TEST_COMMAND ""
|
||||
# Wrap download, configure and build steps in a script to log output
|
||||
LOG_DOWNLOAD ON
|
||||
LOG_CONFIGURE ON
|
||||
LOG_BUILD ON
|
||||
)
|
||||
@@ -0,0 +1,490 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
#include <iostream>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
// manually instantiated all files we intend to test
|
||||
// so that coverage works correctly
|
||||
// doesn't matter what test this is in
|
||||
namespace matrix {
|
||||
template class Matrix<float, 3, 3>;
|
||||
template class Vector3<float>;
|
||||
template class Vector2<float>;
|
||||
template class Vector<float, 4>;
|
||||
template class Quaternion<float>;
|
||||
template class AxisAngle<float>;
|
||||
template class Scalar<float>;
|
||||
template class SquareMatrix<float, 4>;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
// check data
|
||||
Eulerf euler_check(0.1f, 0.2f, 0.3f);
|
||||
Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
|
||||
float dcm_data[] = {
|
||||
0.93629336f, -0.27509585f, 0.21835066f,
|
||||
0.28962948f, 0.95642509f, -0.03695701f,
|
||||
-0.19866933f, 0.0978434f, 0.97517033f
|
||||
};
|
||||
Dcmf dcm_check(dcm_data);
|
||||
|
||||
// euler ctor
|
||||
TEST(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f)));
|
||||
|
||||
// euler default ctor
|
||||
Eulerf e;
|
||||
Eulerf e_zero = zeros<float, 3, 1>();
|
||||
TEST(isEqual(e, e_zero));
|
||||
TEST(isEqual(e, e));
|
||||
|
||||
// euler vector ctor
|
||||
Vector3f v(0.1f, 0.2f, 0.3f);
|
||||
Eulerf euler_copy(v);
|
||||
TEST(isEqual(euler_copy, euler_check));
|
||||
|
||||
// quaternion ctor
|
||||
Quatf q0(1, 2, 3, 4);
|
||||
Quatf q(q0);
|
||||
double eps = 1e-6;
|
||||
TEST(fabs(q(0) - 1) < eps);
|
||||
TEST(fabs(q(1) - 2) < eps);
|
||||
TEST(fabs(q(2) - 3) < eps);
|
||||
TEST(fabs(q(3) - 4) < eps);
|
||||
|
||||
// quaternion ctor: vector to vector
|
||||
// identity test
|
||||
Quatf quat_v(v,v);
|
||||
TEST(isEqual(quat_v.conjugate(v), v));
|
||||
// random test (vector norm can not be preserved with a pure rotation)
|
||||
Vector3f v1(-80.1f, 1.5f, -6.89f);
|
||||
quat_v = Quatf(v1, v);
|
||||
TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
|
||||
// special 180 degree case 1
|
||||
v1 = Vector3f(0.f, 1.f, 1.f);
|
||||
quat_v = Quatf(v1, -v1);
|
||||
TEST(isEqual(quat_v.conjugate(v1), -v1));
|
||||
// special 180 degree case 2
|
||||
v1 = Vector3f(1.f, 2.f, 0.f);
|
||||
quat_v = Quatf(v1, -v1);
|
||||
TEST(isEqual(quat_v.conjugate(v1), -v1));
|
||||
// special 180 degree case 3
|
||||
v1 = Vector3f(0.f, 0.f, 1.f);
|
||||
quat_v = Quatf(v1, -v1);
|
||||
TEST(isEqual(quat_v.conjugate(v1), -v1));
|
||||
// special 180 degree case 4
|
||||
v1 = Vector3f(1.f, 1.f, 1.f);
|
||||
quat_v = Quatf(v1, -v1);
|
||||
TEST(isEqual(quat_v.conjugate(v1), -v1));
|
||||
|
||||
// quat normalization
|
||||
q.normalize();
|
||||
TEST(isEqual(q, Quatf(0.18257419f, 0.36514837f,
|
||||
0.54772256f, 0.73029674f)));
|
||||
TEST(isEqual(q0.unit(), q));
|
||||
TEST(isEqual(q0.unit(), q0.normalized()));
|
||||
|
||||
// quat default ctor
|
||||
q = Quatf();
|
||||
TEST(isEqual(q, Quatf(1, 0, 0, 0)));
|
||||
|
||||
// quaternion exponential with v=0
|
||||
v = Vector3f();
|
||||
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
Dcmf M = Dcmf()*0.5f;
|
||||
TEST(isEqual(q, Quatf::expq(v)));
|
||||
TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
|
||||
|
||||
// quaternion exponential with small v
|
||||
v = Vector3f(0.001f,0.002f,-0.003f);
|
||||
q = Quatf(0.999993000008167f, 0.000999997666668f,
|
||||
0.001999995333337f, -0.002999993000005f);
|
||||
{
|
||||
float M_data[] = {
|
||||
0.499997833331311f, 0.001500333333644f, 0.000999499999533f,
|
||||
-0.001499666666356f, 0.499998333331778f, -0.000501000000933f,
|
||||
-0.001000500000467f, 0.000498999999067f, 0.499999166665889f
|
||||
};
|
||||
M = Dcmf(M_data);
|
||||
}
|
||||
TEST(isEqual(q, Quatf::expq(v)));
|
||||
TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
|
||||
|
||||
// quaternion exponential with v
|
||||
v = Vector3f(1.0f, -2.0f, 3.0f);
|
||||
q = Quatf(-0.825299062075259f, -0.150921327219964f,
|
||||
0.301842654439929f, -0.452763981659893f);
|
||||
{
|
||||
float M_data[] = {
|
||||
2.574616981530584f, -1.180828156687602f, -1.478757764968596f,
|
||||
1.819171843312398f, 2.095859216561988f, 0.457515529937193f,
|
||||
0.521242235031404f, 1.457515529937193f, 1.297929608280994f
|
||||
};
|
||||
M = Dcmf(M_data);
|
||||
}
|
||||
TEST(isEqual(q, Quatf::expq(v)));
|
||||
TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
|
||||
|
||||
// quaternion kinematic update
|
||||
q = Quatf();
|
||||
float h=0.001f; // sampling time [s]
|
||||
Vector3f w_B=Vector3f(0.1f,0.2f,0.3f); // body rate in body frame
|
||||
Quatf qa=q+0.5f*h*q.derivative1(w_B);
|
||||
qa.normalize();
|
||||
Quatf qb=q*Quatf::expq(0.5f*h*w_B);
|
||||
TEST(isEqual(qa, qb));
|
||||
|
||||
// euler to quaternion
|
||||
q = Quatf(euler_check);
|
||||
TEST(isEqual(q, q_check));
|
||||
|
||||
// euler to dcm
|
||||
Dcmf dcm(euler_check);
|
||||
TEST(isEqual(dcm, dcm_check));
|
||||
|
||||
// quaternion to euler
|
||||
Eulerf e1(q_check);
|
||||
TEST(isEqual(e1, euler_check));
|
||||
|
||||
// quaternion to dcm
|
||||
Dcmf dcm1(q_check);
|
||||
TEST(isEqual(dcm1, dcm_check));
|
||||
// quaternion z-axis unit base vector
|
||||
Vector3f q_z = q_check.dcm_z();
|
||||
Vector3f R_z(dcm_check(0, 2), dcm_check(1, 2), dcm_check(2, 2));
|
||||
TEST(isEqual(q_z, R_z));
|
||||
|
||||
// dcm default ctor
|
||||
Dcmf dcm2;
|
||||
SquareMatrix<float, 3> I = eye<float, 3>();
|
||||
TEST(isEqual(dcm2, I));
|
||||
|
||||
// dcm to euler
|
||||
Eulerf e2(dcm_check);
|
||||
TEST(isEqual(e2, euler_check));
|
||||
|
||||
// dcm to quaterion
|
||||
Quatf q2(dcm_check);
|
||||
TEST(isEqual(q2, q_check));
|
||||
|
||||
// dcm renormalize
|
||||
Dcmf A = eye<float, 3>();
|
||||
Dcmf R(euler_check);
|
||||
for (size_t i = 0; i < 1000; i++) {
|
||||
A = R * A;
|
||||
}
|
||||
|
||||
A.renormalize();
|
||||
float err = 0.0f;
|
||||
|
||||
for (size_t r = 0; r < 3; r++) {
|
||||
Vector3f rvec(matrix::Matrix<float,1,3>(A.row(r)).transpose());
|
||||
err += fabs(1.0f - rvec.length());
|
||||
}
|
||||
TEST(err < eps);
|
||||
|
||||
// constants
|
||||
double deg2rad = M_PI / 180.0;
|
||||
double rad2deg = 180.0 / M_PI;
|
||||
|
||||
// euler dcm round trip check
|
||||
for (double roll = -90; roll <= 90; roll += 90) {
|
||||
for (double pitch = -90; pitch <= 90; pitch += 90) {
|
||||
for (double yaw = -179; yaw <= 180; yaw += 90) {
|
||||
// note if theta = pi/2, then roll is set to zero
|
||||
double roll_expected = roll;
|
||||
double yaw_expected = yaw;
|
||||
|
||||
if (fabs(pitch -90) < eps) {
|
||||
roll_expected = 0;
|
||||
yaw_expected = yaw - roll;
|
||||
|
||||
} else if (fabs(pitch + 90) < eps) {
|
||||
roll_expected = 0;
|
||||
yaw_expected = yaw + roll;
|
||||
}
|
||||
|
||||
if (yaw_expected < -180) {
|
||||
yaw_expected += 360;
|
||||
}
|
||||
|
||||
if (yaw_expected > 180) {
|
||||
yaw_expected -= 360;
|
||||
}
|
||||
|
||||
//printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
|
||||
Euler<double> euler_expected(
|
||||
deg2rad * roll_expected,
|
||||
deg2rad * pitch,
|
||||
deg2rad * yaw_expected);
|
||||
Euler<double> euler(
|
||||
deg2rad * roll,
|
||||
deg2rad * pitch,
|
||||
deg2rad * yaw);
|
||||
Dcm<double> dcm_from_euler(euler);
|
||||
//dcm_from_euler.print();
|
||||
Euler<double> euler_out(dcm_from_euler);
|
||||
TEST(isEqual(rad2deg * euler_expected, rad2deg * euler_out));
|
||||
|
||||
Eulerf eulerf_expected(
|
||||
float(deg2rad)*float(roll_expected),
|
||||
float(deg2rad)*float(pitch),
|
||||
float(deg2rad)*float(yaw_expected));
|
||||
Eulerf eulerf(float(deg2rad)*float(roll),
|
||||
float(deg2rad)*float(pitch),
|
||||
float(deg2rad)*float(yaw));
|
||||
Dcm<float> dcm_from_eulerf;
|
||||
dcm_from_eulerf = eulerf;
|
||||
Euler<float> euler_outf(dcm_from_eulerf);
|
||||
TEST(isEqual(float(rad2deg)*eulerf_expected,
|
||||
float(rad2deg)*euler_outf));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// quaterion copy ctors
|
||||
float data_v4[] = {1, 2, 3, 4};
|
||||
Vector<float, 4> v4(data_v4);
|
||||
Quatf q_from_v(v4);
|
||||
TEST(isEqual(q_from_v, v4));
|
||||
|
||||
Matrix<float, 4, 1> m4(data_v4);
|
||||
Quatf q_from_m(m4);
|
||||
TEST(isEqual(q_from_m, m4));
|
||||
|
||||
// quaternion derivative in frame 1
|
||||
Quatf q1(0, 1, 0, 0);
|
||||
Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
|
||||
float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
|
||||
Vector<float, 4> q1_dot1_check(data_q_dot1_check);
|
||||
TEST(isEqual(q1_dot1, q1_dot1_check));
|
||||
|
||||
// quaternion derivative in frame 2
|
||||
Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
|
||||
float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
|
||||
Vector<float, 4> q1_dot2_check(data_q_dot2_check);
|
||||
TEST(isEqual(q1_dot2, q1_dot2_check));
|
||||
|
||||
// quaternion product
|
||||
Quatf q_prod_check(
|
||||
0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
|
||||
TEST(isEqual(q_prod_check, q_check * q_check));
|
||||
q_check *= q_check;
|
||||
TEST(isEqual(q_prod_check, q_check));
|
||||
|
||||
// Quaternion scalar multiplication
|
||||
float scalar = 0.5;
|
||||
Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);
|
||||
Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar,
|
||||
3.0f * scalar, 4.0f * scalar);
|
||||
Quatf q_scalar_mul_res = scalar * q_scalar_mul;
|
||||
TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res));
|
||||
Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
|
||||
TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res2));
|
||||
Quatf q_scalar_mul_res3(q_scalar_mul);
|
||||
q_scalar_mul_res3 *= scalar;
|
||||
TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res3));
|
||||
|
||||
// quaternion inverse
|
||||
q = q_check.inversed();
|
||||
TEST(fabs(q_check(0) - q(0)) < eps);
|
||||
TEST(fabs(q_check(1) + q(1)) < eps);
|
||||
TEST(fabs(q_check(2) + q(2)) < eps);
|
||||
TEST(fabs(q_check(3) + q(3)) < eps);
|
||||
|
||||
q = q_check;
|
||||
q.invert();
|
||||
TEST(fabs(q_check(0) - q(0)) < eps);
|
||||
TEST(fabs(q_check(1) + q(1)) < eps);
|
||||
TEST(fabs(q_check(2) + q(2)) < eps);
|
||||
TEST(fabs(q_check(3) + q(3)) < eps);
|
||||
|
||||
// quaternion canonical
|
||||
Quatf q_non_canonical_1(-0.7f,0.4f, 0.3f, -0.3f);
|
||||
Quatf q_canonical_1(0.7f,-0.4f, -0.3f, 0.3f);
|
||||
Quatf q_canonical_ref_1(0.7f,-0.4f, -0.3f, 0.3f);
|
||||
TEST(isEqual(q_non_canonical_1.canonical(),q_canonical_ref_1));
|
||||
TEST(isEqual(q_canonical_1.canonical(),q_canonical_ref_1));
|
||||
q_non_canonical_1.canonicalize();
|
||||
q_canonical_1.canonicalize();
|
||||
TEST(isEqual(q_non_canonical_1,q_canonical_ref_1));
|
||||
TEST(isEqual(q_canonical_1,q_canonical_ref_1));
|
||||
|
||||
Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f);
|
||||
Quatf q_canonical_2(0.0f, 1.0f, 0.0f, 0.0f);
|
||||
Quatf q_canonical_ref_2(0.0f, 1.0f, 0.0f, 0.0f);
|
||||
TEST(isEqual(q_non_canonical_2.canonical(),q_canonical_ref_2));
|
||||
TEST(isEqual(q_canonical_2.canonical(),q_canonical_ref_2));
|
||||
q_non_canonical_2.canonicalize();
|
||||
q_canonical_2.canonicalize();
|
||||
TEST(isEqual(q_non_canonical_2,q_canonical_ref_2));
|
||||
TEST(isEqual(q_canonical_2,q_canonical_ref_2));
|
||||
|
||||
Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f);
|
||||
Quatf q_canonical_3(0.0f, 0.0f, 1.0f, 0.0f);
|
||||
Quatf q_canonical_ref_3(0.0f, 0.0f, 1.0f, 0.0f);
|
||||
TEST(isEqual(q_non_canonical_3.canonical(),q_canonical_ref_3));
|
||||
TEST(isEqual(q_canonical_3.canonical(),q_canonical_ref_3));
|
||||
q_non_canonical_3.canonicalize();
|
||||
q_canonical_3.canonicalize();
|
||||
TEST(isEqual(q_non_canonical_3,q_canonical_ref_3));
|
||||
TEST(isEqual(q_canonical_3,q_canonical_ref_3));
|
||||
|
||||
Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f);
|
||||
Quatf q_canonical_4(0.0f, 0.0f, 0.0f, 1.0f);
|
||||
Quatf q_canonical_ref_4(0.0f, 0.0f, 0.0f, 1.0f);
|
||||
TEST(isEqual(q_non_canonical_4.canonical(),q_canonical_ref_4));
|
||||
TEST(isEqual(q_canonical_4.canonical(),q_canonical_ref_4));
|
||||
q_non_canonical_4.canonicalize();
|
||||
q_canonical_4.canonicalize();
|
||||
TEST(isEqual(q_non_canonical_4,q_canonical_ref_4));
|
||||
TEST(isEqual(q_canonical_4,q_canonical_ref_4));
|
||||
|
||||
Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
|
||||
Quatf q_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
|
||||
Quatf q_canonical_ref_5(0.0f, 0.0f, 0.0f, 0.0f);
|
||||
TEST(isEqual(q_non_canonical_5.canonical(),q_canonical_ref_5));
|
||||
TEST(isEqual(q_canonical_5.canonical(),q_canonical_ref_5));
|
||||
q_non_canonical_5.canonicalize();
|
||||
q_canonical_5.canonicalize();
|
||||
TEST(isEqual(q_non_canonical_5,q_canonical_ref_5));
|
||||
TEST(isEqual(q_canonical_5,q_canonical_ref_5));
|
||||
|
||||
// quaternion setIdentity
|
||||
Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f);
|
||||
q_nonIdentity.setIdentity();
|
||||
TEST(isEqual(q_nonIdentity, Quatf()));
|
||||
|
||||
// non-unit quaternion invese
|
||||
Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);
|
||||
TEST(isEqual(q_nonunit*q_nonunit.inversed(), Quatf()));
|
||||
|
||||
// rotate quaternion (nonzero rotation)
|
||||
Vector3f rot(1.f, 0.f, 0.f);
|
||||
Quatf q_test;
|
||||
q_test.rotate(rot);
|
||||
Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f);
|
||||
TEST(isEqual(q_test, q_true));
|
||||
|
||||
// rotate quaternion (zero rotation)
|
||||
rot(0) = rot(1) = rot(2) = 0.0f;
|
||||
q_test = Quatf();
|
||||
q_test.rotate(rot);
|
||||
q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f);
|
||||
TEST(isEqual(q_test, q_true));
|
||||
|
||||
// rotate quaternion (random non-commutating rotation)
|
||||
q = Quatf(AxisAnglef(5.1f, 3.2f, 8.4f));
|
||||
rot = Vector3f(1.1f, 2.5f, 3.8f);
|
||||
q.rotate(rot);
|
||||
q_true = Quatf(0.3019f, 0.2645f, 0.2268f, 0.8874f);
|
||||
TEST(isEqual(q, q_true));
|
||||
|
||||
// get rotation axis from quaternion (nonzero rotation)
|
||||
q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f);
|
||||
rot = AxisAnglef(q);
|
||||
TEST(fabs(rot(0)) < eps);
|
||||
TEST(fabs(rot(1) - 1.0f) < eps);
|
||||
TEST(fabs(rot(2)) < eps);
|
||||
|
||||
// get rotation axis from quaternion (zero rotation)
|
||||
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
rot = AxisAnglef(q);
|
||||
TEST(fabs(rot(0)) < eps);
|
||||
TEST(fabs(rot(1)) < eps);
|
||||
TEST(fabs(rot(2)) < eps);
|
||||
|
||||
// from axis angle (zero rotation)
|
||||
rot(0) = rot(1) = rot(2) = 0.0f;
|
||||
q = Quatf(AxisAnglef(rot));
|
||||
q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
TEST(isEqual(q, q_true));
|
||||
|
||||
// from axis angle, with length of vector the rotation
|
||||
float n = float(sqrt(4*M_PI*M_PI/3));
|
||||
q = AxisAnglef(n, n, n);
|
||||
TEST(isEqual(q, Quatf(-1, 0, 0, 0)));
|
||||
q = AxisAnglef(0, 0, 0);
|
||||
TEST(isEqual(q, Quatf(1, 0, 0, 0)));
|
||||
|
||||
// Quaternion initialisation per array
|
||||
float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
|
||||
Quaternion<float>q_from_array(q_array);
|
||||
|
||||
for (size_t i = 0; i < 4; i++) {
|
||||
TEST(fabs(q_from_array(i) - q_array[i]) < eps);
|
||||
}
|
||||
|
||||
// axis angle
|
||||
AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f));
|
||||
TEST(isEqual(aa_true, Vector3f(1.0f, 2.0f, 3.0f)));
|
||||
AxisAnglef aa_empty;
|
||||
TEST(isEqual(aa_empty, AxisAnglef(0.0f, 0.0f, 0.0f)));
|
||||
float aa_data[] = {4.0f, 5.0f, 6.0f};
|
||||
AxisAnglef aa_data_init(aa_data);
|
||||
TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f)));
|
||||
|
||||
AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f));
|
||||
TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0)));
|
||||
TEST(isEqualF(aa_norm_check.angle(), 0.0f));
|
||||
|
||||
q = Quatf(-0.29555112749297824f, 0.25532186f, 0.51064372f, 0.76596558f);
|
||||
float r_array[9] = {-0.6949206f, 0.713521f, 0.089292854f, -0.19200698f, -0.30378509f, 0.93319237f, 0.69297814f, 0.63134968f, 0.34810752f};
|
||||
R = Dcmf(r_array);
|
||||
TEST(isEqual(q.imag(), Vector3f(0.25532186f, 0.51064372f, 0.76596558f)));
|
||||
|
||||
// from dcm
|
||||
TEST(isEqual(Quatf(R), q));
|
||||
TEST(isEqual(Quatf(Dcmf(q)), q));
|
||||
|
||||
// to dcm
|
||||
TEST(isEqual(Dcmf(q), R));
|
||||
TEST(isEqual(Dcmf(Quatf(R)), R));
|
||||
|
||||
// conjugate
|
||||
v = Vector3f(1.5f, 2.2f, 3.2f);
|
||||
TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
|
||||
TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
|
||||
|
||||
AxisAnglef aa_q_init(q);
|
||||
TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
|
||||
|
||||
AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f));
|
||||
TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 0.0f)));
|
||||
|
||||
Dcmf dcm_aa_check = AxisAnglef(dcm_check);
|
||||
TEST(isEqual(dcm_aa_check, dcm_check));
|
||||
|
||||
AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f);
|
||||
TEST(isEqual(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f)));
|
||||
TEST(isEqual(aa_axis_angle_init.axis(), Vector3f(0.26726124f, 0.53452248f, 0.80178373f)));
|
||||
TEST(isEqualF(aa_axis_angle_init.angle(), 3.0f));
|
||||
TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))),
|
||||
Quatf(1.0f, 0.0f, 0.0f, 0.0f)));
|
||||
|
||||
|
||||
// check consistentcy of quaternion and dcm product
|
||||
Dcmf dcm3(Eulerf(1, 2, 3));
|
||||
Dcmf dcm4(Eulerf(4, 5, 6));
|
||||
Dcmf dcm34 = dcm3 * dcm4;
|
||||
TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34)));
|
||||
|
||||
// check corner cases of matrix to quaternion conversion
|
||||
q = Quatf(0,1,0,0); // 180 degree rotation around the x axis
|
||||
R = Dcmf(q);
|
||||
TEST(isEqual(q, Quatf(R)));
|
||||
q = Quatf(0,0,1,0); // 180 degree rotation around the y axis
|
||||
R = Dcmf(q);
|
||||
TEST(isEqual(q, Quatf(R)));
|
||||
q = Quatf(0,0,0,1); // 180 degree rotation around the z axis
|
||||
R = Dcmf(q);
|
||||
TEST(isEqual(q, Quatf(R)));
|
||||
|
||||
#if defined(SUPPORT_STDIOSTREAM)
|
||||
std::cout << "q:" << q;
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,72 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
namespace {
|
||||
void doTheCopy(const Matrix<float, 2, 3>& A, float array_A[6])
|
||||
{
|
||||
A.copyTo(array_A);
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
float eps = 1e-6f;
|
||||
|
||||
// Vector3 copyTo
|
||||
const Vector3f v(1, 2, 3);
|
||||
float dst3[3] = {};
|
||||
v.copyTo(dst3);
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
TEST(fabs(v(i) - dst3[i]) < eps);
|
||||
}
|
||||
|
||||
// Quaternion copyTo
|
||||
Quatf q(1, 2, 3, 4);
|
||||
float dst4[4] = {};
|
||||
q.copyTo(dst4);
|
||||
for (size_t i = 0; i < 4; i++) {
|
||||
TEST(fabs(q(i) - dst4[i]) < eps);
|
||||
}
|
||||
|
||||
// Matrix copyTo
|
||||
Matrix<float, 2, 3> A;
|
||||
A(0,0) = 1;
|
||||
A(0,1) = 2;
|
||||
A(0,2) = 3;
|
||||
A(1,0) = 4;
|
||||
A(1,1) = 5;
|
||||
A(1,2) = 6;
|
||||
float array_A[6] = {};
|
||||
doTheCopy(A, array_A);
|
||||
float array_row[6] = {1, 2, 3, 4, 5, 6};
|
||||
for (size_t i = 0; i < 6; i++) {
|
||||
TEST(fabs(array_A[i] - array_row[i]) < eps);
|
||||
}
|
||||
|
||||
// Matrix copyToColumnMajor
|
||||
A.copyToColumnMajor(array_A);
|
||||
float array_column[6] = {1, 4, 2, 5, 3, 6};
|
||||
for (size_t i = 0; i < 6; i++) {
|
||||
TEST(fabs(array_A[i] - array_column[i]) < eps);
|
||||
}
|
||||
|
||||
// Slice copyTo
|
||||
float dst5[2] = {};
|
||||
v.slice<2,1>(0,0).copyTo(dst5);
|
||||
for (size_t i = 0; i < 2; i++) {
|
||||
TEST(fabs(v(i) - dst5[i]) < eps);
|
||||
}
|
||||
|
||||
float subarray_A[4] = {};
|
||||
A.slice<2,2>(0,0).copyToColumnMajor(subarray_A);
|
||||
float subarray_column[4] = {1,4,2,5};
|
||||
for (size_t i = 0; i < 4; i++) {
|
||||
TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,310 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
#include <iostream>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
template <typename Scalar, size_t N>
|
||||
bool isEqualAll(Dual<Scalar, N> a, Dual<Scalar, N> b)
|
||||
{
|
||||
return isEqualF(a.value, b.value) && a.derivative == b.derivative;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T testFunction(const Vector<T, 3>& point) {
|
||||
// function is f(x,y,z) = x^2 + 2xy + 3y^2 + z
|
||||
return point(0)*point(0)
|
||||
+ 2.f * point(0) * point(1)
|
||||
+ 3.f * point(1) * point(1)
|
||||
+ point(2);
|
||||
}
|
||||
|
||||
template <typename Scalar>
|
||||
Vector<Scalar, 3> positionError(const Vector<Scalar, 3>& positionState,
|
||||
const Vector<Scalar, 3>& velocityStateBody,
|
||||
const Quaternion<Scalar>& bodyOrientation,
|
||||
const Vector<Scalar, 3>& positionMeasurement,
|
||||
Scalar dt
|
||||
)
|
||||
{
|
||||
return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
const Dual<float, 1> a(3,0);
|
||||
const Dual<float, 1> b(6,0);
|
||||
|
||||
{
|
||||
TEST(isEqualF(a.value, 3.f));
|
||||
TEST(isEqualF(a.derivative(0), 1.f));
|
||||
}
|
||||
|
||||
{
|
||||
// addition
|
||||
Dual<float, 1> c = a + b;
|
||||
TEST(isEqualF(c.value, 9.f));
|
||||
TEST(isEqualF(c.derivative(0), 2.f));
|
||||
|
||||
Dual<float, 1> d = +a;
|
||||
TEST(isEqualAll(d, a));
|
||||
d += b;
|
||||
TEST(isEqualAll(d, c));
|
||||
|
||||
Dual<float, 1> e = a;
|
||||
e += b.value;
|
||||
TEST(isEqualF(e.value, c.value));
|
||||
TEST(isEqual(e.derivative, a.derivative));
|
||||
|
||||
Dual<float, 1> f = b.value + a;
|
||||
TEST(isEqualAll(f, e));
|
||||
}
|
||||
|
||||
{
|
||||
// subtraction
|
||||
Dual<float, 1> c = b - a;
|
||||
TEST(isEqualF(c.value, 3.f));
|
||||
TEST(isEqualF(c.derivative(0), 0.f));
|
||||
|
||||
Dual<float, 1> d = b;
|
||||
TEST(isEqualAll(d, b));
|
||||
d -= a;
|
||||
TEST(isEqualAll(d, c));
|
||||
|
||||
Dual<float, 1> e = b;
|
||||
e -= a.value;
|
||||
TEST(isEqualF(e.value, c.value));
|
||||
TEST(isEqual(e.derivative, b.derivative));
|
||||
|
||||
Dual<float, 1> f = a.value - b;
|
||||
TEST(isEqualAll(f, -e));
|
||||
}
|
||||
|
||||
{
|
||||
// multiplication
|
||||
Dual<float, 1> c = a*b;
|
||||
TEST(isEqualF(c.value, 18.f));
|
||||
TEST(isEqualF(c.derivative(0), 9.f));
|
||||
|
||||
Dual<float, 1> d = a;
|
||||
TEST(isEqualAll(d, a));
|
||||
d *= b;
|
||||
TEST(isEqualAll(d, c));
|
||||
|
||||
Dual<float, 1> e = a;
|
||||
e *= b.value;
|
||||
TEST(isEqualF(e.value, c.value));
|
||||
TEST(isEqual(e.derivative, a.derivative * b.value));
|
||||
|
||||
Dual<float, 1> f = b.value * a;
|
||||
TEST(isEqualAll(f, e));
|
||||
}
|
||||
|
||||
{
|
||||
// division
|
||||
Dual<float, 1> c = b/a;
|
||||
TEST(isEqualF(c.value, 2.f));
|
||||
TEST(isEqualF(c.derivative(0), -1.f/3.f));
|
||||
|
||||
Dual<float, 1> d = b;
|
||||
TEST(isEqualAll(d, b));
|
||||
d /= a;
|
||||
TEST(isEqualAll(d, c));
|
||||
|
||||
Dual<float, 1> e = b;
|
||||
e /= a.value;
|
||||
TEST(isEqualF(e.value, c.value));
|
||||
TEST(isEqual(e.derivative, b.derivative / a.value));
|
||||
|
||||
Dual<float, 1> f = a.value / b;
|
||||
TEST(isEqualAll(f, 1.f/e));
|
||||
}
|
||||
|
||||
{
|
||||
Dual<float, 1> blank;
|
||||
TEST(isEqualF(blank.value, 0.f));
|
||||
TEST(isEqualF(blank.derivative(0), 0.f));
|
||||
}
|
||||
|
||||
{
|
||||
// sqrt
|
||||
TEST(isEqualF(sqrt(a).value, sqrt(a.value)));
|
||||
TEST(isEqualF(sqrt(a).derivative(0), 1.f/sqrt(12.f)));
|
||||
}
|
||||
|
||||
{
|
||||
// abs
|
||||
TEST(isEqualAll(a, abs(-a)));
|
||||
TEST(!isEqualAll(-a, abs(a)));
|
||||
TEST(isEqualAll(-a, -abs(a)));
|
||||
}
|
||||
|
||||
{
|
||||
// ceil
|
||||
Dual<float, 1> c(1.5,0);
|
||||
TEST(isEqualF(ceil(c).value, ceil(c.value)));
|
||||
TEST(isEqualF(ceil(c).derivative(0), 0.f));
|
||||
}
|
||||
|
||||
{
|
||||
// floor
|
||||
Dual<float, 1> c(1.5,0);
|
||||
TEST(isEqualF(floor(c).value, floor(c.value)));
|
||||
TEST(isEqualF(floor(c).derivative(0), 0.f));
|
||||
}
|
||||
|
||||
{
|
||||
// fmod
|
||||
TEST(isEqualF(fmod(a, 0.8f).value, fmod(a.value, 0.8f)));
|
||||
TEST(isEqual(fmod(a, 0.8f).derivative, a.derivative));
|
||||
}
|
||||
|
||||
{
|
||||
// max/min
|
||||
TEST(isEqualAll(b, max(a, b)));
|
||||
TEST(isEqualAll(a, min(a, b)));
|
||||
}
|
||||
|
||||
{
|
||||
// isnan
|
||||
TEST(!IsNan(a));
|
||||
Dual<float, 1> c(sqrt(-1.f),0);
|
||||
TEST(IsNan(c));
|
||||
}
|
||||
|
||||
{
|
||||
// isfinite/isinf
|
||||
TEST(IsFinite(a));
|
||||
TEST(!IsInf(a));
|
||||
Dual<float, 1> c(sqrt(-1.f),0);
|
||||
TEST(!IsFinite(c));
|
||||
TEST(!IsInf(c));
|
||||
Dual<float, 1> d(INFINITY,0);
|
||||
TEST(!IsFinite(d));
|
||||
TEST(IsInf(d));
|
||||
}
|
||||
|
||||
{
|
||||
// sin/cos/tan
|
||||
TEST(isEqualF(sin(a).value, sin(a.value)));
|
||||
TEST(isEqualF(sin(a).derivative(0), cos(a.value))); // sin'(x) = cos(x)
|
||||
|
||||
TEST(isEqualF(cos(a).value, cos(a.value)));
|
||||
TEST(isEqualF(cos(a).derivative(0), -sin(a.value))); // cos'(x) = -sin(x)
|
||||
|
||||
TEST(isEqualF(tan(a).value, tan(a.value)));
|
||||
TEST(isEqualF(tan(a).derivative(0), 1.f + tan(a.value)*tan(a.value))); // tan'(x) = 1 + tan^2(x)
|
||||
}
|
||||
|
||||
{
|
||||
// asin/acos/atan
|
||||
Dual<float, 1> c(0.3f, 0);
|
||||
TEST(isEqualF(asin(c).value, asin(c.value)));
|
||||
TEST(isEqualF(asin(c).derivative(0), 1.f/sqrt(1.f - 0.3f*0.3f))); // asin'(x) = 1/sqrt(1-x^2)
|
||||
|
||||
TEST(isEqualF(acos(c).value, acos(c.value)));
|
||||
TEST(isEqualF(acos(c).derivative(0), -1.f/sqrt(1.f - 0.3f*0.3f))); // acos'(x) = -1/sqrt(1-x^2)
|
||||
|
||||
TEST(isEqualF(atan(c).value, atan(c.value)));
|
||||
TEST(isEqualF(atan(c).derivative(0), 1.f/(1.f + 0.3f*0.3f))); // tan'(x) = 1 + x^2
|
||||
}
|
||||
|
||||
{
|
||||
// atan2
|
||||
TEST(isEqualF(atan2(a, b).value, atan2(a.value, b.value)));
|
||||
TEST(isEqualAll(atan2(a, Dual<float,1>(b.value)), atan(a/b.value))); // atan2'(y, x) = atan'(y/x)
|
||||
}
|
||||
|
||||
{
|
||||
// partial derivatives
|
||||
// function is f(x,y,z) = x^2 + 2xy + 3y^2 + z, we need with respect to d/dx and d/dy at the point (0.5, -0.8, 2)
|
||||
|
||||
using D = Dual<float, 2>;
|
||||
|
||||
// set our starting point, requesting partial derivatives of x and y in column 0 and 1
|
||||
Vector3<D> dualPoint(D(0.5f, 0), D(-0.8f, 1), D(2.f));
|
||||
|
||||
Dual<float, 2> dualResult = testFunction(dualPoint);
|
||||
|
||||
// compare to a numerical derivative:
|
||||
Vector<float, 3> floatPoint = collectReals(dualPoint);
|
||||
float floatResult = testFunction(floatPoint);
|
||||
|
||||
float h = 0.0001f;
|
||||
Vector<float, 3> floatPoint_plusDX = floatPoint;
|
||||
floatPoint_plusDX(0) += h;
|
||||
float floatResult_plusDX = testFunction(floatPoint_plusDX);
|
||||
|
||||
Vector<float, 3> floatPoint_plusDY = floatPoint;
|
||||
floatPoint_plusDY(1) += h;
|
||||
float floatResult_plusDY = testFunction(floatPoint_plusDY);
|
||||
|
||||
Vector2f numerical_derivative((floatResult_plusDX - floatResult)/h,
|
||||
(floatResult_plusDY - floatResult)/h);
|
||||
|
||||
TEST(isEqualF(dualResult.value, floatResult, 0.0f));
|
||||
TEST(isEqual(dualResult.derivative, numerical_derivative, 1e-2f));
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
// jacobian
|
||||
// get residual of x/y/z with partial derivatives of rotation
|
||||
|
||||
Vector3f direct_error;
|
||||
Matrix<float, 3, 4> numerical_jacobian;
|
||||
{
|
||||
Vector3f positionState(5,6,7);
|
||||
Vector3f velocityState(-1,0,1);
|
||||
Quaternionf velocityOrientation(0.2f,-0.1f,0,1);
|
||||
Vector3f positionMeasurement(4.5f, 6.2f, 7.9f);
|
||||
float dt = 0.1f;
|
||||
|
||||
direct_error = positionError(positionState,
|
||||
velocityState,
|
||||
velocityOrientation,
|
||||
positionMeasurement,
|
||||
dt);
|
||||
float h = 0.001f;
|
||||
for (size_t i = 0; i < 4; i++)
|
||||
{
|
||||
Quaternion<float> h4 = velocityOrientation;
|
||||
h4(i) += h;
|
||||
numerical_jacobian.col(i) = (positionError(positionState,
|
||||
velocityState,
|
||||
h4,
|
||||
positionMeasurement,
|
||||
dt)
|
||||
- direct_error)/h;
|
||||
}
|
||||
}
|
||||
Vector3f auto_error;
|
||||
Matrix<float, 3, 4> auto_jacobian;
|
||||
{
|
||||
using D4 = Dual<float, 4>;
|
||||
using Vector3d4 = Vector3<D4>;
|
||||
Vector3d4 positionState(D4(5), D4(6), D4(7));
|
||||
Vector3d4 velocityState(D4(-1), D4(0), D4(1));
|
||||
|
||||
// request partial derivatives of velocity orientation
|
||||
// by setting these variables' derivatives in corresponding columns [0...3]
|
||||
Quaternion<D4> velocityOrientation(D4(0.2f, 0),D4(-0.1f, 1),D4(0, 2),D4(1, 3));
|
||||
|
||||
Vector3d4 positionMeasurement(D4(4.5f), D4(6.2f), D4(7.9f));
|
||||
D4 dt(0.1f);
|
||||
|
||||
|
||||
Vector3d4 error = positionError(positionState,
|
||||
velocityState,
|
||||
velocityOrientation,
|
||||
positionMeasurement,
|
||||
dt);
|
||||
auto_error = collectReals(error);
|
||||
auto_jacobian = collectDerivatives(error);
|
||||
}
|
||||
TEST(isEqual(direct_error, auto_error, 0.0f));
|
||||
TEST(isEqual(numerical_jacobian, auto_jacobian, 1e-3f));
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/filter.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
const size_t n_x = 6;
|
||||
const size_t n_y = 5;
|
||||
SquareMatrix<float, n_x> P = eye<float, n_x>();
|
||||
SquareMatrix<float, n_y> R = eye<float, n_y>();
|
||||
Matrix<float, n_y, n_x> C;
|
||||
C.setIdentity();
|
||||
float data[] = {1,2,3,4,5};
|
||||
Vector<float, n_y> r(data);
|
||||
|
||||
Vector<float, n_x> dx;
|
||||
SquareMatrix<float, n_x> dP;
|
||||
float beta = 0;
|
||||
kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
|
||||
|
||||
float data_check[] = {0.5,1,1.5,2,2.5,0};
|
||||
Vector<float, n_x> dx_check(data_check);
|
||||
TEST(isEqual(dx, dx_check));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,12 @@
|
||||
set_directory_properties(PROPERTIES COMPILE_OPTIONS "")
|
||||
|
||||
# Download and unpack googletest at configure time
|
||||
configure_file(${CMAKE_CURRENT_LIST_DIR}/CMakeLists.txt.in googletest-download/CMakeLists.txt)
|
||||
execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . RESULT_VARIABLE result1 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download)
|
||||
execute_process(COMMAND ${CMAKE_COMMAND} --build . RESULT_VARIABLE result2 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download)
|
||||
if(result1 OR result2)
|
||||
message(FATAL_ERROR "Preparing googletest failed: ${result1} ${result2}")
|
||||
endif()
|
||||
|
||||
# Add googletest, defines gtest and gtest_main targets
|
||||
add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src ${CMAKE_CURRENT_BINARY_DIR}/googletest-build EXCLUDE_FROM_ALL)
|
||||
@@ -0,0 +1,19 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Euler<float> euler(0.1f, 0.2f, 0.3f);
|
||||
Dcm<float> R(euler);
|
||||
Dcm<float> skew = R - R.T();
|
||||
Vector3<float> w = skew.vee();
|
||||
Vector3<float> w_check(0.1348f, 0.4170f, 0.5647f);
|
||||
|
||||
TEST(isEqual(w, w_check));
|
||||
TEST(isEqual(skew, w.hat()));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,78 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
// general wraps
|
||||
TEST(fabs(wrap(4., 0., 10.) - 4.) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(4., 0., 1.)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(-4., 0., 10.) - 6.) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(-18., 0., 10.) - 2.) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(-1.5, 3., 5.) - 4.5) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(15.5, 3., 5.) - 3.5) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(-1., 30., 40.) - 39.) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(-8000., -555., 1.) - (-216.)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(0., 0., 360.)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(0. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(0. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(360., 0., 360.)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
|
||||
|
||||
// integer wraps
|
||||
TEST(wrap(-10, 0, 10) == 0);
|
||||
TEST(wrap(-4, 0, 10) == 6);
|
||||
TEST(wrap(0, 0, 10) == 0)
|
||||
TEST(wrap(4, 0, 10) == 4);
|
||||
TEST(wrap(10, 0, 10) == 0);
|
||||
|
||||
// wrap pi
|
||||
TEST(fabs(wrap_pi(0.)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_pi(4.) - (4. - M_TWOPI)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_pi(-4.) - (-4. + M_TWOPI)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_pi(3.) - (3.)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_pi(100.) - (100. - 32. * M_PI)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_pi(-100.) - (-100. + 32. * M_PI)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_pi(-101.) - (-101. + 32. * M_PI)) < FLT_EPSILON);
|
||||
TEST(!is_finite(wrap_pi(NAN)));
|
||||
|
||||
// wrap 2pi
|
||||
TEST(fabs(wrap_2pi(0.)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_2pi(-4.) - (-4. + 2. * M_PI)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_2pi(3.) - (3.)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_2pi(200.) - (200. - 31. * M_TWOPI)) < FLT_EPSILON);
|
||||
TEST(fabs(wrap_2pi(-201.) - (-201. + 32. * M_TWOPI)) < FLT_EPSILON);
|
||||
TEST(!is_finite(wrap_2pi(NAN)));
|
||||
|
||||
// Equality checks
|
||||
TEST(isEqualF(1., 1.));
|
||||
TEST(!isEqualF(1., 2.));
|
||||
TEST(!isEqualF(NAN, 1.f));
|
||||
TEST(!isEqualF(1.f, NAN));
|
||||
TEST(!isEqualF(INFINITY, 1.f));
|
||||
TEST(!isEqualF(1.f, INFINITY));
|
||||
TEST(isEqualF(NAN, NAN));
|
||||
TEST(isEqualF(NAN, -NAN));
|
||||
TEST(isEqualF(-NAN, NAN));
|
||||
TEST(isEqualF(INFINITY, INFINITY));
|
||||
TEST(!isEqualF(INFINITY, -INFINITY));
|
||||
TEST(!isEqualF(-INFINITY, INFINITY));
|
||||
TEST(isEqualF(-INFINITY, -INFINITY));
|
||||
|
||||
Vector3f a(1, 2, 3);
|
||||
Vector3f b(4, 5, 6);
|
||||
TEST(!isEqual(a, b));
|
||||
TEST(isEqual(a, a));
|
||||
|
||||
Vector3f c(1, 2, 3);
|
||||
Vector3f d(1, 2, NAN);
|
||||
TEST(!isEqual(c, d));
|
||||
TEST(isEqual(c, c));
|
||||
TEST(isEqual(d, d));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,26 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/integration.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/);
|
||||
|
||||
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/) {
|
||||
float v = -sin(t);
|
||||
return v*ones<float, 6, 1>();
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
Vector<float, 6> y = ones<float, 6, 1>();
|
||||
Vector<float, 3> u = ones<float, 3, 1>();
|
||||
float t0 = 0;
|
||||
float tf = 2;
|
||||
float h = 0.001f;
|
||||
integrate_rk4(f, y, u, t0, tf, h, y);
|
||||
float v = 1 + cos(tf) - cos(t0);
|
||||
TEST(isEqual(y, (ones<float, 6, 1>()*v)));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,164 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
static const size_t n_large = 50;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {0, 2, 3,
|
||||
4, 5, 6,
|
||||
7, 8, 10
|
||||
};
|
||||
float data_check[9] = {
|
||||
-0.4f, -0.8f, 0.6f,
|
||||
-0.4f, 4.2f, -2.4f,
|
||||
0.6f, -2.8f, 1.6f
|
||||
};
|
||||
|
||||
SquareMatrix<float, 3> A(data);
|
||||
SquareMatrix<float, 3> A_I = inv(A);
|
||||
SquareMatrix<float, 3> A_I_check(data_check);
|
||||
TEST((A_I - A_I_check).abs().max() < 1e-6f);
|
||||
|
||||
float data_2x2[4] = {12, 2,
|
||||
-7, 5
|
||||
};
|
||||
float data_2x2_check[4] = {
|
||||
0.0675675675f, -0.02702702f,
|
||||
0.0945945945f, 0.162162162f
|
||||
};
|
||||
|
||||
SquareMatrix<float, 2> A2x2(data_2x2);
|
||||
SquareMatrix<float, 2> A2x2_I = inv(A2x2);
|
||||
SquareMatrix<float, 2> A2x2_I_check(data_2x2_check);
|
||||
TEST(isEqual(A2x2_I, A2x2_I_check));
|
||||
|
||||
SquareMatrix<float, 2> A2x2_sing = ones<float, 2, 2>();
|
||||
SquareMatrix<float, 2> A2x2_sing_I;
|
||||
TEST(inv(A2x2_sing, A2x2_sing_I) == false);
|
||||
|
||||
SquareMatrix<float, 3> A3x3_sing = ones<float, 3, 3>();
|
||||
SquareMatrix<float, 3> A3x3_sing_I;
|
||||
TEST(inv(A3x3_sing, A3x3_sing_I) == false)
|
||||
|
||||
// stess test
|
||||
SquareMatrix<float, n_large> A_large;
|
||||
A_large.setIdentity();
|
||||
SquareMatrix<float, n_large> A_large_I;
|
||||
A_large_I.setZero();
|
||||
|
||||
for (size_t i = 0; i < n_large; i++) {
|
||||
A_large_I = inv(A_large);
|
||||
TEST(isEqual(A_large, A_large_I));
|
||||
}
|
||||
|
||||
SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
|
||||
TEST(isEqual(inv(zero_test), zeros<float, 3, 3>()));
|
||||
|
||||
// test pivotting
|
||||
float data2[81] = {
|
||||
-2, 1, 1, -1, -5, 1, 2, -1, 0,
|
||||
-3, 2, -1, 0, 2, 2, -1, -5, 3,
|
||||
0, 0, 0, 1, 4, -3, 3, 0, -2,
|
||||
2, 2, -1, -2, -1, 0, 3, 0, 1,
|
||||
-1, 2, -1, -1, -3, 3, 0, -2, 3,
|
||||
0, 1, 1, -3, 3, -2, 0, -4, 0,
|
||||
1, 0, 0, 0, 0, 0, -2, 4, -3,
|
||||
1, -1, 0, -1, -1, 1, -1, -3, 4,
|
||||
0, 3, -1, -2, 2, 1, -2, 0, -1
|
||||
};
|
||||
|
||||
float data2_check[81] = {
|
||||
6, -4, 3, -3, -9, -8, -10, 8, 14,
|
||||
-2, -7, -5, -3, -2, -2, -16, -5, 8,
|
||||
-2, 0, -23, 7, -24, -5, -28, -14, 9,
|
||||
3, -7, 2, -5, -4, -6, -13, 4, 13,
|
||||
-1, 4, -8, 5, -8, 0, -3, -5, -2,
|
||||
6, 7, -7, 7, -21, -7, -5, 3, 6,
|
||||
1, 4, -4, 4, -7, -1, 0, -1, -1,
|
||||
-7, 3, -11, 5, 1, 6, -1, -13, -10,
|
||||
-8, 0, -11, 3, 3, 6, -5, -14, -8
|
||||
};
|
||||
SquareMatrix<float, 9> A2(data2);
|
||||
SquareMatrix<float, 9> A2_I = inv(A2);
|
||||
SquareMatrix<float, 9> A2_I_check(data2_check);
|
||||
TEST((A2_I - A2_I_check).abs().max() < 1e-3f);
|
||||
|
||||
float data3[9] = {
|
||||
0, 1, 2,
|
||||
3, 4, 5,
|
||||
6, 7, 9
|
||||
};
|
||||
float data3_check[9] = {
|
||||
-0.3333333f, -1.6666666f, 1,
|
||||
-1, 4, -2,
|
||||
1, -2, 1
|
||||
};
|
||||
SquareMatrix<float, 3> A3(data3);
|
||||
SquareMatrix<float, 3> A3_I = inv(A3);
|
||||
SquareMatrix<float, 3> A3_I_check(data3_check);
|
||||
TEST(isEqual(inv(A3), A3_I_check));
|
||||
TEST(isEqual(A3_I, A3_I_check));
|
||||
TEST(A3.I(A3_I));
|
||||
TEST(isEqual(A3_I, A3_I_check));
|
||||
|
||||
// cover singular matrices
|
||||
A3(0, 0) = 0;
|
||||
A3(0, 1) = 0;
|
||||
A3(0, 2) = 0;
|
||||
A3_I = inv(A3);
|
||||
SquareMatrix<float, 3> Z3 = zeros<float, 3, 3>();
|
||||
TEST(!A3.I(A3_I));
|
||||
TEST(!Z3.I(A3_I));
|
||||
TEST(isEqual(A3_I, Z3));
|
||||
TEST(isEqual(A3.I(), Z3));
|
||||
|
||||
for(size_t i = 0; i < 9; i++) {
|
||||
A2(0, i) = 0;
|
||||
}
|
||||
A2_I = inv(A2);
|
||||
SquareMatrix<float, 9> Z9 = zeros<float, 9, 9>();
|
||||
TEST(!A2.I(A2_I));
|
||||
TEST(!Z9.I(A2_I));
|
||||
TEST(isEqual(A2_I, Z9));
|
||||
TEST(isEqual(A2.I(), Z9));
|
||||
|
||||
// cover NaN
|
||||
A3(0, 0) = NAN;
|
||||
A3(0, 1) = 0;
|
||||
A3(0, 2) = 0;
|
||||
A3_I = inv(A3);
|
||||
TEST(isEqual(A3_I, Z3));
|
||||
TEST(isEqual(A3.I(), Z3));
|
||||
|
||||
A2(0, 0) = NAN;
|
||||
A2_I = inv(A2);
|
||||
TEST(isEqual(A2_I, Z9));
|
||||
TEST(isEqual(A2.I(), Z9));
|
||||
|
||||
float data4[9] = {
|
||||
1.33471626f, 0.74946721f, -0.0531679f,
|
||||
0.74946721f, 1.07519593f, 0.08036323f,
|
||||
-0.0531679f, 0.08036323f, 1.01618474f
|
||||
};
|
||||
SquareMatrix<float, 3> A4(data4);
|
||||
|
||||
float data4_cholesky[9] = {
|
||||
1.15529921f, 0.f, 0.f,
|
||||
0.6487213f, 0.80892311f, 0.f,
|
||||
-0.04602089f, 0.13625271f, 0.99774847f
|
||||
};
|
||||
SquareMatrix<float, 3> A4_cholesky_check(data4_cholesky);
|
||||
SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
|
||||
TEST(isEqual(A4_cholesky_check, A4_cholesky));
|
||||
|
||||
SquareMatrix<float, 3> I3;
|
||||
I3.setIdentity();
|
||||
TEST(isEqual(choleskyInv(A4)*A4, I3));
|
||||
TEST(isEqual(cholesky(Z3), Z3));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,100 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int test_4x3(void);
|
||||
template<typename Type> int test_4x4(void);
|
||||
int test_4x4_type_double(void);
|
||||
int test_div_zero(void);
|
||||
|
||||
int main()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = test_4x4<float>();
|
||||
if (ret != 0) return ret;
|
||||
|
||||
ret = test_4x4<double>();
|
||||
if (ret != 0) return ret;
|
||||
|
||||
ret = test_4x3();
|
||||
if (ret != 0) return ret;
|
||||
|
||||
ret = test_div_zero();
|
||||
if (ret != 0) return ret;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int test_4x3() {
|
||||
// Start with an (m x n) A matrix
|
||||
float data[12] = {20.f, -10.f, -13.f,
|
||||
17.f, 16.f, -18.f,
|
||||
0.7f, -0.8f, 0.9f,
|
||||
-1.f, -1.1f, -1.2f
|
||||
};
|
||||
Matrix<float, 4, 3> A(data);
|
||||
|
||||
float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
|
||||
Vector<float, 4> b(b_data);
|
||||
|
||||
float x_check_data[3] = {-0.69168233f,
|
||||
-0.26227593f,
|
||||
-1.03767522f
|
||||
};
|
||||
Vector<float, 3> x_check(x_check_data);
|
||||
|
||||
LeastSquaresSolver<float, 4, 3> qrd = LeastSquaresSolver<float, 4, 3>(A);
|
||||
|
||||
Vector<float, 3> x = qrd.solve(b);
|
||||
TEST(isEqual(x, x_check));
|
||||
return 0;
|
||||
}
|
||||
|
||||
template<typename Type>
|
||||
int test_4x4() {
|
||||
// Start with an (m x n) A matrix
|
||||
const Type data[16] = { 20.f, -10.f, -13.f, 21.f,
|
||||
17.f, 16.f, -18.f, -14.f,
|
||||
0.7f, -0.8f, 0.9f, -0.5f,
|
||||
-1.f, -1.1f, -1.2f, -1.3f
|
||||
};
|
||||
Matrix<Type, 4, 4> A(data);
|
||||
|
||||
Type b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
|
||||
Vector<Type, 4> b(b_data);
|
||||
|
||||
Type x_check_data[4] = { 0.97893433f,
|
||||
-2.80798701f,
|
||||
-0.03175765f,
|
||||
-2.19387649f
|
||||
};
|
||||
Vector<Type, 4> x_check(x_check_data);
|
||||
|
||||
LeastSquaresSolver<Type, 4, 4> qrd = LeastSquaresSolver<Type, 4, 4>(A);
|
||||
|
||||
Vector<Type, 4> x = qrd.solve(b);
|
||||
TEST(isEqual(x, x_check));
|
||||
return 0;
|
||||
}
|
||||
|
||||
int test_div_zero() {
|
||||
float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
Matrix<float, 2, 2> A(data);
|
||||
|
||||
float b_data[2] = {1.0f, 1.0f};
|
||||
Vector<float, 2> b(b_data);
|
||||
|
||||
// Implement such that x returns zeros if it reaches div by zero
|
||||
float x_check_data[2] = {0.0f, 0.0f};
|
||||
Vector<float, 2> x_check(x_check_data);
|
||||
|
||||
LeastSquaresSolver<float, 2, 2> qrd = LeastSquaresSolver<float, 2, 2>(A);
|
||||
|
||||
Vector<float, 2> x = qrd.solve(b);
|
||||
TEST(isEqual(x, x_check));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,260 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
template class matrix::Matrix<float, 3, 2>;
|
||||
|
||||
int main()
|
||||
{
|
||||
Matrix3f m;
|
||||
m.setZero();
|
||||
m.zero();
|
||||
m(0, 0) = 1;
|
||||
m(0, 1) = 2;
|
||||
m(0, 2) = 3;
|
||||
m(1, 0) = 4;
|
||||
m(1, 1) = 5;
|
||||
m(1, 2) = 6;
|
||||
m(2, 0) = 7;
|
||||
m(2, 1) = 8;
|
||||
m(2, 2) = 9;
|
||||
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f m2(data);
|
||||
|
||||
for(size_t i=0; i<3; i++) {
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON);
|
||||
}
|
||||
}
|
||||
|
||||
Matrix3f m_nan;
|
||||
m_nan.setNaN();
|
||||
for(size_t i=0; i<3; i++) {
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
TEST(isnan(m_nan(i,j)));
|
||||
}
|
||||
}
|
||||
TEST(m_nan.isAllNan());
|
||||
|
||||
float data2d[3][3] = {
|
||||
{1, 2, 3},
|
||||
{4, 5, 6},
|
||||
{7, 8, 9}
|
||||
};
|
||||
m2 = Matrix3f(data2d);
|
||||
for(size_t i=0; i<3; i++) {
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON);
|
||||
}
|
||||
}
|
||||
TEST(!m2.isAllNan());
|
||||
|
||||
float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
||||
Matrix3f m3(data_times_2);
|
||||
|
||||
TEST(isEqual(m, m2));
|
||||
TEST(!(isEqual(m, m3)));
|
||||
|
||||
m2 *= 2;
|
||||
TEST(isEqual(m2, m3));
|
||||
|
||||
m2 /= 2;
|
||||
m2 -= 1;
|
||||
float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
|
||||
TEST(isEqual(Matrix3f(data_minus_1), m2));
|
||||
|
||||
m2 += 1;
|
||||
TEST(isEqual(Matrix3f(data), m2));
|
||||
|
||||
m3 -= m2;
|
||||
|
||||
TEST(isEqual(m3, m2));
|
||||
|
||||
// set rows and columns to value
|
||||
Matrix3f m2e(data2d);
|
||||
|
||||
float data2e_check1[3][3] = {
|
||||
{1, 11, 3},
|
||||
{4, 11, 6},
|
||||
{7, 11, 9}
|
||||
};
|
||||
Matrix3f m2e_check1(data2e_check1);
|
||||
|
||||
float data2e_check2[3][3] = {
|
||||
{1, 11, 3},
|
||||
{4, 11, 6},
|
||||
{0, 0, 0}
|
||||
};
|
||||
Matrix3f m2e_check2(data2e_check2);
|
||||
|
||||
m2e.setCol(1, 11);
|
||||
TEST(isEqual(m2e, m2e_check1));
|
||||
m2e.setRow(2, 0);
|
||||
TEST(isEqual(m2e, m2e_check2));
|
||||
|
||||
float data_row_02_swap[9] = {
|
||||
7, 8, 9,
|
||||
4, 5, 6,
|
||||
1, 2, 3,
|
||||
};
|
||||
|
||||
float data_col_02_swap[9] = {
|
||||
3, 2, 1,
|
||||
6, 5, 4,
|
||||
9, 8, 7
|
||||
};
|
||||
|
||||
Matrix3f m4(data);
|
||||
|
||||
TEST(isEqual(-m4, m4*(-1)));
|
||||
|
||||
// col swap
|
||||
m4.swapCols(0, 2);
|
||||
TEST(isEqual(m4, Matrix3f(data_col_02_swap)));
|
||||
m4.swapCols(0, 2);
|
||||
|
||||
// row swap
|
||||
m4.swapRows(0, 2);
|
||||
TEST(isEqual(m4, Matrix3f(data_row_02_swap)));
|
||||
m4.swapRows(0, 2);
|
||||
|
||||
// swapping with same row should do nothing
|
||||
m4.swapRows(0, 0);
|
||||
m4.swapRows(1, 1);
|
||||
m4.swapRows(2, 2);
|
||||
TEST(isEqual(m4, Matrix3f(data)));
|
||||
|
||||
// swapping with same col should do nothing
|
||||
m4.swapCols(0, 0);
|
||||
m4.swapCols(1, 1);
|
||||
m4.swapCols(2, 2);
|
||||
TEST(isEqual(m4, Matrix3f(data)));
|
||||
|
||||
TEST(fabs(m4.min() - 1) < FLT_EPSILON);
|
||||
TEST(fabs((-m4).min() + 9) < FLT_EPSILON);
|
||||
|
||||
Scalar<float> s = 1;
|
||||
const Vector<float, 1> & s_vect = s;
|
||||
TEST(fabs(s - 1) < FLT_EPSILON);
|
||||
TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON);
|
||||
|
||||
Matrix<float, 1, 1> m5 = s;
|
||||
TEST(fabs(m5(0,0) - s) < FLT_EPSILON);
|
||||
|
||||
Matrix<float, 2, 2> m6;
|
||||
m6.setRow(0, Vector2f(1, 2));
|
||||
float m7_array[] = {1,2,0,0};
|
||||
Matrix<float, 2, 2> m7(m7_array);
|
||||
TEST(isEqual(m6, m7));
|
||||
m6.setCol(0, Vector2f(3, 4));
|
||||
float m8_array[] = {3,2,4,0};
|
||||
Matrix<float, 2, 2> m8(m8_array);
|
||||
TEST(isEqual(m6, m8));
|
||||
|
||||
m7.setNaN();
|
||||
TEST(m7 != m8);
|
||||
|
||||
// min, max, constrain matrix values with scalar
|
||||
float data_m9[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
||||
float lower_bound = 7;
|
||||
float upper_bound = 11;
|
||||
float data_m9_lower_bounded[9] = {7, 7, 7, 8, 10, 12, 14, 16, 18};
|
||||
float data_m9_upper_bounded[9] = {2, 4, 6, 8, 10, 11, 11, 11, 11};
|
||||
float data_m9_lower_constrained[9] = {7, 7, 7, 8, 10, 11, 11, 11, 11};
|
||||
Matrix3f m9(data_m9);
|
||||
Matrix3f m9_lower_bounded(data_m9_lower_bounded);
|
||||
Matrix3f m9_upper_bounded(data_m9_upper_bounded);
|
||||
Matrix3f m9_lower_upper_constrained(data_m9_lower_constrained);
|
||||
TEST(isEqual(max(m9, lower_bound), m9_lower_bounded));
|
||||
TEST(isEqual(max(lower_bound, m9), m9_lower_bounded));
|
||||
TEST(isEqual(min(m9, upper_bound), m9_upper_bounded));
|
||||
TEST(isEqual(min(upper_bound, m9), m9_upper_bounded));
|
||||
TEST(isEqual(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained));
|
||||
TEST(isEqual(constrain(m9, 8.0f, 7.0f), m_nan));
|
||||
|
||||
// min, max, constrain matrix values with matrix of same size
|
||||
float data_m10[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
||||
float data_m10_lower_bound[9] = {5, 7, 4, 8, 19, 10, 20, 16, 18};
|
||||
float data_m10_lower_bounded_ref[9] = {5, 7, 6, 8, 19, 12, 20, 16, 18};
|
||||
float data_m10_upper_bound[9] = {6, 4, 8, 18, 20, 11, 30, 16, 18};
|
||||
float data_m10_upper_bounded_ref[9] = {2, 4, 6, 8, 10, 11, 14, 16, 18};
|
||||
float data_m10_constrained_ref[9] = {5, NAN, 6, 8, 19, 11, 20, 16, 18};
|
||||
Matrix3f m10(data_m10);
|
||||
Matrix3f m10_lower_bound(data_m10_lower_bound);
|
||||
Matrix3f m10_lower_bounded_ref(data_m10_lower_bounded_ref);
|
||||
Matrix3f m10_upper_bound(data_m10_upper_bound);
|
||||
Matrix3f m10_upper_bounded_ref(data_m10_upper_bounded_ref);
|
||||
Matrix3f m10_constrained_ref(data_m10_constrained_ref);
|
||||
TEST(isEqual(max(m10, m10_lower_bound), m10_lower_bounded_ref));
|
||||
TEST(isEqual(max(m10_lower_bound, m10), m10_lower_bounded_ref));
|
||||
TEST(isEqual(min(m10, m10_upper_bound), m10_upper_bounded_ref));
|
||||
TEST(isEqual(min(m10_upper_bound, m9), m10_upper_bounded_ref));
|
||||
TEST(isEqual(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref));
|
||||
|
||||
// min, max, constrain with NAN
|
||||
TEST(isEqualF(matrix::typeFunction::min(5.f, NAN), 5.f));
|
||||
TEST(isEqualF(matrix::typeFunction::min(NAN, 5.f), 5.f));
|
||||
TEST(isEqualF(matrix::typeFunction::min(NAN, NAN), NAN));
|
||||
TEST(isEqualF(matrix::typeFunction::max(5.f, NAN), 5.f));
|
||||
TEST(isEqualF(matrix::typeFunction::max(NAN, 5.f), 5.f));
|
||||
TEST(isEqualF(matrix::typeFunction::max(NAN, NAN), NAN));
|
||||
TEST(isEqualF(matrix::typeFunction::constrain(NAN, 5.f, 6.f), NAN));
|
||||
TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, 4.f), NAN));
|
||||
TEST(isEqualF(matrix::typeFunction::constrain(6.f, NAN, 5.f), 5.f));
|
||||
TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f));
|
||||
Vector2f v1{NAN, 5.0f};
|
||||
Vector2f v1_min = min(v1, 1.f);
|
||||
Matrix3f m11 = min(m10_constrained_ref,NAN);
|
||||
TEST(isEqualF(fmin(NAN, 1.f), float(v1_min(0))));
|
||||
TEST(isEqual(m11, m10_constrained_ref));
|
||||
|
||||
// check write_string()
|
||||
float comma[6] = {
|
||||
1.f, 12345.123f,
|
||||
12345.1228f, .1234567891011f,
|
||||
12345678910.123456789f, 1234567891011.123456789101112f
|
||||
};
|
||||
Matrix<float, 3, 2> Comma(comma);
|
||||
const size_t len = 15*2*3 + 2 + 1;
|
||||
char buffer[len];
|
||||
Comma.print(); // for debugging in case of failure
|
||||
Comma.write_string(buffer, len);
|
||||
printf("%s\n", buffer); // for debugging in case of failure
|
||||
char output[] = "\t 1\t12345.123\n\t12345.123\t0.12345679\n\t1.2345679e+10\t1.234568e+12\n";
|
||||
printf("%s\n", output); // for debugging in case of failure
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
if(buffer[i] != output[i]) { // for debugging in case of failure
|
||||
printf("%d: \"%c\" != \"%c\"", int(i), buffer[i], output[i]); // LCOV_EXCL_LINE only print on failure
|
||||
}
|
||||
TEST(buffer[i] == output[i]);
|
||||
if (buffer[i] == '\0') {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// check print()
|
||||
// Redirect stdout
|
||||
TEST(freopen("testoutput.txt", "w", stdout) != NULL);
|
||||
// write
|
||||
Comma.print();
|
||||
fclose(stdout);
|
||||
// read
|
||||
FILE *fp = fopen("testoutput.txt", "r");
|
||||
TEST(fp != nullptr);
|
||||
TEST(!fseek(fp, 0, SEEK_SET));
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
char c = static_cast<char>(fgetc(fp));
|
||||
if (c == '\n') {
|
||||
break;
|
||||
}
|
||||
printf("%d %d %d\n", static_cast<int>(i), output[i], c);
|
||||
TEST(c == output[i]);
|
||||
}
|
||||
TEST(!fclose(fp));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,71 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
|
||||
Matrix3f A(data);
|
||||
|
||||
float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
|
||||
Matrix3f A_I(data_check);
|
||||
Matrix3f I;
|
||||
I.setIdentity();
|
||||
Matrix3f R = A * A_I;
|
||||
TEST(isEqual(R, I));
|
||||
|
||||
Matrix3f R2 = A;
|
||||
R2 *= A_I;
|
||||
TEST(isEqual(R2, I));
|
||||
|
||||
TEST(R2==I);
|
||||
TEST(A!=A_I);
|
||||
Matrix3f A2 = eye<float, 3>()*2;
|
||||
Matrix3f B = A2.emult(A2);
|
||||
Matrix3f B_check = eye<float, 3>()*4;
|
||||
Matrix3f C_check = eye<float, 3>()*2;
|
||||
TEST(isEqual(B, B_check));
|
||||
Matrix3f C = B_check.edivide(C_check);
|
||||
|
||||
float off_diagonal_nan[9] = {2, NAN, NAN, NAN, 2, NAN, NAN, NAN, 2};
|
||||
// off diagonal are NANs because division by 0
|
||||
TEST(C == Matrix3f(off_diagonal_nan));
|
||||
|
||||
// Test non-square matrix
|
||||
float data_43[12] = {1,3,2,
|
||||
2,2,1,
|
||||
5,2,1,
|
||||
2,3,4
|
||||
};
|
||||
float data_32[6] = {2,3,
|
||||
1,7,
|
||||
5,4
|
||||
};
|
||||
|
||||
Matrix<float, 4, 3> m43(data_43);
|
||||
Matrix<float, 3, 2> m32(data_32);
|
||||
|
||||
Matrix<float, 4, 2> m42 = m43 * m32;
|
||||
|
||||
float data_42[8] = {15,32,
|
||||
11,24,
|
||||
17,33,
|
||||
27,43
|
||||
};
|
||||
Matrix<float, 4, 2> m42_check(data_42);
|
||||
TEST(isEqual(m42, m42_check))
|
||||
|
||||
float data_42_plus2[8] = {17,34,
|
||||
13,26,
|
||||
19,35,
|
||||
29,45
|
||||
};
|
||||
Matrix<float, 4, 2> m42_plus2_check(data_42_plus2);
|
||||
Matrix<float, 4, 2> m42_plus2 = m42 - (-2);
|
||||
TEST(isEqual(m42_plus2, m42_plus2_check));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,18 @@
|
||||
#include "test_macros.hpp"
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
Matrix3f A(data);
|
||||
A = A * 2;
|
||||
float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
|
||||
Matrix3f A_check(data_check);
|
||||
TEST(isEqual(A, A_check));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,153 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/PseudoInverse.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
static const size_t n_large = 20;
|
||||
|
||||
int main()
|
||||
{
|
||||
// 3x4 Matrix test
|
||||
float data0[12] = {
|
||||
0.f, 1.f, 2.f, 3.f,
|
||||
4.f, 5.f, 6.f, 7.f,
|
||||
8.f, 9.f, 10.f, 11.f
|
||||
};
|
||||
|
||||
float data0_check[12] = {
|
||||
-0.3375f, -0.1f, 0.1375f,
|
||||
-0.13333333f, -0.03333333f, 0.06666667f,
|
||||
0.07083333f, 0.03333333f, -0.00416667f,
|
||||
0.275f, 0.1f, -0.075f
|
||||
};
|
||||
|
||||
Matrix<float, 3, 4> A0(data0);
|
||||
Matrix<float, 4, 3> A0_I;
|
||||
bool ret = geninv(A0, A0_I);
|
||||
TEST(ret);
|
||||
Matrix<float, 4, 3> A0_I_check(data0_check);
|
||||
|
||||
TEST((A0_I - A0_I_check).abs().max() < 1e-5);
|
||||
|
||||
// 4x3 Matrix test
|
||||
float data1[12] = {
|
||||
0.f, 4.f, 8.f,
|
||||
1.f, 5.f, 9.f,
|
||||
2.f, 6.f, 10.f,
|
||||
3.f, 7.f, 11.f
|
||||
};
|
||||
|
||||
float data1_check[12] = {
|
||||
-0.3375f, -0.13333333f, 0.07083333f, 0.275f,
|
||||
-0.1f, -0.03333333f, 0.03333333f, 0.1f,
|
||||
0.1375f, 0.06666667f, -0.00416667f, -0.075f
|
||||
};
|
||||
|
||||
Matrix<float, 4, 3> A1(data1);
|
||||
Matrix<float, 3, 4> A1_I;
|
||||
ret = geninv(A1, A1_I);
|
||||
TEST(ret);
|
||||
Matrix<float, 3, 4> A1_I_check(data1_check);
|
||||
|
||||
TEST((A1_I - A1_I_check).abs().max() < 1e-5);
|
||||
|
||||
// Stess test
|
||||
Matrix<float, n_large, n_large - 1> A_large;
|
||||
A_large.setIdentity();
|
||||
Matrix<float, n_large - 1, n_large> A_large_I;
|
||||
|
||||
for (size_t i = 0; i < n_large; i++) {
|
||||
ret = geninv(A_large, A_large_I);
|
||||
TEST(ret);
|
||||
TEST(isEqual(A_large, A_large_I.T()));
|
||||
}
|
||||
|
||||
// Square matrix test
|
||||
float data2[9] = {0, 2, 3,
|
||||
4, 5, 6,
|
||||
7, 8, 10
|
||||
};
|
||||
float data2_check[9] = {
|
||||
-0.4f, -0.8f, 0.6f,
|
||||
-0.4f, 4.2f, -2.4f,
|
||||
0.6f, -2.8f, 1.6f
|
||||
};
|
||||
|
||||
SquareMatrix<float, 3> A2(data2);
|
||||
SquareMatrix<float, 3> A2_I;
|
||||
ret = geninv(A2, A2_I);
|
||||
TEST(ret);
|
||||
SquareMatrix<float, 3> A2_I_check(data2_check);
|
||||
TEST((A2_I - A2_I_check).abs().max() < 1e-3);
|
||||
|
||||
// Null matrix test
|
||||
Matrix<float, 6, 16> A3;
|
||||
Matrix<float, 16, 6> A3_I;
|
||||
ret = geninv(A3, A3_I);
|
||||
TEST(ret);
|
||||
Matrix<float, 16, 6> A3_I_check;
|
||||
TEST((A3_I - A3_I_check).abs().max() < 1e-5);
|
||||
|
||||
// Mock-up effectiveness matrix
|
||||
const float B_quad_w[6][16] = {
|
||||
{-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
|
||||
{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
|
||||
};
|
||||
Matrix<float, 6, 16> B = Matrix<float, 6, 16>(B_quad_w);
|
||||
const float A_quad_w[16][6] = {
|
||||
{ -0.495383f, 0.707107f, 0.765306f, 0.0f, 0.0f, -1.000000f },
|
||||
{ 0.495383f, -0.707107f, 1.000000f, 0.0f, 0.0f, -1.000000f },
|
||||
{ 0.495383f, 0.707107f, -0.765306f, 0.0f, 0.0f, -1.000000f },
|
||||
{ -0.495383f, -0.707107f, -1.000000f, 0.0f, 0.0f, -1.000000f },
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
|
||||
};
|
||||
Matrix<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
|
||||
Matrix<float, 16, 6> A;
|
||||
ret = geninv(B, A);
|
||||
TEST(ret);
|
||||
TEST((A - A_check).abs().max() < 1e-5);
|
||||
|
||||
// Real-world test case
|
||||
const float real_alloc[5][6] = {
|
||||
{ 0.794079, 0.794079, 0.794079, 0.794079, 0.0000, 0.0000},
|
||||
{ 0.607814, 0.607814, 0.607814, 0.607814, 1.0000, 1.0000},
|
||||
{-0.672516, 0.915642, -0.915642, 0.672516, 0.0000, 0.0000},
|
||||
{ 0.159704, 0.159704, 0.159704, 0.159704, -0.2500, -0.2500},
|
||||
{ 0.607814, -0.607814, 0.607814, -0.607814, 1.0000, 1.0000}
|
||||
};
|
||||
Matrix<float, 5, 6> real ( real_alloc);
|
||||
Matrix<float, 6, 5> real_pinv;
|
||||
ret = geninv(real, real_pinv);
|
||||
TEST(ret);
|
||||
|
||||
// from SVD-based inverse
|
||||
const float real_pinv_expected_alloc[6][5] = {
|
||||
{ 2.096205, -2.722267, 2.056547, 1.503279, 3.098087},
|
||||
{ 1.612621, -1.992694, 2.056547, 1.131090, 2.275467},
|
||||
{-1.062688, 2.043479, -2.056547, -0.927950, -2.275467},
|
||||
{-1.546273, 2.773052, -2.056547, -1.300139, -3.098087},
|
||||
{-0.293930, 0.443445, 0.000000, -0.226222, 0.000000},
|
||||
{-0.293930, 0.443445, 0.000000, -0.226222, 0.000000}
|
||||
};
|
||||
Matrix<float, 6, 5> real_pinv_expected(real_pinv_expected_alloc);
|
||||
TEST((real_pinv - real_pinv_expected).abs().max() < 1e-4);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,39 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Matrix3f A;
|
||||
A.setIdentity();
|
||||
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
if (i == j) {
|
||||
TEST(fabs(A(i, j) - 1) < FLT_EPSILON);
|
||||
|
||||
} else {
|
||||
TEST(fabs(A(i, j) - 0) < FLT_EPSILON);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Matrix3f B;
|
||||
B.identity();
|
||||
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
if (i == j) {
|
||||
TEST(fabs(B(i, j) - 1) < FLT_EPSILON);
|
||||
|
||||
} else {
|
||||
TEST(fabs(B(i, j) - 0) < FLT_EPSILON);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,235 @@
|
||||
#include "test_macros.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
template class matrix::Slice<float, 2, 3, 4, 5>; // so that we get full coverage results
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
float data[9] = {0, 2, 3,
|
||||
4, 5, 6,
|
||||
7, 8, 10
|
||||
};
|
||||
SquareMatrix<float, 3> A(data);
|
||||
|
||||
// Test row slicing
|
||||
Matrix<float, 2, 3> B_rowslice(A.slice<2, 3>(1, 0));
|
||||
float data_check_rowslice[6] = {
|
||||
4, 5, 6,
|
||||
7, 8, 10
|
||||
};
|
||||
Matrix<float, 2, 3> B_check_rowslice(data_check_rowslice);
|
||||
TEST(isEqual(B_rowslice, B_check_rowslice));
|
||||
|
||||
// Test column slicing
|
||||
Matrix<float, 3, 2> B_colslice(A.slice<3, 2>(0, 1));
|
||||
float data_check_colslice[6] = {
|
||||
2, 3,
|
||||
5, 6,
|
||||
8, 10
|
||||
};
|
||||
Matrix<float, 3, 2> B_check_colslice(data_check_colslice);
|
||||
TEST(isEqual(B_colslice, B_check_colslice));
|
||||
|
||||
// Test slicing both
|
||||
Matrix<float, 2, 2> B_bothslice(A.slice<2, 2>(1, 1));
|
||||
float data_check_bothslice[4] = {
|
||||
5, 6,
|
||||
8, 10
|
||||
};
|
||||
Matrix<float, 2, 2> B_check_bothslice(data_check_bothslice);
|
||||
TEST(isEqual(B_bothslice, B_check_bothslice));
|
||||
|
||||
//Test block writing
|
||||
float data_2[4] = {
|
||||
11, 12,
|
||||
13, 14
|
||||
};
|
||||
|
||||
Matrix<float, 2, 2> C(data_2);
|
||||
A.slice<2, 2>(1, 1) = C;
|
||||
|
||||
float data_2_check[9] = {
|
||||
0, 2, 3,
|
||||
4, 11, 12,
|
||||
7, 13, 14
|
||||
};
|
||||
Matrix<float, 3, 3> D(data_2_check);
|
||||
TEST(isEqual(A, D));
|
||||
|
||||
//Test writing to slices
|
||||
Matrix<float, 3, 1> E;
|
||||
E(0,0) = -1;
|
||||
E(1,0) = 1;
|
||||
E(2,0) = 3;
|
||||
|
||||
Matrix<float, 2, 1> F;
|
||||
F(0,0) = 9;
|
||||
F(1,0) = 11;
|
||||
|
||||
E.slice<2,1>(0,0) = F;
|
||||
|
||||
float data_3_check[3] = {9, 11, 3};
|
||||
Matrix<float, 3, 1> G (data_3_check);
|
||||
TEST(isEqual(E, G));
|
||||
TEST(isEqual(E, Matrix<float,3,1>(E.slice<3,1>(0,0))));
|
||||
|
||||
Matrix<float, 2, 1> H = E.slice<2,1>(0,0);
|
||||
TEST(isEqual(H,F));
|
||||
|
||||
float data_4_check[5] = {3, 11, 9, 0, 0};
|
||||
{ // assigning row slices to each other
|
||||
const Matrix<float, 3, 1> J (data_3_check);
|
||||
Matrix<float, 5, 1> K;
|
||||
K.row(2) = J.row(0);
|
||||
K.row(1) = J.row(1);
|
||||
K.row(0) = J.row(2);
|
||||
|
||||
Matrix<float, 5, 1> K_check(data_4_check);
|
||||
TEST(isEqual(K, K_check));
|
||||
}
|
||||
{ // assigning col slices to each other
|
||||
const Matrix<float, 1, 3> J (data_3_check);
|
||||
Matrix<float, 1, 5> K;
|
||||
K.col(2) = J.col(0);
|
||||
K.col(1) = J.col(1);
|
||||
K.col(0) = J.col(2);
|
||||
|
||||
Matrix<float, 1, 5> K_check(data_4_check);
|
||||
TEST(isEqual(K, K_check));
|
||||
}
|
||||
|
||||
// check that slice of a slice works for reading
|
||||
const Matrix<float, 3, 3> cm33(data);
|
||||
Matrix<float, 2, 1> topRight = cm33.slice<2,3>(0,0).slice<2,1>(0,2);
|
||||
float top_right_check[2] = {3,6};
|
||||
TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
|
||||
|
||||
// check that slice of a slice works for writing
|
||||
Matrix<float, 3, 3> m33(data);
|
||||
m33.slice<2,3>(0,0).slice<2,1>(0,2) = Matrix<float, 2, 1>();
|
||||
const float data_check[9] = {0, 2, 0,
|
||||
4, 5, 0,
|
||||
7, 8, 10
|
||||
};
|
||||
TEST(isEqual(m33, Matrix<float, 3, 3>(data_check)));
|
||||
|
||||
// longerThan
|
||||
Vector3f v5;
|
||||
v5(0) = 3;
|
||||
v5(1) = 4;
|
||||
v5(2) = 9;
|
||||
TEST(v5.xy().longerThan(4.99f));
|
||||
TEST(!v5.xy().longerThan(5.f));
|
||||
TEST(isEqualF(5.f, v5.xy().norm()));
|
||||
|
||||
// min/max
|
||||
TEST(m33.row(1).max() == 5);
|
||||
TEST(m33.col(0).min() == 0);
|
||||
TEST((m33.slice<2,2>(1,1).max()) == 10);
|
||||
|
||||
// assign scalar value to slice
|
||||
Matrix<float, 3, 1> L;
|
||||
L(0,0) = -1;
|
||||
L(1,0) = 1;
|
||||
L(2,0) = 3;
|
||||
|
||||
L.slice<2,1>(0,0) = 0.0f;
|
||||
|
||||
float data_5_check[3] = {0, 0, 3};
|
||||
Matrix<float, 3, 1> M (data_5_check);
|
||||
TEST(isEqual(L, M));
|
||||
|
||||
// return diagonal elements
|
||||
float data_6[9] = {0, 2, 3,
|
||||
4, 5, 6,
|
||||
7, 8, 10
|
||||
};
|
||||
SquareMatrix<float, 3> N(data_6);
|
||||
|
||||
Vector3f v6 = N.slice<3,3>(0,0).diag();
|
||||
Vector3f v6_check = {0, 5, 10};
|
||||
TEST(isEqual(v6,v6_check));
|
||||
Vector2f v7 = N.slice<2,3>(1,0).diag();
|
||||
Vector2f v7_check = {4, 8};
|
||||
TEST(isEqual(v7,v7_check));
|
||||
Vector2f v8 = N.slice<3,2>(0,1).diag();
|
||||
Vector2f v8_check = {2, 6};
|
||||
TEST(isEqual(v8,v8_check));
|
||||
Vector2f v9(N.slice<1,2>(1,1));
|
||||
Vector2f v9_check = {5, 6};
|
||||
TEST(isEqual(v9,v9_check));
|
||||
Vector3f v10(N.slice<1,3>(1,0));
|
||||
Vector3f v10_check = {4, 5, 6};
|
||||
TEST(isEqual(v10,v10_check));
|
||||
|
||||
// Different assignment operators
|
||||
SquareMatrix3f O(data);
|
||||
float operand_data [4] = {2, 1, -3, -1};
|
||||
const SquareMatrix<float, 2> operand(operand_data);
|
||||
|
||||
O.slice<2,2>(1,0) += operand;
|
||||
float O_check_data_1 [9] = {0, 2, 3, 6, 6, 6, 4, 7, 10};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_1)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.slice<2,1>(1,1) += operand.slice<2,1>(0,0);
|
||||
float O_check_data_2 [9] = {0, 2, 3, 4, 7, 6, 7, 5, 10};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_2)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.slice<3,3>(0,0) += -1;
|
||||
float O_check_data_3 [9] = {-1, 1, 2, 3, 4, 5, 6, 7, 9};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_3)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.col(1) += Vector3f{1, -2, 3};
|
||||
float O_check_data_4 [9] = {0, 3, 3, 4, 3, 6, 7, 11, 10};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_4)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.slice<2,2>(1,0) -= operand;
|
||||
float O_check_data_5 [9] = {0, 2, 3, 2, 4, 6, 10, 9, 10};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_5)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.slice<2,1>(1,1) -= operand.slice<2,1>(0,0);
|
||||
float O_check_data_6 [9] = {0, 2, 3, 4, 3, 6, 7, 11, 10};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_6)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.slice<3,3>(0,0) -= -1;
|
||||
float O_check_data_7 [9] = {1, 3, 4, 5, 6, 7, 8, 9, 11};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_7)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.col(1) -= Vector3f{1, -2, 3};
|
||||
float O_check_data_8 [9] = {0, 1, 3, 4, 7, 6, 7, 5, 10};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_8)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.slice<2,1>(1,1) *= 5.f;
|
||||
float O_check_data_9 [9] = {0, 2, 3, 4, 25, 6, 7, 40, 10};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_9)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
O.slice<2,1>(1,1) /= 2.f;
|
||||
float O_check_data_10 [9] = {0, 2, 3, 4, 2.5, 6, 7, 4, 10};
|
||||
TEST(isEqual(O, SquareMatrix3f(O_check_data_10)));
|
||||
|
||||
// Different operations
|
||||
O = SquareMatrix3f(data);
|
||||
SquareMatrix<float, 2> res_11(O.slice<2,2>(1,1) * 2.f);
|
||||
float O_check_data_11 [4] = {10, 12, 16, 20};
|
||||
TEST(isEqual(res_11, SquareMatrix<float, 2>(O_check_data_11)));
|
||||
|
||||
O = SquareMatrix3f(data);
|
||||
SquareMatrix<float, 2> res_12(O.slice<2,2>(1,1) / 2.f);
|
||||
float O_check_data_12 [4] = {2.5, 3, 4, 5};
|
||||
TEST(isEqual(res_12, SquareMatrix<float, 2>(O_check_data_12)));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,119 @@
|
||||
#include <matrix/math.hpp>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(sparseVectorTest, defaultConstruction) {
|
||||
SparseVectorf<24, 4, 6> a;
|
||||
EXPECT_EQ(a.non_zeros(), 2);
|
||||
EXPECT_EQ(a.index(0), 4);
|
||||
EXPECT_EQ(a.index(1), 6);
|
||||
a.at<4>() = 1.f;
|
||||
a.at<6>() = 2.f;
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, initializationWithData) {
|
||||
const float data[3] = {1.f, 2.f, 3.f};
|
||||
SparseVectorf<24, 4, 6, 22> a(data);
|
||||
EXPECT_EQ(a.non_zeros(), 3);
|
||||
EXPECT_EQ(a.index(0), 4);
|
||||
EXPECT_EQ(a.index(1), 6);
|
||||
EXPECT_EQ(a.index(2), 22);
|
||||
EXPECT_FLOAT_EQ(a.at<4>(), data[0]);
|
||||
EXPECT_FLOAT_EQ(a.at<6>(), data[1]);
|
||||
EXPECT_FLOAT_EQ(a.at<22>(), data[2]);
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, initialisationFromVector) {
|
||||
const Vector3f vec(1.f, 2.f, 3.f);
|
||||
const SparseVectorf<3, 0, 2> a(vec);
|
||||
EXPECT_FLOAT_EQ(a.at<0>(), vec(0));
|
||||
EXPECT_FLOAT_EQ(a.at<2>(), vec(2));
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, accessDataWithCompressedIndices) {
|
||||
const Vector3f vec(1.f, 2.f, 3.f);
|
||||
SparseVectorf<3, 0, 2> a(vec);
|
||||
for (size_t i = 0; i < a.non_zeros(); i++) {
|
||||
a.atCompressedIndex(i) = static_cast<float>(i);
|
||||
}
|
||||
EXPECT_FLOAT_EQ(a.at<0>(), a.atCompressedIndex(0));
|
||||
EXPECT_FLOAT_EQ(a.at<2>(), a.atCompressedIndex(1));
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, setZero) {
|
||||
const float data[3] = {1.f, 2.f, 3.f};
|
||||
SparseVectorf<24, 4, 6, 22> a(data);
|
||||
a.setZero();
|
||||
EXPECT_FLOAT_EQ(a.at<4>(), 0.f);
|
||||
EXPECT_FLOAT_EQ(a.at<6>(), 0.f);
|
||||
EXPECT_FLOAT_EQ(a.at<22>(), 0.f);
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, additionWithDenseVector) {
|
||||
Vector<float, 4> dense_vec;
|
||||
dense_vec.setAll(1.f);
|
||||
const float data[3] = {1.f, 2.f, 3.f};
|
||||
const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
|
||||
const Vector<float, 4> res = sparse_vec + dense_vec;
|
||||
EXPECT_FLOAT_EQ(res(0), 1.f);
|
||||
EXPECT_FLOAT_EQ(res(1), 2.f);
|
||||
EXPECT_FLOAT_EQ(res(2), 3.f);
|
||||
EXPECT_FLOAT_EQ(res(3), 4.f);
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, addScalar) {
|
||||
const float data[3] = {1.f, 2.f, 3.f};
|
||||
SparseVectorf<4, 1, 2, 3> sparse_vec(data);
|
||||
sparse_vec += 2.f;
|
||||
EXPECT_FLOAT_EQ(sparse_vec.at<1>(), 3.f);
|
||||
EXPECT_FLOAT_EQ(sparse_vec.at<2>(), 4.f);
|
||||
EXPECT_FLOAT_EQ(sparse_vec.at<3>(), 5.f);
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, dotProductWithDenseVector) {
|
||||
Vector<float, 4> dense_vec;
|
||||
dense_vec.setAll(3.f);
|
||||
const float data[3] = {1.f, 2.f, 3.f};
|
||||
const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
|
||||
float res = sparse_vec.dot(dense_vec);
|
||||
EXPECT_FLOAT_EQ(res, 18.f);
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, multiplicationWithDenseMatrix) {
|
||||
Matrix<float, 2, 3> dense_matrix;
|
||||
dense_matrix.setAll(2.f);
|
||||
dense_matrix(1, 1) = 3.f;
|
||||
const Vector3f dense_vec(0.f, 1.f, 5.f);
|
||||
const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
|
||||
const Vector<float, 2> res_sparse = dense_matrix * sparse_vec;
|
||||
const Vector<float, 2> res_dense = dense_matrix * dense_vec;
|
||||
EXPECT_TRUE(isEqual(res_dense, res_sparse));
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, quadraticForm) {
|
||||
float matrix_data[9] = {1, 2, 3,
|
||||
2, 4, 5,
|
||||
3, 5, 6
|
||||
};
|
||||
const SquareMatrix<float, 3> dense_matrix(matrix_data);
|
||||
const Vector3f dense_vec(0.f, 1.f, 5.f);
|
||||
const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
|
||||
EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f);
|
||||
}
|
||||
|
||||
TEST(sparseVectorTest, norms) {
|
||||
const float data[2] = {3.f, 4.f};
|
||||
const SparseVectorf<4, 1, 3> sparse_vec(data);
|
||||
EXPECT_FLOAT_EQ(sparse_vec.norm_squared(), 25.f);
|
||||
EXPECT_FLOAT_EQ(sparse_vec.norm(), 5.f);
|
||||
EXPECT_TRUE(sparse_vec.longerThan(4.5f));
|
||||
EXPECT_FALSE(sparse_vec.longerThan(5.5f));
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
std::cout << "Run SparseVector tests" << std::endl;
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user