SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add TagRelativeTarget3D class

Browse Source
This commit is contained in:
lgv 2026-03-05 16:15:13 +08:00
parent 0fc5e271e8
commit 29e5c8833c
221 changed files with 34874 additions and 831 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

262
README.md
View File

@ -4,7 +4,7 @@
## Installation ## Installation
### 1. Git submodules install ### 1. Git submodules install
``` ```
git submodule update --init --recursive git submodule update --init --recursive
@ -15,14 +15,14 @@ git submodule update --init --recursive
```shell ```shell
# basic # basic
sudo apt-get update sudo apt-get update
sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev
# opencv # opencv
sudo apt install -y \ sudo apt install -y \
libjpeg-dev libpng-dev libtiff-dev \ libjpeg-dev libpng-dev libtiff-dev \
libavcodec-dev libavformat-dev libswscale-dev \ libavcodec-dev libavformat-dev libswscale-dev \
libv4l-dev libxvidcore-dev libx264-dev \ libv4l-dev libxvidcore-dev libx264-dev \
libgtk-3-dev libgtk-3-dev
# PulseAudio # PulseAudio
sudo apt-get install libpulse-dev sudo apt-get install libpulse-dev
@ -37,252 +37,10 @@ sudo apt-get install libglfw3-dev
sudo apt-get install libassimp-dev sudo apt-get install libassimp-dev
# visp # visp
sudo apt-get install -y libx11-dev liblapack-dev libv4l-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev sudo apt-get install -y libx11-dev liblapack-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev
[//]: # () # realsense
[//]: # (# json) sudo apt-get install -y \
libusb-1.0-0-dev libudev-dev \
[//]: # (sudo apt install libjsoncpp-dev -y) libglu1-mesa-dev
```
[//]: # ()
[//]: # ()
[//]: # ()
[//]: # ()
[//]: # ()
[//]: # ()
[//]: # ()
[//]: # ()
[//]: # (# flann)
[//]: # (sudo apt-get install libflann-dev -y)
[//]: # ()
[//]: # ()
[//]: # ()
[//]: # (#alas)
[//]: # (sudo apt-get install libasound2-dev -y)
[//]: # ()
[//]: # (# gstreamer)
[//]: # (sudo apt install gstreamer1.0-plugins-good gstreamer1.0-plugins-bad -y)
[//]: # (sudo apt install gstreamer1.0-plugins-ugly gstreamer1.0-libav -y)
[//]: # ()
[//]: # (# tinyxml2)
[//]: # (sudo apt install libtinyxml2-dev -y)
[//]: # ()
[//]: # ()
[//]: # (```)
[//]: # ()
[//]: # (### 3. gRPC install (build from source))
[//]: # (version: https://grpc.io/docs/languages/cpp/quickstart/（grpc官网地址 v1.73.0）)
[//]: # (#### 安装完成之后要配置环境变量)
[//]: # (vim ~/.bashrc)
[//]: # (在末尾添加)
[//]: # ()
[//]: # (### 4. ffmpeg install (build from source))
[//]: # (#### 4.1 install dependencies)
[//]: # (```shell)
[//]: # (sudo apt-get update)
[//]: # (sudo apt install build-essential yasm nasm git -y)
[//]: # (sudo apt install libx264-dev libx265-dev libvpx-dev libfdk-aac-dev libmp3lame-dev libopus-dev -y)
[//]: # (```)
[//]: # ()
[//]: # (#### 4.2 build from source)
[//]: # (```shell)
[//]: # (cd assets)
[//]: # (tar xjvf ffmpeg-4.4.tar.bz2)
[//]: # (cd ffmpeg-4.4)
[//]: # ()
[//]: # (# set configurations)
[//]: # (./configure \)
[//]: # ( --extra-libs="-lpthread -lm" \)
[//]: # ( --enable-gpl \)
[//]: # ( --enable-libass \)
[//]: # ( --enable-libfdk-aac \)
[//]: # ( --enable-libfreetype \)
[//]: # ( --enable-libmp3lame \)
[//]: # ( --enable-libopus \)
[//]: # ( --enable-libvorbis \)
[//]: # ( --enable-libvpx \)
[//]: # ( --enable-libx264 \)
[//]: # ( --enable-libx265 \)
[//]: # ( --enable-pic \)
[//]: # ( --enable-shared \)
[//]: # ( --enable-nonfree \)
[//]: # ()
[//]: # (# build and install)
[//]: # (make -j4)
[//]: # (sudo make install)
[//]: # (```)
[//]: # ()
[//]: # ()
[//]: # (### 5. librealsense2 2.55.1 install)
[//]: # (```shell)
[//]: # (sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE)
[//]: # (sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u)
[//]: # (sudo apt-get install librealsense2-dkms=1.3.24-0ubuntu1 -y)
[//]: # (sudo apt-get install \)
[//]: # (librealsense2=2.55.1-0~realsense.12473 \)
[//]: # (librealsense2-utils=2.55.1-0~realsense.12473 \)
[//]: # (librealsense2-dev=2.55.1-0~realsense.12473 \)
[//]: # (librealsense2-dbg=2.55.1-0~realsense.12473 -y)
[//]: # (```)
[//]: # ()
[//]: # (### 5. OSQPEigen install(build from source))
[//]: # (#### 5.1 install osqp (v0.6.3))
[//]: # (```shell)
[//]: # (git clone --branch v0.6.3 https://github.com/osqp/osqp.git)
[//]: # (cd osqp)
[//]: # (git submodule init)
[//]: # (git submodule update)
[//]: # (mkdir build)
[//]: # (cd build)
[//]: # (cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..)
[//]: # (make -j)
[//]: # (sudo make install)
[//]: # (```)
[//]: # (#### 5.2 install OsqpEigen (v0.10.1))
[//]: # (```shell)
[//]: # (git clone --branch v0.10.1 https://github.com/robotology/osqp-eigen.git)
[//]: # (cd osqp-eigen)
[//]: # (git submodule init)
[//]: # (git submodule update)
[//]: # (mkdir build)
[//]: # (cd build)
[//]: # (cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..)
[//]: # (make -j)
[//]: # (sudo make install)
[//]: # (```)
[//]: # ()
[//]: # (#### 6 mech-eye-sdk)
[//]: # (```shell)
[//]: # (cd assets)
[//]: # (sudo dkpg -i Mech-Eye_API_2.5.1_amd64.deb)
[//]: # (```)
[//]: # ()
[//]: # (### 串口冲突问题)
[//]: # (```shell)
[//]: # (sudo systemctl stop brltty)
[//]: # (sudo systemctl disable brltty)
[//]: # (sudo apt remove brltty)
[//]: # (```)
[//]: # ()
[//]: # (### ~./bashrc)
[//]: # (```shell)
[//]: # (export ARCH="x86")
[//]: # (export CMVR_ES_PROJECT_PATH="/home/cmvr/Projects/cmvr-es")
[//]: # (# >>> cmake )
[//]: # (export PATH="$CMVR_ES_PROJECT_PATH/dependency/$ARCH/third_party/cmake/v3.30.3/cmake-3.30.3-linux-x86_64/bin:$PATH")
[//]: # (# <<< cmake)
[//]: # ()
[//]: # (# >>> ffmpeg)
[//]: # (export LD_LIBRARY_PATH="$CMVR_ES_PROJECT_PATH/dependency/$ARCH/third_party/x264/v165/lib:$LD_LIBRARY_PATH")
[//]: # (export LD_LIBRARY_PATH="$CMVR_ES_PROJECT_PATH/dependency/$ARCH/third_party/x265/v215/lib:$LD_LIBRARY_PATH")
[//]: # (export LD_LIBRARY_PATH="$CMVR_ES_PROJECT_PATH/dependency/$ARCH/third_party/ffmpeg/v4.2/lib:$LD_LIBRARY_PATH")
[//]: # (# <<< ffmpeg)
[//]: # (```)
[//]: # ()

311
cmvr-es/common/utils/image/image_process.h
View File

@ -6,55 +6,284 @@
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
#include <cstdint>
#include <limits>
#include <vector> #include <vector>
#include <Eigen/Core>
#include "devices/camera/abstract_camera.h"
#include <opencv2/core.hpp> #include <opencv2/core.hpp>
namespace cmvr { namespace cmvr {
// 在像素 (u,v) 的邻域内采样深度并取中值，输出单位米。 /**
// 支持: * @brief 图像几何处理静态工具类。
// - CV_32FC1: 米 *
// - CV_16UC1: 毫米（内部转换为米） * 提供像素射线反投影、3D 点投影和深度稳健采样等通用能力。
inline bool sampleDepthMeters(const cv::Mat& depth, */
int u, class ImageProcess final {
int v, public:
double& z_m, /**
int kernel_half = 1, * @brief 将像素坐标反投影为相机坐标系下单位射线（无畸变 pinhole）。
double min_valid_m = 1e-4) { *
if (depth.empty() || depth.channels() != 1) return false; * @param intrinsics 相机内参。
if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) return false; * @param u 像素横坐标（列）。
if (kernel_half < 0) return false; * @param v 像素纵坐标（行）。
* @param ray_c 输出单位射线方向（相机坐标系）。
std::vector<double> valid; * @return 成功返回 `true`，失败返回 `false`。
valid.reserve(static_cast<size_t>((2 * kernel_half + 1) * (2 * kernel_half + 1))); */
static inline bool pixelToRayCamera(const device::Rs2Intrinsics& intrinsics,
for (int dv = -kernel_half; dv <= kernel_half; ++dv) { int u,
for (int du = -kernel_half; du <= kernel_half; ++du) { int v,
const int uu = u + du; Eigen::Vector3d& ray_c) {
const int vv = v + dv; const double fx = static_cast<double>(intrinsics.fx);
if (uu < 0 || vv < 0 || uu >= depth.cols || vv >= depth.rows) continue; const double fy = static_cast<double>(intrinsics.fy);
const double cx = static_cast<double>(intrinsics.cx);
double z = 0.0; const double cy = static_cast<double>(intrinsics.cy);
if (depth.type() == CV_32FC1) { if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 ||
z = static_cast<double>(depth.at<float>(vv, uu)); !std::isfinite(cx) || !std::isfinite(cy)) {
} else if (depth.type() == CV_16UC1) { return false;
z = static_cast<double>(depth.at<uint16_t>(vv, uu)) * 1e-3;
} else {
return false;
}
if (std::isfinite(z) && z > min_valid_m) {
valid.push_back(z);
}
} }
ray_c.z() = 1.0;
ray_c.x() = (static_cast<double>(u) - cx) / fx;
ray_c.y() = (static_cast<double>(v) - cy) / fy;
const double n = ray_c.norm();
if (!std::isfinite(n) || n <= 1e-12) {
return false;
}
ray_c /= n;
return true;
} }
if (valid.empty()) return false; /**
std::nth_element(valid.begin(), valid.begin() + valid.size() / 2, valid.end()); * @brief 将相机坐标系下 3D 点投影到像素坐标（无畸变 pinhole）。
z_m = valid[valid.size() / 2]; *
return true; * @param intrinsics 相机内参。
} * @param p_c 输入 3D 点（相机坐标系，单位米）。
* @param uv 输出像素坐标。
* @return 成功返回 `true`，失败返回 `false`。
*/
static inline bool projectCameraPointToPixel(const device::Rs2Intrinsics& intrinsics,
const Eigen::Vector3d& p_c,
Eigen::Vector2d& uv) {
if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z()) ||
p_c.z() <= 1e-9) {
return false;
}
const double fx = static_cast<double>(intrinsics.fx);
const double fy = static_cast<double>(intrinsics.fy);
const double cx = static_cast<double>(intrinsics.cx);
const double cy = static_cast<double>(intrinsics.cy);
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 ||
!std::isfinite(cx) || !std::isfinite(cy)) {
return false;
}
uv.x() = fx * (p_c.x() / p_c.z()) + cx;
uv.y() = fy * (p_c.y() / p_c.z()) + cy;
return std::isfinite(uv.x()) && std::isfinite(uv.y());
}
/**
* @brief 将像素坐标和深度值反投影为相机坐标系下 3D 点。
*
* @param intrinsics 相机内参。
* @param u 像素横坐标（列）。
* @param v 像素纵坐标（行）。
* @param z_m 深度值（米）。
* @param p_c 输出 3D 点（相机坐标系，单位米）。
* @return 成功返回 `true`，失败返回 `false`。
*/
static inline bool pixelToCameraPoint(const device::Rs2Intrinsics& intrinsics,
int u,
int v,
double z_m,
Eigen::Vector3d& p_c) {
const double fx = static_cast<double>(intrinsics.fx);
const double fy = static_cast<double>(intrinsics.fy);
const double cx = static_cast<double>(intrinsics.cx);
const double cy = static_cast<double>(intrinsics.cy);
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0 ||
!std::isfinite(cx) || !std::isfinite(cy)) {
return false;
}
if (!std::isfinite(z_m) || z_m <= 1e-9) {
return false;
}
p_c.z() = z_m;
p_c.x() = (static_cast<double>(u) - cx) / fx * z_m;
p_c.y() = (static_cast<double>(v) - cy) / fy * z_m;
return std::isfinite(p_c.x()) && std::isfinite(p_c.y()) && std::isfinite(p_c.z());
}
/**
* @brief 在像素邻域采样深度并取中值，支持 MAD 离群剔除，输出单位米。
*
* 支持深度图类型：
* - `CV_16UC1`：毫米（内部转换为米）
* - `CV_32FC1`：米（若值明显为毫米尺度会自动转米）
* - `CV_64FC1`：米（若值明显为毫米尺度会自动转米）
*
* @param depth 深度图。
* @param u 像素横坐标（列）。
* @param v 像素纵坐标（行）。
* @param z_m 输出深度值（米）。
* @param window_size_px 采样窗口边长（像素，内部会修正为正奇数）。
* @param enable_outlier_reject 是否启用离群值剔除。
* @param outlier_sigma MAD 门限倍数。
* @param outlier_min_dev_m 最小离群门限（米）。
* @param min_valid_m 最小有效深度（米）。
* @return 成功返回 `true`，失败返回 `false`。
*/
static inline bool sampleDepthMeters(const cv::Mat& depth,
int u,
int v,
double& z_m,
int window_size_px = 3,
bool enable_outlier_reject = true,
double outlier_sigma = 2.5,
double outlier_min_dev_m = 0.002,
double min_valid_m = 1e-4) {
if (depth.empty() || depth.channels() != 1) {
return false;
}
if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) {
return false;
}
const int type = depth.type();
if (type != CV_16UC1 && type != CV_32FC1 && type != CV_64FC1) {
return false;
}
auto medianOf = [](std::vector<double> values, double& m) -> bool {
if (values.empty()) {
return false;
}
const size_t k = values.size() / 2;
std::nth_element(values.begin(), values.begin() + k, values.end());
m = values[k];
return std::isfinite(m);
};
int w = std::max(1, window_size_px);
if ((w % 2) == 0) {
++w;
}
const int radius = w / 2;
std::vector<double> valid_depths;
valid_depths.reserve(static_cast<size_t>((2 * radius + 1) * (2 * radius + 1)));
for (int yy = std::max(0, v - radius); yy <= std::min(depth.rows - 1, v + radius); ++yy) {
for (int xx = std::max(0, u - radius); xx <= std::min(depth.cols - 1, u + radius); ++xx) {
double z = std::numeric_limits<double>::quiet_NaN();
if (type == CV_16UC1) {
const uint16_t raw = depth.at<uint16_t>(yy, xx);
if (raw == 0) {
continue;
}
z = static_cast<double>(raw) * 1e-3; // mm -> m
} else if (type == CV_32FC1) {
z = static_cast<double>(depth.at<float>(yy, xx));
} else { // CV_64FC1
z = depth.at<double>(yy, xx);
}
if (std::isfinite(z) && z > 100.0) {
z *= 1e-3; // 兼容浮点毫米
}
if (!std::isfinite(z) || z <= min_valid_m || z >= 50.0) {
continue;
}
valid_depths.push_back(z);
}
}
if (valid_depths.empty()) {
return false;
}
double med = 0.0;
if (!medianOf(valid_depths, med)) {
return false;
}
if (enable_outlier_reject && std::isfinite(outlier_sigma) && outlier_sigma > 0.0) {
std::vector<double> abs_dev;
abs_dev.reserve(valid_depths.size());
for (const double z : valid_depths) {
abs_dev.push_back(std::abs(z - med));
}
double mad = 0.0;
if (medianOf(abs_dev, mad)) {
const double robust_sigma = 1.4826 * mad;
const double gate = std::max(std::max(0.0, outlier_min_dev_m), outlier_sigma * robust_sigma);
std::vector<double> inliers;
inliers.reserve(valid_depths.size());
for (const double z : valid_depths) {
if (std::abs(z - med) <= gate) {
inliers.push_back(z);
}
}
if (!inliers.empty() && medianOf(inliers, med)) {
// use filtered median
}
}
}
z_m = med;
return std::isfinite(z_m) && z_m > 0.0;
}
/**
* @brief 从深度图直接将像素反投影为相机坐标系下 3D 点。
*
* 该函数先调用 `sampleDepthMeters(...)` 估计像素深度，再调用
* `pixelToCameraPoint(...)` 完成反投影。
*
* @param intrinsics 相机内参。
* @param depth 深度图。
* @param u 像素横坐标（列）。
* @param v 像素纵坐标（行）。
* @param p_c 输出 3D 点（相机坐标系，单位米）。
* @param window_size_px 采样窗口边长（像素，内部会修正为正奇数）。
* @param enable_outlier_reject 是否启用离群值剔除。
* @param outlier_sigma MAD 门限倍数。
* @param outlier_min_dev_m 最小离群门限（米）。
* @param min_valid_m 最小有效深度（米）。
* @return 成功返回 `true`，失败返回 `false`。
*/
static inline bool pixelToCameraPointWithDepth(const device::Rs2Intrinsics& intrinsics,
const cv::Mat& depth,
int u,
int v,
Eigen::Vector3d& p_c,
int window_size_px = 3,
bool enable_outlier_reject = true,
double outlier_sigma = 2.5,
double outlier_min_dev_m = 0.002,
double min_valid_m = 1e-4) {
double z_m = 0.0;
if (!sampleDepthMeters(depth,
u,
v,
z_m,
window_size_px,
enable_outlier_reject,
outlier_sigma,
outlier_min_dev_m,
min_valid_m)) {
return false;
}
return pixelToCameraPoint(intrinsics, u, v, z_m, p_c);
}
private:
/** @brief 禁止实例化。 */
ImageProcess() = delete;
};
} // namespace cmvr } // namespace cmvr

1
cmvr-es/controller/include/ibvs_controller.h
View File

@ -303,6 +303,7 @@ private:
Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()}; Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()};
// 坐标对齐 // 坐标对齐
// R = R_camera_urdf_ * （R_cv_转置） == ViSP相机坐标系 到 URDF 相机系 的等效旋转
// AbstractCamera -> ViSP 的旋转矩阵。 // AbstractCamera -> ViSP 的旋转矩阵。
Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()}; Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
// AbstractCamera -> URDF 相机系的旋转矩阵。 // AbstractCamera -> URDF 相机系的旋转矩阵。

2
cmvr-es/controller/src/ibvs_controller.cpp
View File

@ -248,7 +248,7 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
const int u = static_cast<int>(std::lround(fx * x_depth_ctrl + cx)); const int u = static_cast<int>(std::lround(fx * x_depth_ctrl + cx));
const int v = static_cast<int>(std::lround(fy * y_depth_ctrl + cy)); const int v = static_cast<int>(std::lround(fy * y_depth_ctrl + cy));
double z_from_depth = 0.0; double z_from_depth = 0.0;
if (sampleDepthMeters(depth, u, v, z_from_depth)) { if (ImageProcess::sampleDepthMeters(depth, u, v, z_from_depth)) {
z_depth_ctrl = std::max(z_from_depth, 0.05); z_depth_ctrl = std::max(z_from_depth, 0.05);
depth_ctrl_used = true; depth_ctrl_used = true;
} else if (depth_mode_ == DepthMode::DEPTH_ONLY) { } else if (depth_mode_ == DepthMode::DEPTH_ONLY) {

28
cmvr-es/perception/CMakeLists.txt
View File

@ -31,17 +31,17 @@ target_link_libraries(perception PUBLIC
add_library(cmvr_es::perception ALIAS perception) add_library(cmvr_es::perception ALIAS perception)
install(TARGETS perception LIBRARY DESTINATION lib) install(TARGETS perception LIBRARY DESTINATION lib)
#add_executable(tag_relative_target_3d_test add_executable(tag_relative_target_3d_test
# src/tag_relative_target_3d_test.cpp src/tag_relative_target_3d_test.cpp
#) )
#
#target_link_libraries(tag_relative_target_3d_test target_link_libraries(tag_relative_target_3d_test
# PRIVATE PRIVATE
# cmvr_es::perception cmvr_es::perception
# cmvr_es::device::realsense_camera cmvr_es::device::realsense_camera
# cmvr_es::proto cmvr_es::proto
# glog glog
# gtest gtest
# gtest_main gtest_main
# pthread pthread
#) )

472
cmvr-es/perception/include/tag_relative_target_3d.h
View File

@ -1,215 +1,457 @@
//
// Created by lgv on 2026/2/26.
//
#pragma once #pragma once
#ifndef CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H
#define CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H
#include <memory> #include <memory>
#include <unordered_map> #include <unordered_map>
#include <vector> #include <vector>
#include <Eigen/Dense> #include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <visp3/detection/vpDetectorAprilTag.h> #include <visp3/detection/vpDetectorAprilTag.h>
#include "devices/camera/abstract_camera.h" #include "devices/camera/abstract_camera.h"
namespace cmvr::perception { namespace cmvr::perception {
/** /**
* @brief 单个 tag 观测：tag 相对于相机的位姿（`T_c_t`）。 * @brief Tag 相对目标 3D 跟踪器（全封装版）。
*/
struct TagPoseObservation {
// tag id。
int tag_id{-1};
// tag 坐标系 -> 相机坐标系 变换。
Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
// 观测权重（<=0 或非法时按 1.0 处理）。
double weight{1.0};
};
/**
* @brief 维护“目标点在各 tag 坐标系下的 3D 相对坐标”，并支持在线恢复目标点相机坐标。
* *
* 典型流程： * 该类内部完成：
* 1) 锁定目标时已知目标点相机坐标 `p_c_target` 与可见 tag 位姿观测； * 1) 从相机抓取 RGB 或 RGBD；
* 2) 计算并缓存每个可见 tag 下的 `p_ti_target`； * 2) AprilTag 检测与位姿估计；
* 3) 运行时只要某个已缓存的 tag 可见，就可恢复目标点在当前相机系下位置。 * 3) 目标点像素 `(u, v)` 到相机 3D 点的求解；
* 4) 目标点在 tag 坐标系下的锚点缓存与基于 active tag 的跟踪。
*
* 对外仅暴露 `(u, v)` 输入，结果通过 `last*` getter 读取。
*/ */
class TagRelativeTarget3D { class TagRelativeTarget3D {
public: public:
/** /**
* @brief 最近一次调用状态。 * @brief 最近一次调用的状态码。
*/ */
enum class Status { enum class Status {
OK = 0, // 计算成功 OK = 0, ///< 调用成功。
INVALID_INPUT, // 输入参数非法 INVALID_INPUT, ///< 输入参数非法。
NO_CAMERA, // 未注入相机 NO_CAMERA, ///< 相机对象为空。
NO_NEW_FRAME, // 相机未取到新图像 NO_NEW_FRAME, ///< 未获取到新图像帧。
BAD_IMAGE, // 图像或内参非法 BAD_IMAGE, ///< 图像或内参无效。
NO_TAG, // 未检测到 tag NO_TAG, ///< 未检测到 tag。
NO_OBSERVATION, // 无可用观测 NO_OBSERVATION, ///< 当前无可用 tag 观测。
TARGET_NOT_LOCKED, // 尚未缓存任何 tag 锚点 TARGET_NOT_LOCKED, ///< 尚未建立目标锚点。
NO_MATCHING_TAG // 有观测，但没有命中已缓存的 tag NO_MATCHING_TAG ///< 有观测但没有命中已缓存锚点。
}; };
/** /**
* @brief 构造函数，支持直接注入抽象相机。 * @brief 目标像素转 3D 点的方法。
* @param camera 抽象相机，可为空（后续可仅使用几何接口）。 */
enum class TargetPointMethod {
TAG_PLANE = 0, ///< 像素射线与 tag 平面求交（多 tag 融合）。
DEPTH_IMAGE ///< 深度反投影（要求 RGBD 对齐）。
};
/**
* @brief 构造函数。
* @param camera 抽象相机对象，可为空。
*/ */
explicit TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera = nullptr); explicit TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera = nullptr);
~TagRelativeTarget3D() = default;
/** /**
* @brief 设置/替换相机对象。 * @brief 设置/替换相机对象。
* @param camera 抽象相机。 * @param camera 抽象相机对象。
*/ */
void setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera) { camera_ = camera; } void setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera);
/** /**
* @brief 清空全部 tag 锚点。 * @brief 清空内部状态（锚点、缓存、追踪状态与最近输出）。
*/ */
void clear(); void clear();
/** /**
* @brief 是否没有任何缓存锚点。 * @brief 是否没有任何已缓存锚点。
* @return 为空返回 `true`。 * @return 没有锚点返回 `true`。
*/ */
bool empty() const { return anchors_in_tag_.empty(); } bool empty() const { return anchors_in_tag_.empty(); }
/** /**
* @brief 当前缓存锚点数量。 * @brief 当前已缓存锚点数量。
* @return 锚点数量。 * @return 锚点数量。
*/ */
size_t anchorCount() const { return anchors_in_tag_.size(); } size_t anchorCount() const { return anchors_in_tag_.size(); }
/** /**
* @brief 获取最近状态。 * @brief 获取最近一次调用状态。
* @return 最近状态枚举。 * @return 状态码。
*/ */
Status lastStatus() const { return last_status_; } Status lastStatus() const { return last_status_; }
/** /**
* @brief 状态枚举转字符串。 * @brief 状态码转可读字符串。
* @param status 状态枚举。 * @param status 状态码。
* @return 字符串常量。 * @return 对应字符串常量。
*/ */
static const char* statusToString(Status status); static const char* statusToString(Status status);
/** /**
* @brief 设置 AprilTag 边长。 * @brief 设置目标像素转 3D 方法。
* @param tag_size_m tag 边长（米）。 * @param method 求解方法。
*/
void setTargetPointMethod(TargetPointMethod method) { target_point_method_ = method; }
/**
* @brief 获取当前目标像素转 3D 方法。
* @return 当前方法。
*/
TargetPointMethod targetPointMethod() const { return target_point_method_; }
/**
* @brief 设置 AprilTag 物理边长。
* @param tag_size_m 边长（单位：米），必须大于 0。
*/ */
void setTagSize(double tag_size_m); void setTagSize(double tag_size_m);
/** /**
* @brief 通过内部相机抓帧并检测 tag 观测（`T_c_t`）。 * @brief 设置深度采样与离群剔除参数。
* @param observations 输出观测列表。 *
* @return 检测成功返回 `true`。 * 该配置仅在 `DEPTH_IMAGE` 模式生效。
*
* @param window_size_px 深度采样窗口边长（像素，自动修正为奇数）。
* @param enable_outlier_reject 是否启用离群值剔除。
* @param outlier_sigma MAD 门限倍数。
* @param outlier_min_dev_m 离群剔除最小绝对阈值（米）。
*/ */
bool detectObservations(std::vector<TagPoseObservation>& observations); void setDepthSamplingConfig(int window_size_px = 3,
bool enable_outlier_reject = true,
double outlier_sigma = 2.5,
double outlier_min_dev_m = 0.003);
/** /**
* @brief 使用外部图像与内参检测 tag 观测（`T_c_t`）。 * @brief 设置 active tag 切换策略。
* @param color 输入彩色/灰度图。 * @param missing_before_switch active tag 连续丢失多少帧后允许切换。
* @param intrinsics 相机内参。 * @param switch_hysteresis 新候选切换阈值倍率（>1 时更保守）。
* @param observations 输出观测列表。
* @return 检测成功返回 `true`。
*/ */
bool detectObservationsFromImage(const cv::Mat& color, void setActiveTagSwitchPolicy(int missing_before_switch = 4,
const device::Rs2Intrinsics& intrinsics, double switch_hysteresis = 1.2);
std::vector<TagPoseObservation>& observations);
/** /**
* @brief 使用内部相机检测结果直接建立/更新锚点。 * @brief 设置 active tag 候选评分权重。
* @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。 * @param weight_power 观测权重项指数。
* @param overwrite_existing 是否覆盖已存在锚点。 * @param proximity_power 距离项指数（目标越靠近 tag 原点评分越高）。
* @return 成功建立至少一个锚点返回 `true`。 * @param proximity_scale_m 距离归一化尺度（米）。
*/ */
bool lockTargetInCamera(const Eigen::Vector3d& p_c_target, void setTrackingCandidateScoreWeights(double weight_power = 1.0,
bool overwrite_existing = true); double proximity_power = 1.0,
double proximity_scale_m = 0.08);
/** /**
* @brief 使用内部相机检测结果直接恢复目标点相机坐标。 * @brief 重置 active tag 跟踪状态（不清空锚点）。
* @param p_c_target_out 输出目标点相机坐标（米）。
* @param used_tag_id_out 若非空，输出主导 tag id。
* @param spread_out 若非空，输出多 tag 融合离散误差（米）。
* @return 恢复成功返回 `true`。
*/ */
bool resolveTargetInCamera(Eigen::Vector3d& p_c_target_out, void resetActiveTagTracking();
int* used_tag_id_out = nullptr,
double* spread_out = nullptr);
/** /**
* @brief 根据“目标点相机坐标 + 可见 tag 观测”批量建立/更新各 tag 锚点。 * @brief 当前 active tag id。
* @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。 * @return active tag id；无效时为 `-1`。
* @param observations 可见 tag 观测集合。
* @param overwrite_existing 是否覆盖已存在的同 id 锚点。
* @return 成功建立至少一个锚点时返回 `true`。
*/ */
bool lockTargetInCamera(const Eigen::Vector3d& p_c_target, int activeTagId() const { return active_tag_id_; }
const std::vector<TagPoseObservation>& observations,
bool overwrite_existing = true);
/** /**
* @brief 用单个 tag 观测建立/更新锚点。 * @brief 当前 active tag 连续丢失帧数。
* @return 丢失帧计数。
*/
int activeTagMissingCount() const { return active_tag_missing_count_; }
/**
* @brief 对目标像素进行单次 3D 解算。
* @param u 像素列坐标。
* @param v 像素行坐标。
* @return 成功返回 `true`，结果写入 `last*`。
*/
bool solveFromPixel(int u, int v);
/**
* @brief 以目标像素初始化跟踪（解算 + 建锚点 + 选择 active tag）。
* @param u 像素列坐标。
* @param v 像素行坐标。
* @return 成功返回 `true`，结果写入 `last*`。
*/
bool startTrackingFromPixel(int u, int v);
/**
* @brief 进行一次跟踪迭代（active tag 跟踪 + 必要时自动切换）。
* @return 成功返回 `true`，结果写入 `last*`。
*/
bool track();
/**
* @brief 最近一次成功解算/跟踪得到的目标点相机坐标。
* @return 目标点 `p_c_target`（米）。
*/
const Eigen::Vector3d& lastTargetInCamera() const { return last_p_c_target_; }
/**
* @brief 最近一次结果对应的主导 tag id。
* @return 平面法为主导 tag id；深度法通常为 `-1`。
*/
int lastUsedTagId() const { return last_used_tag_id_; }
/**
* @brief 最近一次平面法融合离散度。
* @return 离散度（米）；深度法通常为 0。
*/
double lastSpread() const { return last_spread_m_; }
/**
* @brief 最近一次 `track()` 是否发生 active tag 切换。
* @return 切换返回 `true`。
*/
bool lastSwitched() const { return last_switched_; }
/**
* @brief 是否存在指定 tag 的目标锚点。
* @param tag_id tag id。 * @param tag_id tag id。
* @param T_c_t tag 坐标系到相机坐标系变换。 * @return 存在返回 `true`。
* @param p_c_target 目标点在相机坐标系的 3D 坐标（米）。
* @return 建立成功返回 `true`。
*/
bool upsertAnchorFromCamera(int tag_id,
const Eigen::Matrix4d& T_c_t,
const Eigen::Vector3d& p_c_target);
/**
* @brief 根据当前可见 tag，恢复目标点在当前相机坐标系下的 3D 坐标。
* @param observations 当前可见 tag 观测。
* @param p_c_target_out 输出目标点相机坐标（米）。
* @param used_tag_id_out 若非空，输出本次主导 tag id（仅 1 个匹配时等于该 id，多匹配时为最大权重 id）。
* @param spread_out 若非空，输出多 tag 融合后最大离散误差（米）。
* @return 恢复成功返回 `true`。
*/
bool resolveTargetInCamera(const std::vector<TagPoseObservation>& observations,
Eigen::Vector3d& p_c_target_out,
int* used_tag_id_out = nullptr,
double* spread_out = nullptr);
/**
* @brief 是否已缓存指定 tag 的锚点。
* @param tag_id tag id。
* @return 命中返回 `true`。
*/ */
bool hasAnchorForTag(int tag_id) const; bool hasAnchorForTag(int tag_id) const;
/** /**
* @brief 获取目标点在指定 tag 坐标系下的 3D 相对坐标。 * @brief 获取指定 tag 坐标系下缓存的目标点坐标。
* @param tag_id tag id。 * @param tag_id tag id。
* @param p_t_target_out 输出 `p_t_target`（米）。 * @param p_t_target_out 输出目标点在该 tag 坐标系下的坐标。
* @return 获取成功返回 `true`。 * @return 成功返回 `true`。
*/ */
bool getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const; bool getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const;
private:
/** /**
* @brief 获取构造时注入的相机对象。 * @brief 单帧 tag 位姿观测（tag 坐标系到相机坐标系）。
* @return 抽象相机智能指针。
*/ */
const std::shared_ptr<cmvr::device::AbstractCamera>& camera() const { return camera_; } struct TagPoseObservation {
// tag id。
int tag_id{-1};
// tag -> camera 变换矩阵。
Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
// 观测权重（检测置信度）。
double weight{1.0};
};
/**
* @brief 最近一帧图像与内参缓存。
*/
struct FrameCache {
// RGB 图。
cv::Mat color;
// 深度图。
cv::Mat depth;
// 相机内参。
device::Rs2Intrinsics intr{};
// 当前缓存是否有 RGB。
bool has_color{false};
// 当前缓存是否有深度。
bool has_depth{false};
// 当前缓存是否有内参。
bool has_intr{false};
// 帧序号（用于观测同步）。
uint64_t frame_id{0};
};
private: private:
/**
* @brief 重置最近一次调用输出缓存。
*/
void resetLastOutputs();
/**
* @brief 内部抓帧与观测更新入口。
* @param need_depth 是否需要深度图。
* @param need_tags 是否需要更新 tag 观测。
* @return 成功返回 `true`。
*/
bool updateInternal(bool need_depth, bool need_tags);
/**
* @brief 抓取一帧 RGB 与内参。
* @return 成功返回 `true`。
*/
bool grabRGB();
/**
* @brief 抓取一帧 RGBD 与内参。
* @return 成功返回 `true`。
*/
bool grabRGBD();
/**
* @brief 从当前缓存 RGB + 内参中检测 tag 并更新 `observations_`。
* @return 成功返回 `true`。
*/
bool updateObservations(); // uses frame_.color + frame_.intr
/**
* @brief 依据当前观测将目标点写入各 tag 锚点缓存。
* @param p_c_target 目标点在相机坐标系下位置。
* @param overwrite_existing 是否覆盖已有锚点。
* @return 成功返回 `true`。
*/
bool lockTargetInCameraInternal(const Eigen::Vector3d& p_c_target, bool overwrite_existing);
/**
* @brief 依据当前观测与锚点恢复目标点相机坐标。
* @return 成功返回 `true`。
*/
bool resolveTargetInCameraInternal(); // anchors + observations_ => last_p_c_target_ (+spread/+used)
/**
* @brief 在当前缓存下使用平面法解像素目标点。
* @param u 像素列坐标。
* @param v 像素行坐标。
* @return 成功返回 `true`。
*/
bool solveFromPixelOnPlaneCached(int u, int v);
/**
* @brief 在当前缓存下使用深度法解像素目标点。
* @param u 像素列坐标。
* @param v 像素行坐标。
* @return 成功返回 `true`。
*/
bool solveFromPixelWithDepthCached(int u, int v);
/**
* @brief 使用锁定点初始化锚点并选择 active tag。
* @param p_c_lock 锁定时目标点相机坐标。
* @param used_tag 平面法主导 tag id（深度法可为 -1）。
* @param spread 平面法融合离散度。
* @return 成功返回 `true`。
*/
bool initTrackingFromResolvedPoint(const Eigen::Vector3d& p_c_lock,
int used_tag,
double spread);
/**
* @brief 在当前缓存观测下执行 active tag 跟踪。
* @return 成功返回 `true`。
*/
bool trackWithActiveTagFromCached();
/**
* @brief 判断 4x4 矩阵元素是否全为有限数。
* @param T 输入矩阵。
* @return 全有限返回 `true`。
*/
static bool isFiniteMatrix(const Eigen::Matrix4d& T); static bool isFiniteMatrix(const Eigen::Matrix4d& T);
/**
* @brief 判断 3D 向量是否全为有限数。
* @param p 输入向量。
* @return 全有限返回 `true`。
*/
static bool isFiniteVector(const Eigen::Vector3d& p); static bool isFiniteVector(const Eigen::Vector3d& p);
/**
* @brief 点从 tag 坐标系变换到相机坐标系。
* @param T_c_t tag 到相机变换。
* @param p_t tag 坐标系下点。
* @return 相机坐标系下点。
*/
static Eigen::Vector3d pointTagToCamera(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_t); static Eigen::Vector3d pointTagToCamera(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_t);
/**
* @brief 点从相机坐标系变换到 tag 坐标系。
* @param T_c_t tag 到相机变换。
* @param p_c 相机坐标系下点。
* @return tag 坐标系下点。
*/
static Eigen::Vector3d pointCameraToTag(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_c); static Eigen::Vector3d pointCameraToTag(const Eigen::Matrix4d& T_c_t, const Eigen::Vector3d& p_c);
/**
* @brief 相机射线与 tag 平面求交。
* @param T_c_t tag 到相机变换。
* @param ray_c 相机坐标系下射线方向。
* @param p_c_intersection 输出交点（相机坐标系）。
* @param view_cos_out 可选输出：视线与平面法向夹角余弦绝对值。
* @return 求交成功返回 `true`。
*/
static bool intersectRayWithTagPlane(const Eigen::Matrix4d& T_c_t,
const Eigen::Vector3d& ray_c,
Eigen::Vector3d& p_c_intersection,
double* view_cos_out = nullptr);
/**
* @brief 归一化观测权重（非法权重回退到默认值）。
* @param w 输入权重。
* @return 处理后的权重。
*/
static double sanitizeObsWeight(double w);
/**
* @brief 计算单个观测作为 active tag 候选的评分。
* @param obs tag 观测。
* @return 候选评分。
*/
double trackingCandidateScore(const TagPoseObservation& obs) const;
private: private:
// 缓存: tag_id -> 目标点在该 tag 坐标系下的 3D 坐标。 // 目标锚点缓存：tag_id -> p_t_target。
std::unordered_map<int, Eigen::Vector3d> anchors_in_tag_; std::unordered_map<int, Eigen::Vector3d> anchors_in_tag_;
// 抽象相机对象（由构造函数注入）。
// 相机对象。
std::shared_ptr<cmvr::device::AbstractCamera> camera_{nullptr}; std::shared_ptr<cmvr::device::AbstractCamera> camera_{nullptr};
// AprilTag 检测器。 // AprilTag 检测器。
vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11}; vpDetectorAprilTag detector_{vpDetectorAprilTag::TAG_36h11};
// tag 边长（米）。 // AprilTag 物理边长（米）。
double tag_size_m_{0.12}; double tag_size_m_{0.12};
// 最近一次调用状态。 // 最近一次调用状态。
Status last_status_{Status::TARGET_NOT_LOCKED}; Status last_status_{Status::TARGET_NOT_LOCKED};
// 图像/内参缓存。
FrameCache frame_;
// 当前帧 tag 观测缓存。
std::vector<TagPoseObservation> observations_;
// 观测缓存对应的帧 id。
uint64_t obs_frame_id_{0};
// 当前 active tag id。
int active_tag_id_{-1};
// active tag 连续丢失帧计数。
int active_tag_missing_count_{0};
// 连续丢失多少帧后允许切换 active tag。
int missing_before_switch_{4};
// active 切换迟滞阈值。
double switch_hysteresis_{1.2};
// 候选评分中权重项指数。
double candidate_weight_power_{1.0};
// 候选评分中距离项指数。
double candidate_proximity_power_{1.0};
// 候选评分距离归一化尺度（米）。
double candidate_proximity_scale_m_{0.08};
// 当前像素转 3D 方法。
TargetPointMethod target_point_method_{TargetPointMethod::TAG_PLANE};
// 深度采样窗口边长（像素）。
int depth_window_size_px_{3};
// 是否启用深度离群剔除。
bool depth_outlier_reject_enabled_{true};
// 深度离群剔除 sigma 参数。
double depth_outlier_sigma_{2.5};
// 深度离群剔除最小绝对偏差（米）。
double depth_outlier_min_dev_m_{0.002};
// 最近一次输出的目标点相机坐标。
Eigen::Vector3d last_p_c_target_{Eigen::Vector3d::Zero()};
// 最近一次输出使用的主导 tag id。
int last_used_tag_id_{-1};
// 最近一次输出离散度（米）。
double last_spread_m_{0.0};
// 最近一次 track 是否发生 active tag 切换。
bool last_switched_{false};
}; };
} // namespace cmvr::perception } // namespace cmvr::perception
#endif // CMVR_PERCEPTION_TAG_RELATIVE_TARGET_3D_H

833
cmvr-es/perception/src/tag_relative_target_3d.cpp
View File

@ -1,25 +1,82 @@
//
// Created by lgv on 2026/2/26.
//
#include "perception/include/tag_relative_target_3d.h" #include "perception/include/tag_relative_target_3d.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
#include <cstdint>
#include <cstring> #include <cstring>
#include <limits> #include <limits>
#include <opencv2/imgproc.hpp> #include <opencv2/imgproc.hpp>
#include <visp3/core/vpCameraParameters.h> #include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h> #include <visp3/core/vpImage.h>
#include "common/utils/image/image_process.h"
namespace cmvr::perception { namespace cmvr::perception {
namespace {
// -------- helpers --------
double sanitizeWeight(double w) {
if (!std::isfinite(w) || w <= 0.0) return 1.0;
return w;
}
class WeightedPointFusion {
public:
void reserve(size_t n) { points_.reserve(n); }
void addPoint(int tag_id, const Eigen::Vector3d& p_c, double weight) {
if (!std::isfinite(weight) || weight <= 0.0) return;
points_.push_back(p_c);
weighted_sum_ += weight * p_c;
weight_sum_ += weight;
if (weight > best_weight_) {
best_weight_ = weight;
best_id_ = tag_id;
}
}
bool finalize(Eigen::Vector3d& p_c_out, int& best_id_out, double& spread_out) const {
if (points_.empty() || weight_sum_ <= 0.0) return false;
p_c_out = weighted_sum_ / weight_sum_;
best_id_out = best_id_;
double max_err = 0.0;
for (const auto& p : points_) {
max_err = std::max(max_err, (p - p_c_out).norm());
}
spread_out = max_err;
return true;
}
private:
std::vector<Eigen::Vector3d> points_;
Eigen::Vector3d weighted_sum_ = Eigen::Vector3d::Zero();
double weight_sum_ = 0.0;
double best_weight_ = -std::numeric_limits<double>::infinity();
int best_id_ = -1;
};
} // namespace
TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera) TagRelativeTarget3D::TagRelativeTarget3D(const std::shared_ptr<cmvr::device::AbstractCamera>& camera)
: camera_(camera) { : camera_(camera) {
detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
} }
void TagRelativeTarget3D::setCamera(const std::shared_ptr<cmvr::device::AbstractCamera>& camera) {
camera_ = camera;
frame_ = FrameCache{};
observations_.clear();
obs_frame_id_ = 0;
anchors_in_tag_.clear();
resetActiveTagTracking();
resetLastOutputs();
last_status_ = Status::TARGET_NOT_LOCKED;
}
const char* TagRelativeTarget3D::statusToString(Status status) { const char* TagRelativeTarget3D::statusToString(Status status) {
switch (status) { switch (status) {
case Status::OK: return "ok"; case Status::OK: return "ok";
@ -35,82 +92,303 @@ const char* TagRelativeTarget3D::statusToString(Status status) {
} }
} }
void TagRelativeTarget3D::clear() {
anchors_in_tag_.clear();
resetActiveTagTracking();
resetLastOutputs();
frame_ = FrameCache{};
observations_.clear();
obs_frame_id_ = 0;
last_status_ = Status::TARGET_NOT_LOCKED;
}
void TagRelativeTarget3D::resetLastOutputs() {
last_p_c_target_.setZero();
last_used_tag_id_ = -1;
last_spread_m_ = 0.0;
last_switched_ = false;
}
void TagRelativeTarget3D::setTagSize(double tag_size_m) { void TagRelativeTarget3D::setTagSize(double tag_size_m) {
if (std::isfinite(tag_size_m) && tag_size_m > 0.0) { if (std::isfinite(tag_size_m) && tag_size_m > 0.0) tag_size_m_ = tag_size_m;
tag_size_m_ = tag_size_m; }
void TagRelativeTarget3D::setDepthSamplingConfig(int window_size_px,
bool enable_outlier_reject,
double outlier_sigma,
double outlier_min_dev_m) {
int w = std::max(1, window_size_px);
if ((w % 2) == 0) ++w;
w = std::min(w, 31);
depth_window_size_px_ = w;
depth_outlier_reject_enabled_ = enable_outlier_reject;
if (std::isfinite(outlier_sigma) && outlier_sigma > 0.0) depth_outlier_sigma_ = outlier_sigma;
if (std::isfinite(outlier_min_dev_m) && outlier_min_dev_m >= 0.0) depth_outlier_min_dev_m_ = outlier_min_dev_m;
}
void TagRelativeTarget3D::setActiveTagSwitchPolicy(int missing_before_switch,
double switch_hysteresis) {
missing_before_switch_ = std::max(1, missing_before_switch);
if (std::isfinite(switch_hysteresis) && switch_hysteresis > 0.0) {
switch_hysteresis_ = std::max(1.0, switch_hysteresis);
} }
} }
bool TagRelativeTarget3D::detectObservations(std::vector<TagPoseObservation>& observations) { void TagRelativeTarget3D::setTrackingCandidateScoreWeights(double weight_power,
observations.clear(); double proximity_power,
double proximity_scale_m) {
if (std::isfinite(weight_power) && weight_power >= 0.0) candidate_weight_power_ = weight_power;
if (std::isfinite(proximity_power) && proximity_power >= 0.0) candidate_proximity_power_ = proximity_power;
if (std::isfinite(proximity_scale_m) && proximity_scale_m > 1e-6) candidate_proximity_scale_m_ = proximity_scale_m;
}
void TagRelativeTarget3D::resetActiveTagTracking() {
active_tag_id_ = -1;
active_tag_missing_count_ = 0;
}
// =========================
// Internal frame grabbing
// =========================
bool TagRelativeTarget3D::grabRGB() {
if (!camera_) { if (!camera_) {
last_status_ = Status::NO_CAMERA; last_status_ = Status::NO_CAMERA;
return false; return false;
} }
cv::Mat color; frame_.has_color = frame_.has_depth = frame_.has_intr = false;
device::Rs2Intrinsics intrinsics{};
try { try {
// Tag 检测只需要彩色图像与相机内参，避免依赖深度流导致阻塞/超时。 camera_->getRGBImage(frame_.color, frame_.intr);
camera_->getRGBImage(color, intrinsics); } catch (...) {
} catch (const std::exception&) {
last_status_ = Status::NO_NEW_FRAME; last_status_ = Status::NO_NEW_FRAME;
return false; return false;
} }
if (frame_.color.empty()) {
last_status_ = Status::NO_NEW_FRAME;
return false;
}
frame_.has_color = true;
frame_.has_intr = true;
frame_.frame_id++;
return true;
}
bool TagRelativeTarget3D::grabRGBD() {
if (!camera_) {
last_status_ = Status::NO_CAMERA;
return false;
}
frame_.has_color = frame_.has_depth = frame_.has_intr = false;
try {
camera_->getRGBDImages(frame_.color, frame_.depth, frame_.intr);
} catch (...) {
last_status_ = Status::NO_NEW_FRAME;
return false;
}
if (frame_.color.empty() || frame_.depth.empty()) {
last_status_ = Status::NO_NEW_FRAME;
return false;
}
frame_.has_color = true;
frame_.has_depth = true;
frame_.has_intr = true;
frame_.frame_id++;
return true;
}
bool TagRelativeTarget3D::updateInternal(bool need_depth, bool need_tags) {
// Always grab a NEW frame when called
observations_.clear();
if (need_depth) {
if (!grabRGBD()) return false;
} else {
if (!grabRGB()) return false;
}
if (need_tags) {
if (!updateObservations()) return false;
} else {
last_status_ = Status::OK;
}
return true;
}
bool TagRelativeTarget3D::updateObservations() {
if (!frame_.has_color || !frame_.has_intr) {
last_status_ = Status::BAD_IMAGE;
return false;
}
const cv::Mat& color = frame_.color;
if (color.empty()) { if (color.empty()) {
last_status_ = Status::NO_NEW_FRAME; last_status_ = Status::NO_NEW_FRAME;
return false; return false;
} }
if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) {
return detectObservationsFromImage(color, intrinsics, observations); last_status_ = Status::BAD_IMAGE;
}
bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target,
bool overwrite_existing) {
std::vector<TagPoseObservation> observations;
if (!detectObservations(observations)) {
return false; return false;
} }
return lockTargetInCamera(p_c_target, observations, overwrite_existing);
}
bool TagRelativeTarget3D::resolveTargetInCamera(Eigen::Vector3d& p_c_target_out, const double fx = static_cast<double>(frame_.intr.fx);
int* used_tag_id_out, const double fy = static_cast<double>(frame_.intr.fy);
double* spread_out) { const double cx = static_cast<double>(frame_.intr.cx);
std::vector<TagPoseObservation> observations; const double cy = static_cast<double>(frame_.intr.cy);
if (!detectObservations(observations)) { if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
last_status_ = Status::BAD_IMAGE;
return false; return false;
} }
return resolveTargetInCamera(observations, p_c_target_out, used_tag_id_out, spread_out);
cv::Mat gray;
if (color.channels() == 1) gray = color;
else if (color.channels() == 3) cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
else cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
if (gray.empty() || gray.type() != CV_8UC1) {
last_status_ = Status::BAD_IMAGE;
return false;
}
if (!gray.isContinuous()) gray = gray.clone();
const int width = gray.cols;
const int height = gray.rows;
vpCameraParameters cam;
cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
vpImage<unsigned char> I(height, width);
for (int y = 0; y < height; ++y) {
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
}
std::vector<vpHomogeneousMatrix> cMo_vec;
const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
if (!detected || cMo_vec.empty()) {
last_status_ = Status::NO_TAG;
obs_frame_id_ = frame_.frame_id;
return false;
}
const std::vector<int> tag_ids = detector_.getTagsId();
const std::vector<float> margins = detector_.getTagsDecisionMargin();
const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size());
if (pair_size == 0) {
last_status_ = Status::NO_TAG;
obs_frame_id_ = frame_.frame_id;
return false;
}
observations_.clear();
observations_.reserve(pair_size);
for (size_t i = 0; i < pair_size; ++i) {
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
const vpHomogeneousMatrix& cMo = cMo_vec[i];
for (int r = 0; r < 3; ++r) {
for (int c = 0; c < 3; ++c) T(r, c) = cMo[r][c];
}
T(0, 3) = cMo[0][3];
T(1, 3) = cMo[1][3];
T(2, 3) = cMo[2][3];
double w = 1.0;
if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) {
w = static_cast<double>(margins[i]);
}
observations_.push_back(TagPoseObservation{tag_ids[i], T, w});
}
if (observations_.empty()) {
last_status_ = Status::NO_TAG;
obs_frame_id_ = frame_.frame_id;
return false;
}
obs_frame_id_ = frame_.frame_id;
last_status_ = Status::OK;
return true;
} }
void TagRelativeTarget3D::clear() { // =========================
anchors_in_tag_.clear(); // Public APIs (u,v only)
last_status_ = Status::TARGET_NOT_LOCKED; // =========================
bool TagRelativeTarget3D::solveFromPixel(int u, int v) {
resetLastOutputs();
if (target_point_method_ == TargetPointMethod::DEPTH_IMAGE) {
if (!updateInternal(true, false)) return false; // RGBD only
return solveFromPixelWithDepthCached(u, v);
}
if (!updateInternal(false, true)) return false; // RGB + tags
return solveFromPixelOnPlaneCached(u, v);
} }
bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target, bool TagRelativeTarget3D::startTrackingFromPixel(int u, int v) {
const std::vector<TagPoseObservation>& observations, resetLastOutputs();
bool overwrite_existing) { resetActiveTagTracking();
const bool need_depth = (target_point_method_ == TargetPointMethod::DEPTH_IMAGE);
if (!updateInternal(need_depth, true)) return false; // 追踪必须要 tags
// 先求 lock 点（写入 last_*）
if (target_point_method_ == TargetPointMethod::DEPTH_IMAGE) {
if (!solveFromPixelWithDepthCached(u, v)) return false;
// 深度法：used_tag=-1, spread=0（已经在 solveFromPixelWithDepthCached 里写好）
} else {
if (!solveFromPixelOnPlaneCached(u, v)) return false;
}
// 用 lock 点建锚点 + 选 active
const Eigen::Vector3d p_lock = last_p_c_target_;
const int used_tag = last_used_tag_id_;
const double spread = last_spread_m_;
return initTrackingFromResolvedPoint(p_lock, used_tag, spread);
}
bool TagRelativeTarget3D::track() {
resetLastOutputs();
if (anchors_in_tag_.empty()) {
last_status_ = Status::TARGET_NOT_LOCKED;
return false;
}
if (!updateInternal(false, true)) return false; // RGB + tags
return trackWithActiveTagFromCached();
}
// =========================
// Internal anchor ops
// =========================
bool TagRelativeTarget3D::lockTargetInCameraInternal(const Eigen::Vector3d& p_c_target, bool overwrite_existing) {
if (!isFiniteVector(p_c_target)) { if (!isFiniteVector(p_c_target)) {
last_status_ = Status::INVALID_INPUT; last_status_ = Status::INVALID_INPUT;
return false; return false;
} }
if (observations.empty()) { if (observations_.empty()) {
last_status_ = Status::NO_OBSERVATION; last_status_ = Status::NO_OBSERVATION;
return false; return false;
} }
size_t updated = 0; size_t updated = 0;
for (const auto& obs : observations) { for (const auto& obs : observations_) {
if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) { if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) continue;
continue; if (!overwrite_existing && anchors_in_tag_.find(obs.tag_id) != anchors_in_tag_.end()) continue;
}
if (!overwrite_existing && anchors_in_tag_.find(obs.tag_id) != anchors_in_tag_.end()) {
continue;
}
anchors_in_tag_[obs.tag_id] = pointCameraToTag(obs.T_c_t, p_c_target); anchors_in_tag_[obs.tag_id] = pointCameraToTag(obs.T_c_t, p_c_target);
++updated; ++updated;
} }
@ -124,120 +402,304 @@ bool TagRelativeTarget3D::lockTargetInCamera(const Eigen::Vector3d& p_c_target,
return true; return true;
} }
bool TagRelativeTarget3D::upsertAnchorFromCamera(int tag_id, bool TagRelativeTarget3D::resolveTargetInCameraInternal() {
const Eigen::Matrix4d& T_c_t,
const Eigen::Vector3d& p_c_target) {
if (tag_id < 0 || !isFiniteMatrix(T_c_t) || !isFiniteVector(p_c_target)) {
last_status_ = Status::INVALID_INPUT;
return false;
}
anchors_in_tag_[tag_id] = pointCameraToTag(T_c_t, p_c_target);
last_status_ = Status::OK;
return true;
}
bool TagRelativeTarget3D::resolveTargetInCamera(const std::vector<TagPoseObservation>& observations,
Eigen::Vector3d& p_c_target_out,
int* used_tag_id_out,
double* spread_out) {
if (used_tag_id_out) {
*used_tag_id_out = -1;
}
if (spread_out) {
*spread_out = 0.0;
}
if (anchors_in_tag_.empty()) { if (anchors_in_tag_.empty()) {
last_status_ = Status::TARGET_NOT_LOCKED; last_status_ = Status::TARGET_NOT_LOCKED;
return false; return false;
} }
if (observations.empty()) { if (observations_.empty()) {
last_status_ = Status::NO_OBSERVATION; last_status_ = Status::NO_OBSERVATION;
return false; return false;
} }
std::vector<Eigen::Vector3d> matched_points; WeightedPointFusion fusion;
matched_points.reserve(observations.size()); fusion.reserve(observations_.size());
Eigen::Vector3d weighted_sum = Eigen::Vector3d::Zero(); for (const auto& obs : observations_) {
double weight_sum = 0.0; if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) continue;
double best_weight = -std::numeric_limits<double>::infinity();
int best_id = -1;
for (const auto& obs : observations) {
if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) {
continue;
}
const auto it = anchors_in_tag_.find(obs.tag_id); const auto it = anchors_in_tag_.find(obs.tag_id);
if (it == anchors_in_tag_.end()) { if (it == anchors_in_tag_.end()) continue;
continue;
}
const Eigen::Vector3d p_c = pointTagToCamera(obs.T_c_t, it->second); const Eigen::Vector3d p_c = pointTagToCamera(obs.T_c_t, it->second);
if (!isFiniteVector(p_c)) { if (!isFiniteVector(p_c)) continue;
continue;
}
double w = obs.weight; fusion.addPoint(obs.tag_id, p_c, sanitizeObsWeight(obs.weight));
if (!std::isfinite(w) || w <= 0.0) {
w = 1.0;
}
matched_points.push_back(p_c);
weighted_sum += w * p_c;
weight_sum += w;
if (w > best_weight) {
best_weight = w;
best_id = obs.tag_id;
}
} }
if (matched_points.empty() || weight_sum <= 0.0) { Eigen::Vector3d p_out = Eigen::Vector3d::Zero();
int used = -1;
double spread = 0.0;
if (!fusion.finalize(p_out, used, spread)) {
last_status_ = Status::NO_MATCHING_TAG; last_status_ = Status::NO_MATCHING_TAG;
return false; return false;
} }
p_c_target_out = weighted_sum / weight_sum; last_p_c_target_ = p_out;
last_used_tag_id_ = used;
if (spread_out) { last_spread_m_ = spread;
double max_err = 0.0;
for (const auto& p : matched_points) {
max_err = std::max(max_err, (p - p_c_target_out).norm());
}
*spread_out = max_err;
}
if (used_tag_id_out) {
*used_tag_id_out = best_id;
}
last_status_ = Status::OK; last_status_ = Status::OK;
return true; return true;
} }
// =========================
// Internal solvers (cached)
// =========================
bool TagRelativeTarget3D::solveFromPixelOnPlaneCached(int u, int v) {
if (observations_.empty()) {
last_status_ = Status::NO_OBSERVATION;
return false;
}
Eigen::Vector3d ray_c = Eigen::Vector3d::Zero();
if (!cmvr::ImageProcess::pixelToRayCamera(frame_.intr, u, v, ray_c)) {
last_status_ = Status::INVALID_INPUT;
return false;
}
constexpr double kMinViewCos = 0.10; // 掠射剔除
constexpr double kDistNormPx = 200.0; // 像素邻近性尺度
const Eigen::Vector2d uv_target(static_cast<double>(u), static_cast<double>(v));
WeightedPointFusion fusion;
fusion.reserve(observations_.size());
for (const auto& obs : observations_) {
if (obs.tag_id < 0 || !isFiniteMatrix(obs.T_c_t)) continue;
Eigen::Vector3d p_c_intersection = Eigen::Vector3d::Zero();
double view_cos = 0.0;
if (!intersectRayWithTagPlane(obs.T_c_t, ray_c, p_c_intersection, &view_cos)) continue;
if (view_cos < kMinViewCos) continue;
const double w_obs = sanitizeObsWeight(obs.weight);
const Eigen::Vector3d tag_center_c = obs.T_c_t.block<3, 1>(0, 3);
Eigen::Vector2d uv_tag = Eigen::Vector2d::Zero();
double dist_px = kDistNormPx;
if (cmvr::ImageProcess::projectCameraPointToPixel(frame_.intr, tag_center_c, uv_tag)) {
dist_px = (uv_tag - uv_target).norm();
}
const double dist_gain = 1.0 / (1.0 + dist_px / kDistNormPx);
const double score = w_obs * view_cos * dist_gain;
if (!std::isfinite(score) || score <= 0.0) continue;
fusion.addPoint(obs.tag_id, p_c_intersection, score);
}
Eigen::Vector3d p_out = Eigen::Vector3d::Zero();
int used = -1;
double spread = 0.0;
if (!fusion.finalize(p_out, used, spread)) {
last_status_ = Status::NO_MATCHING_TAG;
return false;
}
last_p_c_target_ = p_out;
last_used_tag_id_ = used;
last_spread_m_ = spread;
last_status_ = Status::OK;
return true;
}
bool TagRelativeTarget3D::solveFromPixelWithDepthCached(int u, int v) {
if (!frame_.has_depth || frame_.depth.empty() || !frame_.has_intr) {
last_status_ = Status::BAD_IMAGE;
return false;
}
Eigen::Vector3d p = Eigen::Vector3d::Zero();
if (!cmvr::ImageProcess::pixelToCameraPointWithDepth(frame_.intr,
frame_.depth,
u,
v,
p,
depth_window_size_px_,
depth_outlier_reject_enabled_,
depth_outlier_sigma_,
depth_outlier_min_dev_m_)) {
last_status_ = Status::BAD_IMAGE;
return false;
}
last_p_c_target_ = p;
last_used_tag_id_ = -1;
last_spread_m_ = 0.0;
last_status_ = Status::OK;
return true;
}
bool TagRelativeTarget3D::initTrackingFromResolvedPoint(const Eigen::Vector3d& p_c_lock,
int used_tag,
double spread) {
if (!isFiniteVector(p_c_lock)) {
last_status_ = Status::INVALID_INPUT;
return false;
}
if (observations_.empty()) {
last_status_ = Status::NO_OBSERVATION;
return false;
}
// 建锚点（覆盖）
if (!lockTargetInCameraInternal(p_c_lock, true)) return false;
// 选 active tag
int active_tag = used_tag;
if (active_tag < 0 || !hasAnchorForTag(active_tag)) {
double best_score = -std::numeric_limits<double>::infinity();
for (const auto& obs : observations_) {
const double sc = trackingCandidateScore(obs);
if (sc > best_score) {
best_score = sc;
active_tag = obs.tag_id;
}
}
}
if (active_tag < 0 || !hasAnchorForTag(active_tag)) {
last_status_ = Status::NO_MATCHING_TAG;
return false;
}
active_tag_id_ = active_tag;
active_tag_missing_count_ = 0;
// 输出保持 lock 点
last_p_c_target_ = p_c_lock;
last_used_tag_id_ = used_tag;
last_spread_m_ = spread;
last_switched_ = false;
last_status_ = Status::OK;
return true;
}
bool TagRelativeTarget3D::trackWithActiveTagFromCached() {
if (observations_.empty()) {
last_status_ = Status::NO_OBSERVATION;
return false;
}
int active_obs_index = -1;
double active_score = -std::numeric_limits<double>::infinity();
int best_candidate_tag = -1;
int best_candidate_index = -1;
double best_candidate_score = -std::numeric_limits<double>::infinity();
for (size_t i = 0; i < observations_.size(); ++i) {
const auto& obs = observations_[i];
const double score = trackingCandidateScore(obs);
if (!std::isfinite(score) || score <= 0.0) continue;
if (obs.tag_id == active_tag_id_) {
active_obs_index = static_cast<int>(i);
active_score = score;
}
if (score > best_candidate_score) {
best_candidate_score = score;
best_candidate_tag = obs.tag_id;
best_candidate_index = static_cast<int>(i);
}
}
bool switched = false;
bool should_switch = false;
if (active_obs_index < 0) {
++active_tag_missing_count_;
if ((active_tag_id_ < 0 && best_candidate_index >= 0) ||
(active_tag_missing_count_ >= missing_before_switch_ && best_candidate_index >= 0)) {
should_switch = true;
}
} else {
active_tag_missing_count_ = 0;
if (best_candidate_tag >= 0 &&
best_candidate_tag != active_tag_id_ &&
best_candidate_score > active_score * switch_hysteresis_) {
should_switch = true;
}
}
if (should_switch && best_candidate_index >= 0) {
switched = (best_candidate_tag != active_tag_id_);
active_tag_id_ = best_candidate_tag;
active_tag_missing_count_ = 0;
active_obs_index = best_candidate_index;
}
if (active_obs_index < 0) {
last_status_ = Status::NO_MATCHING_TAG;
return false;
}
const auto& obs = observations_[active_obs_index];
const auto it = anchors_in_tag_.find(obs.tag_id);
if (it == anchors_in_tag_.end()) {
last_status_ = Status::NO_MATCHING_TAG;
return false;
}
const Eigen::Vector3d p_c = pointTagToCamera(obs.T_c_t, it->second);
if (!isFiniteVector(p_c)) {
last_status_ = Status::INVALID_INPUT;
return false;
}
last_p_c_target_ = p_c;
last_used_tag_id_ = obs.tag_id;
last_spread_m_ = 0.0; // 单 tag
last_switched_ = switched;
last_status_ = Status::OK;
return true;
}
// =========================
// Diagnostics
// =========================
bool TagRelativeTarget3D::hasAnchorForTag(int tag_id) const { bool TagRelativeTarget3D::hasAnchorForTag(int tag_id) const {
return anchors_in_tag_.find(tag_id) != anchors_in_tag_.end(); return anchors_in_tag_.find(tag_id) != anchors_in_tag_.end();
} }
bool TagRelativeTarget3D::getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const { bool TagRelativeTarget3D::getAnchorInTag(int tag_id, Eigen::Vector3d& p_t_target_out) const {
const auto it = anchors_in_tag_.find(tag_id); const auto it = anchors_in_tag_.find(tag_id);
if (it == anchors_in_tag_.end()) { if (it == anchors_in_tag_.end()) return false;
return false;
}
p_t_target_out = it->second; p_t_target_out = it->second;
return true; return true;
} }
// =========================
// math helpers
// =========================
double TagRelativeTarget3D::sanitizeObsWeight(double w) {
return sanitizeWeight(w);
}
double TagRelativeTarget3D::trackingCandidateScore(const TagPoseObservation& obs) const {
if (obs.tag_id < 0) return -std::numeric_limits<double>::infinity();
const auto it = anchors_in_tag_.find(obs.tag_id);
if (it == anchors_in_tag_.end()) return -std::numeric_limits<double>::infinity();
const double w = std::max(1e-12, sanitizeObsWeight(obs.weight));
const Eigen::Vector3d& p_t_target = it->second;
const double dist_in_tag = std::hypot(p_t_target.x(), p_t_target.y());
const double proximity_gain = 1.0 / (1.0 + dist_in_tag / candidate_proximity_scale_m_);
const double w_term = std::pow(w, candidate_weight_power_);
const double p_term = std::pow(std::max(1e-12, proximity_gain), candidate_proximity_power_);
const double score = w_term * p_term;
if (!std::isfinite(score)) return -std::numeric_limits<double>::infinity();
return score;
}
bool TagRelativeTarget3D::isFiniteMatrix(const Eigen::Matrix4d& T) { bool TagRelativeTarget3D::isFiniteMatrix(const Eigen::Matrix4d& T) {
for (int r = 0; r < 4; ++r) { for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c) { for (int c = 0; c < 4; ++c)
if (!std::isfinite(T(r, c))) { if (!std::isfinite(T(r, c))) return false;
return false;
}
}
}
return true; return true;
} }
@ -245,100 +707,6 @@ bool TagRelativeTarget3D::isFiniteVector(const Eigen::Vector3d& p) {
return std::isfinite(p.x()) && std::isfinite(p.y()) && std::isfinite(p.z()); return std::isfinite(p.x()) && std::isfinite(p.y()) && std::isfinite(p.z());
} }
bool TagRelativeTarget3D::detectObservationsFromImage(const cv::Mat& color,
const device::Rs2Intrinsics& intrinsics,
std::vector<TagPoseObservation>& observations) {
observations.clear();
if (color.empty()) {
last_status_ = Status::NO_NEW_FRAME;
return false;
}
if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) {
last_status_ = Status::BAD_IMAGE;
return false;
}
const double fx = static_cast<double>(intrinsics.fx);
const double fy = static_cast<double>(intrinsics.fy);
const double cx = static_cast<double>(intrinsics.cx);
const double cy = static_cast<double>(intrinsics.cy);
if (!std::isfinite(fx) || !std::isfinite(fy) || fx <= 0.0 || fy <= 0.0) {
last_status_ = Status::BAD_IMAGE;
return false;
}
cv::Mat gray;
if (color.channels() == 1) {
gray = color;
} else if (color.channels() == 3) {
cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
} else {
cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
}
if (gray.empty() || gray.type() != CV_8UC1) {
last_status_ = Status::BAD_IMAGE;
return false;
}
if (!gray.isContinuous()) {
gray = gray.clone();
}
const int width = gray.cols;
const int height = gray.rows;
vpCameraParameters cam;
cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
vpImage<unsigned char> I(height, width);
for (int y = 0; y < height; ++y) {
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(width));
}
std::vector<vpHomogeneousMatrix> cMo_vec;
const bool detected = detector_.detect(I, tag_size_m_, cam, cMo_vec);
if (!detected || cMo_vec.empty()) {
last_status_ = Status::NO_TAG;
return false;
}
const std::vector<int> tag_ids = detector_.getTagsId();
const std::vector<float> margins = detector_.getTagsDecisionMargin();
const size_t pair_size = std::min(tag_ids.size(), cMo_vec.size());
if (pair_size == 0) {
last_status_ = Status::NO_TAG;
return false;
}
observations.reserve(pair_size);
for (size_t i = 0; i < pair_size; ++i) {
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
const vpHomogeneousMatrix& cMo = cMo_vec[i];
for (int r = 0; r < 3; ++r) {
for (int c = 0; c < 3; ++c) {
T(r, c) = cMo[r][c];
}
}
T(0, 3) = cMo[0][3];
T(1, 3) = cMo[1][3];
T(2, 3) = cMo[2][3];
double w = 1.0;
if (i < margins.size() && std::isfinite(margins[i]) && margins[i] > 0.0f) {
w = static_cast<double>(margins[i]);
}
observations.push_back(TagPoseObservation{tag_ids[i], T, w});
}
if (observations.empty()) {
last_status_ = Status::NO_TAG;
return false;
}
last_status_ = Status::OK;
return true;
}
Eigen::Vector3d TagRelativeTarget3D::pointTagToCamera(const Eigen::Matrix4d& T_c_t, Eigen::Vector3d TagRelativeTarget3D::pointTagToCamera(const Eigen::Matrix4d& T_c_t,
const Eigen::Vector3d& p_t) { const Eigen::Vector3d& p_t) {
const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0); const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
@ -353,4 +721,35 @@ Eigen::Vector3d TagRelativeTarget3D::pointCameraToTag(const Eigen::Matrix4d& T_c
return R.transpose() * (p_c - t); return R.transpose() * (p_c - t);
} }
bool TagRelativeTarget3D::intersectRayWithTagPlane(const Eigen::Matrix4d& T_c_t,
const Eigen::Vector3d& ray_c,
Eigen::Vector3d& p_c_intersection,
double* view_cos_out) {
if (!isFiniteMatrix(T_c_t) || !isFiniteVector(ray_c)) return false;
const Eigen::Matrix3d R = T_c_t.block<3, 3>(0, 0);
const Eigen::Vector3d t = T_c_t.block<3, 1>(0, 3);
Eigen::Vector3d n = R.col(2); // tag 局部 z 轴
const double n_norm = n.norm();
if (!std::isfinite(n_norm) || n_norm <= 1e-12) return false;
n /= n_norm;
const double ray_norm = ray_c.norm();
if (!std::isfinite(ray_norm) || ray_norm <= 1e-12) return false;
const Eigen::Vector3d d = ray_c / ray_norm;
const double denom = n.dot(d);
if (!std::isfinite(denom) || std::abs(denom) <= 1e-8) return false;
const double lambda = n.dot(t) / denom;
if (!std::isfinite(lambda) || lambda <= 1e-8) return false;
p_c_intersection = lambda * d;
if (!isFiniteVector(p_c_intersection)) return false;
if (view_cos_out) *view_cos_out = std::abs(denom); // [0,1]
return true;
}
} // namespace cmvr::perception } // namespace cmvr::perception

561
cmvr-es/perception/src/tag_relative_target_3d_test.cpp
View File

@ -10,19 +10,28 @@
#include <algorithm> #include <algorithm>
#include <chrono> #include <chrono>
#include <cmath> #include <cmath>
#include <cstring>
#include <cstdint>
#include <iomanip>
#include <iostream> #include <iostream>
#include <limits>
#include <sstream>
#include <thread> #include <thread>
#include <opencv2/highgui.hpp> #include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp> #include <opencv2/imgproc.hpp>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
namespace { namespace {
// 直接在这里改测试参数。 // 直接在这里改测试参数。
constexpr const char* kRsSerial = "243122074587"; constexpr const char* kRsSerial = "243122074587";
// Target pixel used for depth sampling. Negative means image center. // constexpr const char* kRsSerial = "243122075389";
constexpr int kTargetU = 350; // constexpr const char* kRsSerial = "f1421544";
constexpr int kTargetV = 250; // Target pixel. Negative means image center.
constexpr double kTagSize = 0.02; constexpr int kTargetU = -1;
constexpr int kTargetV = -1;
constexpr double kTagSize = 0.01975 ;
// <= 0 means loop until user abort. // <= 0 means loop until user abort.
constexpr int kTries = -1; constexpr int kTries = -1;
constexpr int kWidth = 1280; constexpr int kWidth = 1280;
@ -33,11 +42,7 @@ constexpr int kWarmupMaxTries = 60;
constexpr int kWarmupSleepMs = 50; constexpr int kWarmupSleepMs = 50;
constexpr int kFrameRetryMax = 3; constexpr int kFrameRetryMax = 3;
constexpr int kFrameRetrySleepMs = 25; constexpr int kFrameRetrySleepMs = 25;
constexpr int kLogEveryNFrames = 20; constexpr int kLogEveryNFrames = 1;
// Depth sample window radius (pixels):
// 1 -> 3x3, 2 -> 5x5, 3 -> 7x7.
constexpr int kDepthSampleRadiusTarget = 3;
constexpr int kDepthSampleRadiusTag = 3;
struct FramePack { struct FramePack {
cv::Mat color; cv::Mat color;
@ -45,37 +50,25 @@ struct FramePack {
cmvr::device::Rs2Intrinsics intrinsics{}; cmvr::device::Rs2Intrinsics intrinsics{};
}; };
bool sampleDepthMeters(const cv::Mat& depth, int u, int v, double& z_m, int radius_px) { bool fetchRGBFrame(const std::shared_ptr<cmvr::device::RealsenseCamera>& camera,
if (depth.empty() || depth.channels() != 1) return false; FramePack& frame,
if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) return false; std::string& error_out) {
if (radius_px < 0) return false; error_out.clear();
for (int retry = 0; retry < kFrameRetryMax; ++retry) {
std::vector<double> valid; try {
const int side = radius_px * 2 + 1; camera->getRGBImage(frame.color, frame.intrinsics);
valid.reserve(static_cast<size_t>(side * side)); if (!frame.color.empty()) {
for (int dv = -radius_px; dv <= radius_px; ++dv) { return true;
for (int du = -radius_px; du <= radius_px; ++du) {
const int uu = u + du;
const int vv = v + dv;
if (uu < 0 || vv < 0 || uu >= depth.cols || vv >= depth.rows) continue;
double z = 0.0;
if (depth.type() == CV_16UC1) {
z = static_cast<double>(depth.at<uint16_t>(vv, uu)) * 1e-3;
} else if (depth.type() == CV_32FC1) {
z = static_cast<double>(depth.at<float>(vv, uu));
} else {
return false;
}
if (std::isfinite(z) && z > 1e-4) {
valid.push_back(z);
} }
error_out = "empty_frame";
} catch (const std::exception& e) {
error_out = e.what();
}
if (retry + 1 < kFrameRetryMax) {
std::this_thread::sleep_for(std::chrono::milliseconds(kFrameRetrySleepMs));
} }
} }
if (valid.empty()) return false; return false;
std::nth_element(valid.begin(), valid.begin() + valid.size() / 2, valid.end());
z_m = valid[valid.size() / 2];
return true;
} }
bool fetchRGBDFrame(const std::shared_ptr<cmvr::device::RealsenseCamera>& camera, bool fetchRGBDFrame(const std::shared_ptr<cmvr::device::RealsenseCamera>& camera,
@ -88,7 +81,7 @@ bool fetchRGBDFrame(const std::shared_ptr<cmvr::device::RealsenseCamera>& camera
if (!frame.color.empty() && !frame.depth.empty()) { if (!frame.color.empty() && !frame.depth.empty()) {
return true; return true;
} }
error_out = "empty_frame"; error_out = "empty_rgbd_frame";
} catch (const std::exception& e) { } catch (const std::exception& e) {
error_out = e.what(); error_out = e.what();
} }
@ -120,21 +113,38 @@ bool projectPoint(const Eigen::Vector3d& p_c,
return true; return true;
} }
bool backProjectPixelToCamera(const cmvr::device::Rs2Intrinsics& K, bool sampleDepthMetersAtPixel(const cv::Mat& depth, int u, int v, double& z_m) {
int u, if (depth.empty() || depth.type() != CV_16UC1) {
int v,
double z_m,
Eigen::Vector3d& p_c) {
if (!std::isfinite(z_m) || z_m <= 1e-6) return false;
if (!std::isfinite(K.fx) || !std::isfinite(K.fy) ||
!std::isfinite(K.cx) || !std::isfinite(K.cy) ||
K.fx <= 0.0f || K.fy <= 0.0f) {
return false; return false;
} }
p_c.z() = z_m; if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) {
p_c.x() = (static_cast<double>(u) - static_cast<double>(K.cx)) / static_cast<double>(K.fx) * z_m; return false;
p_c.y() = (static_cast<double>(v) - static_cast<double>(K.cy)) / static_cast<double>(K.fy) * z_m; }
return true;
std::vector<double> vals;
vals.reserve(9);
constexpr int kRadius = 1; // 3x3
for (int yy = std::max(0, v - kRadius); yy <= std::min(depth.rows - 1, v + kRadius); ++yy) {
for (int xx = std::max(0, u - kRadius); xx <= std::min(depth.cols - 1, u + kRadius); ++xx) {
const uint16_t raw = depth.at<uint16_t>(yy, xx);
if (raw == 0) {
continue;
}
const double z = static_cast<double>(raw) * 1e-3; // mm -> m
if (!std::isfinite(z) || z <= 1e-4 || z >= 50.0) {
continue;
}
vals.push_back(z);
}
}
if (vals.empty()) {
return false;
}
const size_t mid = vals.size() / 2;
std::nth_element(vals.begin(), vals.begin() + mid, vals.end());
z_m = vals[mid];
return std::isfinite(z_m) && z_m > 0.0;
} }
void drawTagAxes(cv::Mat& img, void drawTagAxes(cv::Mat& img,
@ -170,8 +180,97 @@ void drawTagAxes(cv::Mat& img,
1); 1);
} }
struct DisplayTagPose {
int tag_id{-1};
Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
};
bool detectTagPosesForDisplay(const cv::Mat& color,
const cmvr::device::Rs2Intrinsics& intrinsics,
double tag_size_m,
std::vector<DisplayTagPose>& out_tags) {
out_tags.clear();
if (color.empty()) {
return false;
}
if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) {
return false;
}
if (!std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
!std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) {
return false;
}
cv::Mat gray;
if (color.channels() == 1) {
gray = color;
} else if (color.channels() == 3) {
cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
} else {
cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
}
if (gray.empty() || gray.type() != CV_8UC1) {
return false;
}
if (!gray.isContinuous()) {
gray = gray.clone();
}
vpCameraParameters cam;
cam.initPersProjWithoutDistortion(
static_cast<double>(intrinsics.fx),
static_cast<double>(intrinsics.fy),
static_cast<double>(intrinsics.cx),
static_cast<double>(intrinsics.cy));
vpImage<unsigned char> I(gray.rows, gray.cols);
for (int y = 0; y < gray.rows; ++y) {
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(gray.cols));
}
static vpDetectorAprilTag detector(vpDetectorAprilTag::TAG_36h11);
static bool detector_init = false;
if (!detector_init) {
detector.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
detector_init = true;
}
std::vector<vpHomogeneousMatrix> cMo_vec;
const bool detected = detector.detect(I, tag_size_m, cam, cMo_vec);
if (!detected || cMo_vec.empty()) {
return false;
}
const std::vector<int> ids = detector.getTagsId();
const size_t n = std::min(ids.size(), cMo_vec.size());
out_tags.reserve(n);
for (size_t i = 0; i < n; ++i) {
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
const auto& cMo = cMo_vec[i];
for (int r = 0; r < 3; ++r) {
for (int c = 0; c < 3; ++c) {
T(r, c) = cMo[r][c];
}
}
T(0, 3) = cMo[0][3];
T(1, 3) = cMo[1][3];
T(2, 3) = cMo[2][3];
out_tags.push_back(DisplayTagPose{ids[i], T});
}
return !out_tags.empty();
}
std::string formatVec3(const Eigen::Vector3d& p, int precision = 6) {
std::ostringstream oss;
oss << std::fixed << std::setprecision(precision)
<< "[" << p.x() << ", " << p.y() << ", " << p.z() << "]";
return oss.str();
}
} // namespace } // namespace
TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
const std::string serial = kRsSerial; const std::string serial = kRsSerial;
if (serial.empty()) { if (serial.empty()) {
@ -198,8 +297,8 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
cam_cfg.set_fps(fps); cam_cfg.set_fps(fps);
cam_cfg.set_codec("H265"); cam_cfg.set_codec("H265");
cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO); cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD); cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGB);
cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR); // cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
cam_cfg.set_buffer_size(30); cam_cfg.set_buffer_size(30);
cam_cfg.set_sync(true); cam_cfg.set_sync(true);
cam_cfg.set_enable(true); cam_cfg.set_enable(true);
@ -207,6 +306,9 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg); auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
cmvr::perception::TagRelativeTarget3D tracker(camera); cmvr::perception::TagRelativeTarget3D tracker(camera);
tracker.setTagSize(tag_size); tracker.setTagSize(tag_size);
tracker.setActiveTagSwitchPolicy(4, 1.2);
tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08);
tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
ASSERT_NO_THROW(camera->init()); ASSERT_NO_THROW(camera->init());
ASSERT_NO_THROW(camera->start()); ASSERT_NO_THROW(camera->start());
@ -238,22 +340,28 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
bool ok = false; bool ok = false;
bool user_abort = false; bool user_abort = false;
int frame_fail_count = 0; int frame_fail_count = 0;
int detect_success_count = 0; int plane_valid_count = 0;
int target_valid_count = 0;
int lock_success_count = 0; int lock_success_count = 0;
int track_success_count = 0;
int track_fail_count = 0;
int tag_switch_count = 0;
int printed_count = 0; int printed_count = 0;
bool target_uv_initialized = false;
int target_u = -1;
int target_v = -1;
bool target_locked = false;
FramePack last_good_frame; FramePack last_good_frame;
last_good_frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); last_good_frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
bool has_last_good_frame = false; bool has_last_good_frame = false;
const auto t_loop_start = std::chrono::steady_clock::now(); const auto t_loop_start = std::chrono::steady_clock::now();
// Warmup: try to get RGBD frames after pipeline start. // Warmup: try to get RGB frames after pipeline start.
bool warmup_ok = false; bool warmup_ok = false;
bool warmup_abort = false; bool warmup_abort = false;
for (int i = 0; i < kWarmupMaxTries; ++i) { for (int i = 0; i < kWarmupMaxTries; ++i) {
FramePack warm_frame; FramePack warm_frame;
std::string warm_error; std::string warm_error;
warmup_ok = fetchRGBDFrame(camera, warm_frame, warm_error); warmup_ok = fetchRGBFrame(camera, warm_frame, warm_error);
if (warmup_ok) { if (warmup_ok) {
break; break;
} }
@ -275,54 +383,93 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
FramePack frame; FramePack frame;
frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
std::string frame_error; std::string frame_error;
const bool frame_ok = fetchRGBDFrame(camera, frame, frame_error); const bool frame_ok = fetchRGBFrame(camera, frame, frame_error);
if (!frame_ok) { if (!frame_ok) {
++frame_fail_count; ++frame_fail_count;
if (has_last_good_frame) { if (has_last_good_frame) {
frame.color = last_good_frame.color.clone(); frame.color = last_good_frame.color.clone();
frame.depth = last_good_frame.depth.clone();
frame.intrinsics = last_good_frame.intrinsics; frame.intrinsics = last_good_frame.intrinsics;
} else { } else {
frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
} }
} else { } else {
last_good_frame.color = frame.color.clone(); last_good_frame.color = frame.color.clone();
last_good_frame.depth = frame.depth.clone();
last_good_frame.intrinsics = frame.intrinsics; last_good_frame.intrinsics = frame.intrinsics;
has_last_good_frame = true; has_last_good_frame = true;
} }
std::vector<DisplayTagPose> vis_tags;
const bool vis_tag_detected = detectTagPosesForDisplay(frame.color, frame.intrinsics, tag_size, vis_tags);
std::vector<cmvr::perception::TagPoseObservation> observations; if (!target_uv_initialized) {
const bool detected = tracker.detectObservationsFromImage(frame.color, frame.intrinsics, observations); target_u = (kTargetU >= 0) ? kTargetU : (frame.color.cols / 2);
const auto status = tracker.lastStatus(); target_v = (kTargetV >= 0) ? kTargetV : (frame.color.rows / 2);
const char* status_str = cmvr::perception::TagRelativeTarget3D::statusToString(status); target_uv_initialized = true;
if (detected) {
++detect_success_count;
} }
Eigen::Vector3d p_c_target = Eigen::Vector3d::Zero(); Eigen::Vector3d p_c_target = Eigen::Vector3d::Zero();
const int target_u = (kTargetU >= 0) ? kTargetU : (frame.color.cols / 2); int used_tag_id = -1;
const int target_v = (kTargetV >= 0) ? kTargetV : (frame.color.rows / 2); double spread_m = 0.0;
double z_depth_m = 0.0; bool target_valid = false;
bool target_pc_valid = false; std::string phase = target_locked ? "track" : "lock";
if (!frame.color.empty()) {
const bool depth_ok = sampleDepthMeters(frame.depth, if (!target_locked) {
target_u, if (tracker.startTrackingFromPixel(target_u, target_v)) {
target_v, p_c_target = tracker.lastTargetInCamera();
z_depth_m, used_tag_id = tracker.lastUsedTagId();
kDepthSampleRadiusTarget); spread_m = tracker.lastSpread();
if (depth_ok && std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) && target_locked = true;
std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) && phase = "track";
frame.intrinsics.fx > 1e-6f && frame.intrinsics.fy > 1e-6f) { target_valid = true;
p_c_target.z() = z_depth_m; ok = true;
p_c_target.x() = (static_cast<double>(target_u) - static_cast<double>(frame.intrinsics.cx)) ++plane_valid_count;
/ static_cast<double>(frame.intrinsics.fx) * z_depth_m; ++lock_success_count;
p_c_target.y() = (static_cast<double>(target_v) - static_cast<double>(frame.intrinsics.cy)) ++printed_count;
/ static_cast<double>(frame.intrinsics.fy) * z_depth_m;
target_pc_valid = true; std::cout << "[TagRelativeTarget3DTest][LOCK] target_p_c="
++target_valid_count; << formatVec3(p_c_target)
<< " used_tag=" << used_tag_id
<< " active_tag=" << tracker.activeTagId()
<< " spread=" << spread_m
<< "\n";
std::cout << "[TagRelativeTarget3DTest] anchors=" << tracker.anchorCount() << "\n";
} else if (((i + 1) % kLogEveryNFrames) == 0) {
std::cout << "[TagRelativeTarget3DTest] startTracking failed: "
<< cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus())
<< "\n";
}
} else {
const int old_active_tag = tracker.activeTagId();
if (tracker.track()) {
p_c_target = tracker.lastTargetInCamera();
used_tag_id = tracker.lastUsedTagId();
spread_m = tracker.lastSpread();
const bool switched = tracker.lastSwitched();
target_valid = true;
ok = true;
++track_success_count;
++printed_count;
if (switched) {
++tag_switch_count;
std::cout << "[TagRelativeTarget3DTest][SWITCH] active_tag "
<< old_active_tag << " -> " << tracker.activeTagId() << "\n";
}
std::cout << "[TagRelativeTarget3DTest][TRACK] target_p_c="
<< formatVec3(p_c_target)
<< " used_tag=" << used_tag_id
<< " active_tag=" << tracker.activeTagId()
<< " spread=" << spread_m
<< "\n";
} else {
++track_fail_count;
if (((i + 1) % kLogEveryNFrames) == 0) {
std::cout << "[TagRelativeTarget3DTest] track failed: "
<< cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus())
<< "\n";
}
} }
} }
const auto status = tracker.lastStatus();
const char* status_str = cmvr::perception::TagRelativeTarget3D::statusToString(status);
cv::Mat vis = frame.color.clone(); cv::Mat vis = frame.color.clone();
cv::drawMarker(vis, cv::drawMarker(vis,
@ -346,7 +493,11 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
cv::Scalar(0, 255, 255), cv::Scalar(0, 255, 255),
2); 2);
cv::putText(vis, cv::putText(vis,
"tags=" + std::to_string(observations.size()) + "phase=" + phase + " locked=" + std::to_string(target_locked ? 1 : 0) +
" active_tag=" + std::to_string(tracker.activeTagId()) +
" miss=" + std::to_string(tracker.activeTagMissingCount()) +
" anchors=" + std::to_string(tracker.anchorCount()) +
" vis_tags=" + std::to_string(vis_tag_detected ? vis_tags.size() : 0) +
" frame_fail=" + std::to_string(frame_fail_count), " frame_fail=" + std::to_string(frame_fail_count),
cv::Point(12, 84), cv::Point(12, 84),
cv::FONT_HERSHEY_SIMPLEX, cv::FONT_HERSHEY_SIMPLEX,
@ -363,23 +514,30 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
1); 1);
} }
cv::putText(vis, cv::putText(vis,
std::string("depth@target: ") + (target_pc_valid ? std::to_string(z_depth_m) + "m" : "invalid"), std::string("target3d: ") + (target_valid ? "valid" : "invalid") +
" used_tag=" + std::to_string(used_tag_id) +
" spread=" + std::to_string(spread_m),
cv::Point(12, 128), cv::Point(12, 128),
cv::FONT_HERSHEY_SIMPLEX, cv::FONT_HERSHEY_SIMPLEX,
0.5, 0.45,
target_pc_valid ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255), target_valid ? cv::Scalar(0, 255, 255) : cv::Scalar(0, 0, 255),
1);
cv::putText(vis,
"target_p_c=" + (target_valid ? formatVec3(p_c_target) : "invalid"),
cv::Point(12, 150),
cv::FONT_HERSHEY_SIMPLEX,
0.45,
target_valid ? cv::Scalar(0, 255, 255) : cv::Scalar(0, 0, 255),
1); 1);
// Draw detected tag axes (camera frame). if (vis_tag_detected) {
if (detected && !observations.empty()) {
const double axis_len = tag_size * 0.5; const double axis_len = tag_size * 0.5;
for (const auto& obs : observations) { for (const auto& tag : vis_tags) {
drawTagAxes(vis, obs.T_c_t, frame.intrinsics, axis_len, obs.tag_id); drawTagAxes(vis, tag.T_c_t, frame.intrinsics, axis_len, tag.tag_id);
} }
} }
// Draw target point (projected from camera coordinates). if (target_valid && std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) &&
if (std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) &&
std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) && std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) &&
p_c_target.z() > 1e-6) { p_c_target.z() > 1e-6) {
const double u = frame.intrinsics.fx * (p_c_target.x() / p_c_target.z()) + frame.intrinsics.cx; const double u = frame.intrinsics.fx * (p_c_target.x() / p_c_target.z()) + frame.intrinsics.cx;
@ -387,16 +545,17 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
const int ui = static_cast<int>(std::lround(u)); const int ui = static_cast<int>(std::lround(u));
const int vi = static_cast<int>(std::lround(v)); const int vi = static_cast<int>(std::lround(v));
if (ui >= 0 && ui < vis.cols && vi >= 0 && vi < vis.rows) { if (ui >= 0 && ui < vis.cols && vi >= 0 && vi < vis.rows) {
cv::circle(vis, cv::Point(ui, vi), 6, cv::Scalar(0, 0, 255), 2); cv::circle(vis, cv::Point(ui, vi), 6, cv::Scalar(0, 255, 255), 2);
cv::putText(vis, cv::putText(vis,
"target", "target",
cv::Point(ui + 8, vi - 8), cv::Point(ui + 8, vi + 16),
cv::FONT_HERSHEY_SIMPLEX, cv::FONT_HERSHEY_SIMPLEX,
0.5, 0.5,
cv::Scalar(0, 0, 255), cv::Scalar(0, 255, 255),
1); 1);
} }
} }
try { try {
cv::imshow(kWindowName, vis); cv::imshow(kWindowName, vis);
const int key = cv::waitKey(1); const int key = cv::waitKey(1);
@ -410,96 +569,7 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
break; break;
} }
if (!detected) { std::this_thread::sleep_for(std::chrono::milliseconds(30));
if (((i + 1) % kLogEveryNFrames) == 0) {
std::cout << "[TagRelativeTarget3DTest] detect failed: "
<< status_str
<< " (try=" << (i + 1) << "/" << tries << ")\n";
}
std::this_thread::sleep_for(std::chrono::milliseconds(30));
continue;
}
if (!observations.empty()) {
const double tag_tz = observations.front().T_c_t(2, 3);
if (((i + 1) % kLogEveryNFrames) == 0) {
std::cout << "[TagRelativeTarget3DTest] t.z=" << tag_tz
<< " depth@target=" << (target_pc_valid ? z_depth_m : -1.0)
<< " tag_id=" << observations.front().tag_id << "\n";
}
}
for (const auto& obs : observations) {
const Eigen::Vector3d p_c_tag_pose = obs.T_c_t.block<3, 1>(0, 3);
cv::Point uv_tag;
double z_tag_depth_m = 0.0;
Eigen::Vector3d p_c_tag_depth = Eigen::Vector3d::Zero();
const bool uv_ok = projectPoint(p_c_tag_pose, frame.intrinsics, uv_tag);
bool tag_depth_ok = false;
if (uv_ok) {
tag_depth_ok = sampleDepthMeters(frame.depth,
uv_tag.x,
uv_tag.y,
z_tag_depth_m,
kDepthSampleRadiusTag) &&
backProjectPixelToCamera(frame.intrinsics,
uv_tag.x,
uv_tag.y,
z_tag_depth_m,
p_c_tag_depth);
}
std::cout << "[TagRelativeTarget3DTest] tag_id=" << obs.tag_id
<< " pose_p_c=[" << p_c_tag_pose.x() << ", " << p_c_tag_pose.y() << ", " << p_c_tag_pose.z() << "]"
<< " depth_p_c="
<< (tag_depth_ok
? "[" + std::to_string(p_c_tag_depth.x()) + ", " +
std::to_string(p_c_tag_depth.y()) + ", " +
std::to_string(p_c_tag_depth.z()) + "]"
: "[invalid]")
<< " uv=[" << (uv_ok ? std::to_string(uv_tag.x) : "invalid")
<< ", " << (uv_ok ? std::to_string(uv_tag.y) : "invalid") << "]\n";
}
std::cout << "[TagRelativeTarget3DTest] target_depth_p_c="
<< (target_pc_valid
? "[" + std::to_string(p_c_target.x()) + ", " +
std::to_string(p_c_target.y()) + ", " +
std::to_string(p_c_target.z()) + "]"
: "[invalid]")
<< " target_uv=[" << target_u << ", " << target_v << "]\n";
if (!target_pc_valid) {
if (((i + 1) % kLogEveryNFrames) == 0) {
std::cout << "[TagRelativeTarget3DTest] skip lock: invalid depth at target pixel\n";
}
std::this_thread::sleep_for(std::chrono::milliseconds(30));
continue;
}
if (!tracker.lockTargetInCamera(p_c_target, observations, true)) {
if (((i + 1) % kLogEveryNFrames) == 0) {
std::cout << "[TagRelativeTarget3DTest] lock failed: "
<< cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus())
<< "\n";
}
std::this_thread::sleep_for(std::chrono::milliseconds(30));
continue;
}
++lock_success_count;
std::cout << "[TagRelativeTarget3DTest] detected_tags=" << observations.size() << "\n";
for (const auto& obs : observations) {
Eigen::Vector3d p_t = Eigen::Vector3d::Zero();
if (!tracker.getAnchorInTag(obs.tag_id, p_t)) {
continue;
}
std::cout << " tag_id=" << obs.tag_id
<< " p_t_target=[" << p_t.x() << ", " << p_t.y() << ", " << p_t.z() << "]\n";
}
++printed_count;
ok = true;
} }
try { try {
@ -514,11 +584,120 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
const auto t_loop_end = std::chrono::steady_clock::now(); const auto t_loop_end = std::chrono::steady_clock::now();
const double total_s = std::chrono::duration_cast<std::chrono::duration<double>>(t_loop_end - t_loop_start).count(); const double total_s = std::chrono::duration_cast<std::chrono::duration<double>>(t_loop_end - t_loop_start).count();
std::cout << "[TagRelativeTarget3DTest] summary " std::cout << "[TagRelativeTarget3DTest] summary "
<< "detect_ok=" << detect_success_count << " plane_valid=" << plane_valid_count
<< " target_valid=" << target_valid_count
<< " lock_ok=" << lock_success_count << " lock_ok=" << lock_success_count
<< " track_ok=" << track_success_count
<< " track_fail=" << track_fail_count
<< " switch=" << tag_switch_count
<< " printed=" << printed_count << " printed=" << printed_count
<< " frame_fail=" << frame_fail_count << " frame_fail=" << frame_fail_count
<< " duration_s=" << total_s << " duration_s=" << total_s
<< "\n"; << "\n";
} }
TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointNoDisplayMinimal) {
const std::string serial = kRsSerial;
if (serial.empty()) {
GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp";
}
cmvr::config::RealSenseCameraConfig cam_cfg;
cam_cfg.set_id("tag_relative_target_3d_test_no_display");
cam_cfg.set_serialnumber(serial);
cam_cfg.set_width(kWidth);
cam_cfg.set_height(kHeight);
cam_cfg.set_fps(kFps);
cam_cfg.set_codec("H265");
cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
cam_cfg.set_buffer_size(30);
cam_cfg.set_sync(true);
cam_cfg.set_enable(true);
auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
cmvr::perception::TagRelativeTarget3D tracker(camera);
ASSERT_NO_THROW(camera->init());
ASSERT_NO_THROW(camera->start());
struct CameraStopGuard {
std::shared_ptr<cmvr::device::RealsenseCamera> cam;
~CameraStopGuard() {
if (!cam) return;
try {
cam->stop();
} catch (...) {
}
}
} stop_guard{camera};
tracker.setTagSize(kTagSize);
tracker.setActiveTagSwitchPolicy(4, 1.2);
tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08);
// 深度法采样：5x5 中值 + MAD 离群剔除。
tracker.setDepthSamplingConfig(5, true, 2.5, 0.003);
const int tries = kTries;
const bool infinite = (tries <= 0);
int plane_fail_count = 0;
int depth_fail_count = 0;
int plane_ok_count = 0;
int depth_ok_count = 0;
int both_ok_count = 0;
int printed_count = 0;
bool got_target = false;
const int target_u = (kTargetU >= 0) ? kTargetU : (kWidth / 2);
const int target_v = (kTargetV >= 0) ? kTargetV : (kHeight / 2);
for (int i = 0; infinite || i < tries; ++i) {
// 用公开接口分别求解两种方法（内部自行抓帧/缓存）。
tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
const bool plane_ok = tracker.solveFromPixel(target_u, target_v);
const Eigen::Vector3d p_c_plane = plane_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero();
const int plane_used_tag = plane_ok ? tracker.lastUsedTagId() : -1;
const double plane_spread = plane_ok ? tracker.lastSpread() : 0.0;
if (plane_ok) {
++plane_ok_count;
} else {
++plane_fail_count;
}
tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE);
const bool depth_ok = tracker.solveFromPixel(target_u, target_v);
const Eigen::Vector3d p_c_depth = depth_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero();
if (depth_ok) {
++depth_ok_count;
} else {
++depth_fail_count;
}
if (plane_ok && depth_ok) {
++both_ok_count;
}
if (plane_ok || depth_ok) {
got_target = true;
}
std::cout << "[TagRelativeTarget3DNoDisplay][COMPARE] "
<< "uv=[" << target_u << "," << target_v << "] "
<< "plane_ok=" << plane_ok
<< " plane_p_c=" << (plane_ok ? formatVec3(p_c_plane) : "[invalid]")
<< " used_tag=" << plane_used_tag
<< " spread=" << plane_spread
<< " | depth_ok=" << depth_ok
<< " depth_p_c=" << (depth_ok ? formatVec3(p_c_depth) : "[invalid]");
if (plane_ok && depth_ok) {
std::cout << " | diff_norm=" << (p_c_plane - p_c_depth).norm();
}
std::cout << std::endl;
++printed_count;
std::this_thread::sleep_for(std::chrono::milliseconds(30));
}
EXPECT_TRUE(got_target)
<< "Failed to solve target point in camera frame. "
<< " plane_fail=" << plane_fail_count
<< " depth_fail=" << depth_fail_count
<< " plane_ok=" << plane_ok_count
<< " depth_ok=" << depth_ok_count
<< " both_ok=" << both_ok_count
<< " printed=" << printed_count;
}

17
dependency/x86/third_party/mainif/0.0.5/include/manif/Bundle.h vendored Normal file
View File

@ -0,0 +1,17 @@
#ifndef _MANIF_BUNDLE_H_
#define _MANIF_BUNDLE_H_
#include "manif/impl/macro.h"
#include "manif/impl/utils.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/bundle/Bundle_properties.h"
#include "manif/impl/bundle/Bundle_base.h"
#include "manif/impl/bundle/Bundle_map.h"
#include "manif/impl/bundle/Bundle.h"
#include "manif/impl/bundle/BundleTangent_base.h"
#include "manif/impl/bundle/BundleTangent.h"
#include "manif/impl/bundle/BundleTangent_map.h"
#endif // _MANIF_BUNDLE_H_

17
dependency/x86/third_party/mainif/0.0.5/include/manif/Rn.h vendored Normal file
View File

@ -0,0 +1,17 @@
#ifndef _MANIF_RN_H_
#define _MANIF_RN_H_
#include "manif/impl/macro.h"
#include "manif/impl/utils.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/rn/Rn_properties.h"
#include "manif/impl/rn/Rn_base.h"
#include "manif/impl/rn/RnTangent_base.h"
#include "manif/impl/rn/Rn.h"
#include "manif/impl/rn/RnTangent.h"
#include "manif/impl/rn/Rn_map.h"
#include "manif/impl/rn/RnTangent_map.h"
#endif // _MANIF_RN_H_

16
dependency/x86/third_party/mainif/0.0.5/include/manif/SE2.h vendored Normal file
View File

@ -0,0 +1,16 @@
#ifndef _MANIF_SE2_H_
#define _MANIF_SE2_H_
#include "manif/impl/macro.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/se2/SE2_properties.h"
#include "manif/impl/se2/SE2_base.h"
#include "manif/impl/se2/SE2Tangent_base.h"
#include "manif/impl/se2/SE2.h"
#include "manif/impl/se2/SE2Tangent.h"
#include "manif/impl/se2/SE2_map.h"
#include "manif/impl/se2/SE2Tangent_map.h"
#endif /* _MANIF_SE2_H_ */

16
dependency/x86/third_party/mainif/0.0.5/include/manif/SE3.h vendored Normal file
View File

@ -0,0 +1,16 @@
#ifndef _MANIF_SE3_H_
#define _MANIF_SE3_H_
#include "manif/impl/macro.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/se3/SE3_properties.h"
#include "manif/impl/se3/SE3_base.h"
#include "manif/impl/se3/SE3Tangent_base.h"
#include "manif/impl/se3/SE3.h"
#include "manif/impl/se3/SE3Tangent.h"
#include "manif/impl/se3/SE3_map.h"
#include "manif/impl/se3/SE3Tangent_map.h"
#endif /* _MANIF_SE3_H_ */

16
dependency/x86/third_party/mainif/0.0.5/include/manif/SE_2_3.h vendored Normal file
View File

@ -0,0 +1,16 @@
#ifndef _MANIF_SE_2_3_H_
#define _MANIF_SE_2_3_H_
#include "manif/impl/macro.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/se_2_3/SE_2_3_properties.h"
#include "manif/impl/se_2_3/SE_2_3_base.h"
#include "manif/impl/se_2_3/SE_2_3Tangent_base.h"
#include "manif/impl/se_2_3/SE_2_3.h"
#include "manif/impl/se_2_3/SE_2_3Tangent.h"
#include "manif/impl/se_2_3/SE_2_3_map.h"
#include "manif/impl/se_2_3/SE_2_3Tangent_map.h"
#endif /* _MANIF_SE_2_3_H_ */

16
dependency/x86/third_party/mainif/0.0.5/include/manif/SGal3.h vendored Normal file
View File

@ -0,0 +1,16 @@
#ifndef _MANIF_SGAL3_H_
#define _MANIF_SGAL3_H_
#include "manif/impl/macro.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/sgal3/SGal3_properties.h"
#include "manif/impl/sgal3/SGal3_base.h"
#include "manif/impl/sgal3/SGal3Tangent_base.h"
#include "manif/impl/sgal3/SGal3.h"
#include "manif/impl/sgal3/SGal3Tangent.h"
#include "manif/impl/sgal3/SGal3_map.h"
#include "manif/impl/sgal3/SGal3Tangent_map.h"
#endif // _MANIF_SGAL3_H_

17
dependency/x86/third_party/mainif/0.0.5/include/manif/SO2.h vendored Normal file
View File

@ -0,0 +1,17 @@
#ifndef _MANIF_SO2_H_
#define _MANIF_SO2_H_
#include "manif/impl/macro.h"
#include "manif/impl/utils.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/so2/SO2_properties.h"
#include "manif/impl/so2/SO2_base.h"
#include "manif/impl/so2/SO2Tangent_base.h"
#include "manif/impl/so2/SO2.h"
#include "manif/impl/so2/SO2Tangent.h"
#include "manif/impl/so2/SO2_map.h"
#include "manif/impl/so2/SO2Tangent_map.h"
#endif /* _MANIF_SO2_H_ */

16
dependency/x86/third_party/mainif/0.0.5/include/manif/SO3.h vendored Normal file
View File

@ -0,0 +1,16 @@
#ifndef _MANIF_SO3_H_
#define _MANIF_SO3_H_
#include "manif/impl/macro.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/so3/SO3_properties.h"
#include "manif/impl/so3/SO3_base.h"
#include "manif/impl/so3/SO3Tangent_base.h"
#include "manif/impl/so3/SO3.h"
#include "manif/impl/so3/SO3Tangent.h"
#include "manif/impl/so3/SO3_map.h"
#include "manif/impl/so3/SO3Tangent_map.h"
#endif /* _MANIF_SO3_H_ */

274
dependency/x86/third_party/mainif/0.0.5/include/manif/algorithms/average.h vendored Normal file
View File

@ -0,0 +1,274 @@
#ifndef _MANIF_MANIF_AVERAGE_H_
#define _MANIF_MANIF_AVERAGE_H_
#include "manif/impl/lie_group_base.h"
//#include "manif/interpolation.h"
#include <iostream>
namespace manif {
//template <template <typename LieGroup, typename...Args> class Container,
// typename LieGroup, typename...Args>
//LieGroup
//average_slerp(const Container<LieGroup, Args...>& mans)
//{
// if (mans.empty())
// return LieGroup();
// else if (mans.size() == 1)
// return *mans.begin();
// auto it = mans.begin();
// LieGroup carry = *it;
// ++it;
// double i = 2;
// for (; it != mans.end(); ++it, ++i)
// {
// carry = interpolate(carry, *it, (i-1.)/i);
// }
// return carry;
//}
/**
* @brief Compute an average point on Lie groups given a list of
* 'close' points.
* @param[in] points A list of 'close' points to compute an average point from.
* @param[in] eps, update norm threshold to break the iterative averaging.
* @param[in] max_iterations, max number of iterations.
* @return The average point of the input points.
*
* @note see (a)
* "Bi-invariant Means in Lie Groups.
* Application to Left-in variant Polyaffine Transformations" p. 21 Sec. 4.2
* @link ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_mean.pdf
*
* @note see also (b)
* "A globally convergent numerical algorithm for computing the center of
* mass on compact Lie groups."
* @link http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.721.8010&rep=rep1&type=pdf
*/
template <template <typename LieGroup, typename...Args> class Container,
typename LieGroup, typename...Args>
LieGroup
average_biinvariant(const Container<LieGroup, Args...>& points,
typename LieGroup::Scalar eps =
Constants<typename LieGroup::Scalar>::eps,
int max_iterations = 20)
{
using Scalar = typename LieGroup::Scalar;
using Tangent = typename LieGroup::Tangent;
MANIF_CHECK(!points.empty(), "Points container is empty !");
if (points.size() == 1)
return *points.begin();
LieGroup avg = *points.begin();
const Scalar w = Scalar(1) / Scalar(points.size());
Tangent ts;
int i=0;
for (; i<max_iterations; ++i)
{
auto it = points.begin();
const auto end = points.end();
ts.setZero();
for (; it != end; ++it)
{
// Update as in (a) & (b)
ts += ((*it) - avg);
}
ts *= w; // doing the common product by 1/N just once
//////////////
// Stopping criterion is from (b)
//////////////
if (ts.coeffs().squaredNorm() < eps)
break;
avg += ts;
//////////////
// Stopping criterion is from (a)
//////////////
// const LieGroup avg_0 = avg;
// avg += ts;
// if (avg.between(avg_0).log().coeffs().squaredNorm() < eps)
// break;
}
//std::cout << "Biinvariant stopped after " << i << " iterations.\n";
return avg;
}
template <template <typename LieGroup, typename...Args> class Container,
typename LieGroup, typename...Args>
LieGroup
average(const Container<LieGroup, Args...>& points,
typename LieGroup::Scalar eps =
Constants<typename LieGroup::Scalar>::eps,
int max_iterations = 20)
{
using Scalar = typename LieGroup::Scalar;
using Tangent = typename LieGroup::Tangent;
MANIF_CHECK(!points.empty(), "Points container is empty !");
if (points.size() == 1)
return *points.begin();
LieGroup avg = *points.begin();
const Scalar w = Scalar(1) / Scalar(points.size());
Tangent ts, tmp;
typename LieGroup::Jacobian Jr, G;
for (int i=0; i<max_iterations; ++i)
{
auto it = points.begin();
const auto end = points.end();
ts.setZero();
for (; it != end; ++it)
{
tmp = avg.between(*it).log();
// Neither (a) nor (b) use G for weighting
Jr = tmp.rjac();
G.noalias() = Jr.transpose() * Jr;
ts += G * tmp;
}
ts *= w; // doing the common product by 1/N just once
// This stopping criterion is derived from (b)
typename LieGroup::Jacobian G = ts.rjac().transpose() * ts.rjac();
const Scalar n = ts.coeffs().transpose() * G * ts.coeffs();
if (n < Constants<Scalar>::eps)
break;
avg += ts;
}
return avg;
}
// ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_mean.pdf
// page 38
// https://hal.inria.fr/hal-00938320/document#subsection.118
// page 94
template <template <typename LieGroup, typename...Args> class Container,
typename LieGroup, typename...Args>
LieGroup
average_frechet_left(const Container<LieGroup, Args...>& points,
typename LieGroup::Scalar eps =
Constants<typename LieGroup::Scalar>::eps,
int max_iterations = 20)
{
using Scalar = typename LieGroup::Scalar;
using Tangent = typename LieGroup::Tangent;
MANIF_CHECK(!points.empty(), "Points container is empty !");
if (points.size() == 1)
return *points.begin();
LieGroup avg = *points.begin();
const Scalar w = Scalar(1) / Scalar(points.size());
Tangent ts, tmp;
typename LieGroup::Jacobian Jl;
int i=0;
for (; i<max_iterations; ++i)
{
auto it = points.begin();
const auto end = points.end();
ts.setZero();
const LieGroup avg_0 = avg;
for (; it != end; ++it)
{
tmp = (*it) - avg_0; // Log( Avg^-1 . Xi )
Jl = avg_0.log().ljac(); // Jl(Avg)
ts += Jl * tmp * w;
}
// Avg = Avg . Exp( Jl^-1(Avg) . ts )
avg = avg_0 + ( avg_0.log().ljacinv() * ts );
tmp = avg_0.log().ljac() * (avg - avg_0);
if (tmp.coeffs().squaredNorm() < eps)
break;
}
//std::cout << "Frechet Left stopped after " << i << " iterations.\n";
return avg;
}
template <template <typename LieGroup, typename...Args> class Container,
typename LieGroup, typename...Args>
LieGroup
average_frechet_right(const Container<LieGroup, Args...>& points,
typename LieGroup::Scalar eps =
Constants<typename LieGroup::Scalar>::eps,
int max_iterations = 20)
{
using Scalar = typename LieGroup::Scalar;
using Tangent = typename LieGroup::Tangent;
MANIF_CHECK(!points.empty(), "Points container is empty !");
if (points.size() == 1)
return *points.begin();
LieGroup avg = *points.begin();
const Scalar w = Scalar(1) / Scalar(points.size());
Tangent ts, tmp;
typename LieGroup::Jacobian Jr;
int i=0;
for (; i<max_iterations; ++i)
{
auto it = points.begin();
const auto end = points.end();
ts.setZero();
const LieGroup avg_0 = avg;
for (; it != end; ++it)
{
tmp = it->lminus(avg_0); // Log( Xi . Avg^-1 )
Jr = avg_0.log().rjac(); // Jr(Avg)
ts += Jr * tmp * w;
}
// Avg = Exp(Jr^-1(Avg) . ts) * Avg
avg = avg_0.lplus( avg_0.log().rjacinv() * ts );
tmp = avg_0.log().rjac() * (avg.lminus(avg_0));
if (tmp.coeffs().squaredNorm() < eps)
break;
}
//std::cout << "Frechet Right stopped after " << i << " iterations.\n";
return avg;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_AVERAGE_H_ */

91
dependency/x86/third_party/mainif/0.0.5/include/manif/algorithms/bezier.h vendored Normal file
View File

@ -0,0 +1,91 @@
#ifndef _MANIF_MANIF_BEZIER_H_
#define _MANIF_MANIF_BEZIER_H_
#include "manif/impl/lie_group_base.h"
#include "manif/algorithms/interpolation.h"
#include <vector>
namespace manif {
/**
* @brief Curve fitting using the DeCasteljau algorithm
* on Lie groups.
*
* @param trajectory, a discretized trajectory.
* @param degree, the degree of smoothness of the fitted curve.
* @param k_interp, the number of points to interpolate
* between two consecutive points of the trajectory.
* interpolate k_interp for t in ]0,1].
* @param closed_curve Whether the input trajectory is closed or not.
* If true, the first and the last points of the input trajectory are used
* to interpolate points inbetween. Default false.
* @return The interpolated smooth trajectory
*
* @note A naive implementation of the DeCasteljau algorithm
* on Lie groups.
*
* @link https://www.wikiwand.com/en/De_Casteljau%27s_algorithm
*/
template <typename LieGroup>
std::vector<typename LieGroup::LieGroup>
computeBezierCurve(const std::vector<LieGroup>& control_points,
const unsigned int degree,
const unsigned int k_interp)
{
MANIF_CHECK(control_points.size() > 2, "Oups0");
MANIF_CHECK(degree <= control_points.size(), "Oups1");
MANIF_CHECK(k_interp > 0, "Oups2");
const unsigned int n_segments =
std::floor(double(control_points.size()-degree)/(degree-1)+1);
std::vector<std::vector<const LieGroup*>> segments_control_points;
for (unsigned int t=0; t<n_segments; ++t)
{
segments_control_points.emplace_back(std::vector<const LieGroup*>());
// Retrieve control points of the current segment
for (int n=0; n<degree; ++n)
{
if (verbose)
std::cout << (t*degree+n) << ", ";
segments_control_points.back().push_back( &control_points[t*(degree-1)+n] );
}
if (verbose)
std::cout << "\n";
}
const int segment_k_interp = (degree == 2) ?
k_interp : k_interp * degree;
// Actual curve fitting
std::vector<LieGroup> curve;
for (unsigned int s=0; s<segments_control_points.size(); ++s)
{
for (int t=1; t<=segment_k_interp; ++t)
{
// t in [0,1]
const double t_01 = static_cast<double>(t)/(segment_k_interp);
LieGroup Qc = LieGroup::Identity();
// recursive chunk of the algo,
// compute tmp control points.
for (int i=0; i<degree-1; ++i)
{
Qc = Qc.lplus(segments_control_points[s][i]->log() *
polynomialBernstein((double)degree, (double)i, (double)t_01));
}
curve.push_back(Qc);
}
}
return curve;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_BEZIER_H_ */

118
dependency/x86/third_party/mainif/0.0.5/include/manif/algorithms/decasteljau.h vendored Normal file
View File

@ -0,0 +1,118 @@
#ifndef _MANIF_MANIF_DECASTELJAU_H_
#define _MANIF_MANIF_DECASTELJAU_H_
#include "manif/impl/lie_group_base.h"
#include <vector>
namespace manif {
/**
* @brief Curve fitting using the DeCasteljau algorithm
* on Lie groups.
*
* @param trajectory, a discretized trajectory.
* @param degree, the degree of smoothness of the fitted curve.
* @param k_interp, the number of points to interpolate
* between two consecutive points of the trajectory.
* interpolate k_interp for t in ]0,1].
* @param closed_curve Whether the input trajectory is closed or not.
* If true, the first and the last points of the input trajectory are used
* to interpolate points inbetween. Default false.
* @return The interpolated smooth trajectory
*
* @note A naive implementation of the DeCasteljau algorithm
* on Lie groups.
*
* @link https://www.wikiwand.com/en/De_Casteljau%27s_algorithm
*/
template <typename LieGroup>
std::vector<typename LieGroup::LieGroup>
decasteljau(const std::vector<LieGroup>& trajectory,
const unsigned int degree,
const unsigned int k_interp,
const bool closed_curve = false)
{
MANIF_CHECK(trajectory.size() > 2,
"Input trajectory must have more than two points!");
MANIF_CHECK(degree <= trajectory.size(),
"Degree must be less or equal to the number of input points!");
MANIF_CHECK(k_interp > 0,
"k_interp must be greater than zero!");
// Number of connected, non-overlapping segments
const unsigned int n_segments = static_cast<unsigned int>(
std::floor(double(trajectory.size()-degree)/double((degree-1)+1))
);
std::vector<std::vector<const LieGroup*>> segments_control_points;
for (unsigned int t=0; t<n_segments; ++t)
{
segments_control_points.emplace_back(std::vector<const LieGroup*>());
// Retrieve control points of the current segment
for (unsigned int n=0; n<degree; ++n)
{
segments_control_points.back().push_back( &trajectory[t*(degree-1)+n] );
}
}
// Close the curve if there are left-over points
if (closed_curve && (n_segments*(degree-1)) <= trajectory.size()-1)
{
const unsigned int last_pts_idx = n_segments*(degree-1);
const unsigned int left_over = trajectory.size()-1-last_pts_idx;
segments_control_points.emplace_back(std::vector<const LieGroup*>());
// Get the left-over points
for (unsigned int p=last_pts_idx; p<trajectory.size(); ++p)
{
segments_control_points.back().push_back( &trajectory[p] );
}
// Add a extra points from the beginning of the trajectory
for (unsigned int p=0; p<degree-left_over-1; ++p)
{
segments_control_points.back().push_back( &trajectory[p] );
}
}
const unsigned int segment_k_interp = (degree == 2) ?
k_interp : k_interp * degree;
// Actual curve fitting
std::vector<LieGroup> curve;
for (unsigned int s=0; s<segments_control_points.size(); ++s)
{
for (unsigned int t=1; t<=segment_k_interp; ++t)
{
// t in [0,1]
const double t_01 = static_cast<double>(t)/(segment_k_interp);
std::vector<LieGroup> Qs, Qs_tmp;
for (const auto m : segments_control_points[s])
Qs.emplace_back(*m);
// recursive chunk of the algo,
// compute tmp control points.
for (unsigned int i=0; i<degree-1; ++i)
{
for (unsigned int q=0; q<Qs.size()-1; ++q)
{
Qs_tmp.push_back( Qs[q].rplus(Qs[q+1].rminus(Qs[q]) * t_01) );
}
Qs = Qs_tmp;
Qs_tmp.clear();
}
curve.push_back(Qs[0]);
}
}
return curve;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_DECASTELJAU_H_ */

325
dependency/x86/third_party/mainif/0.0.5/include/manif/algorithms/interpolation.h vendored Normal file
View File

@ -0,0 +1,325 @@
#ifndef _MANIF_MANIF_INTERPOLATION_H_
#define _MANIF_MANIF_INTERPOLATION_H_
#include "manif/impl/lie_group_base.h"
namespace manif {
/**
* @brief Constexpr function to compute binomial coefficient.
*/
template <typename T>
constexpr T binomial_coefficient(const T n, const T k)
{
return (n >= k) ? (k >= 0) ?
(k*2 > n) ? binomial_coefficient(n, n-k) :
k ? binomial_coefficient(n, k - 1) * (n - k + 1) / k : 1
// assert n ≥ k ≥ 0
: (throw std::logic_error("k >= 0 !")) : (throw std::logic_error("n >= k !"));
}
/**
* @brief Constexpr function to compute power.
*/
template <typename T>
constexpr T ipow(const T base, const int exp, T carry = 1) {
return exp < 1 ? carry : ipow(base*base, exp/2, (exp % 2) ? carry*base : carry);
}
/**
* @brief Constexpr function to compute the Bernstein polynomial
*/
template <typename T>
constexpr T polynomialBernstein(const T n, const T i, const T t)
{
return binomial_coefficient(n, i) * ipow(T(1)-t, n-i) * ipow(t,i);
}
/**
* @brief
*/
template <typename T>
T smoothing_phi(const T t, const std::size_t degree)
{
// if (degree < 5)
// {
const T t2 = t*t;
const T t3 = t2*t;
const T t4 = t3*t;
const T t5 = t4*t;
const T t6 = t5*t;
const T t7 = t6*t;
const T t8 = t7*t;
const T t9 = t8*t;
return degree == 1 ? (T(3.) *t2 - T(2.) *t3) :
degree == 2 ? (T(10.) *t3 - T(15.) *t4 + T(6.) *t5) :
degree == 3 ? (T(35.) *t4 - T(84.) *t5 + T(70.) *t6 - T(20.) *t7) :
degree == 4 ? (T(126.)*t5 - T(420.)*t6 + T(540.)*t7 - T(315.)*t8 + T(70.)*t9):
(throw std::logic_error("Not implemented yet !"));
// }
// T sum = 0;
// T sum_gamma = 0;
// for (std::size_t i=0; i<=degree; ++i)
// {
// const T am = (i % 2 == 0? T(1.) : T(-1.)) * binomial_coefficient(degree, i);
// sum_gamma += (am / (degree + 1. + i));
// sum += ((am / (degree + 1. + i)) * ipow(t, degree + 1. + i));
// }
// return (double(1) / sum_gamma) * sum;
}
/**
* @brief Slerp interpolation.
* @detail Interpolate a point mc between ma and mb at t in [0,1].
* mc=ma if t=0
* mc=mb if t=1
* @param[in] ma Initial point.
* @param[in] mb Final Point.
* @param[in] t Time at which to interpolate in [0,1].
* @param[in] -optional- J_mc_ma Jacobian of the interpolated point wrt ma.
* @param[in] -optional- J_mc_mb Jacobian of the interpolated point wrt mb.
*/
template <typename _Derived, typename _Scalar>
static typename LieGroupBase<_Derived>::LieGroup
interpolate_slerp(const LieGroupBase<_Derived>& ma,
const LieGroupBase<_Derived>& mb,
const _Scalar t/*,
typename LieGroupBase<_Derived>::OptJacobianRef J_mc_ma =
LieGroupBase<_Derived>::_,
typename LieGroupBase<_Derived>::OptJacobianRef J_mc_mb =
LieGroupBase<_Derived>::_*/)
{
MANIF_CHECK(t >= _Scalar(0) && t <= _Scalar(1),
"s must be be in [0, 1].");
using LieGroup = typename LieGroupBase<_Derived>::LieGroup;
// using Jacobian = typename LieGroupBase<_Derived>::Jacobian;
LieGroup mc;
const auto _ = LieGroupBase<_Derived>::_;
/// @todo optimize this
// if (J_mc_ma && J_mc_mb)
// {
// Jacobian J_rmin_ma, J_rmin_mb;
// Jacobian p1J_mc_ma;
// Jacobian J_mc_rmin;
// mc = ma.rplus( mb.rminus(ma, J_rmin_mb, J_rmin_ma) * t, p1J_mc_ma, J_mc_rmin);
// (*J_mc_ma) = p1J_mc_ma + J_mc_rmin * (J_rmin_ma * t);
// (*J_mc_mb) = J_mc_rmin * (J_rmin_mb * t);
// }
// else if (J_mc_ma)
// {
// Jacobian J_rmin_ma, p1J_mc_ma;
// Jacobian J_mc_rmin;
// mc = ma.rplus( mb.rminus(ma, _, J_rmin_ma) * t, p1J_mc_ma, J_mc_rmin);
// (*J_mc_ma) = p1J_mc_ma + J_mc_rmin * (J_rmin_ma * t);
// }
// else if (J_mc_mb)
// {
// Jacobian J_rmin_mb, J_mc_rmin;
// mc = ma.rplus( mb.rminus(ma, J_rmin_mb, _) * t, _, J_mc_rmin);
// (*J_mc_mb) = J_mc_rmin * (J_rmin_mb * t);
// }
// else
{
mc = ma.rplus( mb.rminus(ma) * t );
}
return mc;
}
/**
* @brief Cubic interpolation.
* @detail Interpolate a point mc between ma and mb at t in [0,1].
* mc=ma if t=0
* mc=mb if t=1
* @param[in] ma Initial point.
* @param[in] mb Final Point.
* @param[in] t Time at which to interpolate in [0,1].
* @param[in] -optional- ta.
* @param[in] -optional- tb.
* @param[out] -optional- J_mc_ma Jacobian of the interpolated point wrt ma.
* @param[out] -optional- J_mc_mb Jacobian of the interpolated point wrt mb.
*/
template <typename _Derived, typename _Scalar>
static typename LieGroupBase<_Derived>::LieGroup
interpolate_cubic(const LieGroupBase<_Derived>& ma,
const LieGroupBase<_Derived>& mb,
const _Scalar t,
const typename LieGroupBase<_Derived>::Tangent& ta =
LieGroupBase<_Derived>::Tangent::Zero(),
const typename LieGroupBase<_Derived>::Tangent& tb =
LieGroupBase<_Derived>::Tangent::Zero()/*,
typename LieGroupBase<_Derived>::OptJacobianRef J_mc_ma =
LieGroupBase<_Derived>::_,
typename LieGroupBase<_Derived>::OptJacobianRef J_mc_mb =
LieGroupBase<_Derived>::_*/)
{
using Scalar = typename LieGroupBase<_Derived>::Scalar;
using LieGroup = typename LieGroupBase<_Derived>::LieGroup;
// using Jacobian = typename LieGroupBase<_Derived>::Jacobian;
Scalar interp_factor(t);
MANIF_CHECK(interp_factor >= Scalar(0) && interp_factor <= Scalar(1),
"s must be be in [0, 1].");
const Scalar t2 = t*t;
const Scalar t3 = t2*t;
LieGroup mc;
// /// @todo optimize this
// if (J_mc_ma && J_mc_mb)
// {
// /// @todo
// }
// else if (J_mc_ma)
// {
// /// @todo
// }
// else if (J_mc_mb)
// {
// /// @todo
// }
// else
{
const auto tab = mb.rminus(ma);
// const auto tba = ma.rminus(mb);
const Scalar h00 = Scalar(2)*t3 - Scalar(3)*t2 + Scalar(1);
const Scalar h01 = -Scalar(2)*t3 + Scalar(3)*t2;
const Scalar h10 = t3 - Scalar(2)*t2 + t;
const Scalar h11 = t3 - t2;
const auto l = ma.rplus(tab*h00).rplus(ta*h10);
const auto r = mb.rplus(tab*(-h01)).rplus(tb*h11);
const auto B = l.rminus(r);
mc = r.rplus(B);
}
return mc;
}
/**
* @brief Smooth interpolation.
* @detail Interpolate a point mc between ma and mb at t in [0,1].
* mc=ma if t=0
* mc=mb if t=1
* @param[in] ma Initial point.
* @param[in] mb Final Point.
* @param[in] t Time at which to interpolate in [0,1].
* @param[in] -optional- ta.
* @param[in] -optional- tb.
* @param[out] -optional- J_mc_ma Jacobian of the interpolated point wrt ma.
* @param[out] -optional- J_mc_mb Jacobian of the interpolated point wrt mb.
*
* @note "A two-step algorithm of smooth spline
* generation on Riemannian manifolds",
* Janusz Jakubiak and Fátima Silva Leite and Rui C. Rodrigues.
*/
template <typename _Derived, typename _Scalar>
static typename LieGroupBase<_Derived>::LieGroup
interpolate_smooth(const LieGroupBase<_Derived>& ma,
const LieGroupBase<_Derived>& mb,
const _Scalar t,
const unsigned int m,
const typename LieGroupBase<_Derived>::Tangent& ta =
LieGroupBase<_Derived>::Tangent::Zero(),
const typename LieGroupBase<_Derived>::Tangent& tb =
LieGroupBase<_Derived>::Tangent::Zero()/*,
typename LieGroupBase<_Derived>::OptJacobianRef J_mc_ma = LieGroupBase<_Derived>::_,
typename LieGroupBase<_Derived>::OptJacobianRef J_mc_mb = LieGroupBase<_Derived>::_*/)
{
using Scalar = typename LieGroupBase<_Derived>::Scalar;
// using LieGroup = typename LieGroupBase<_Derived>::LieGroup;
// using Jacobian = typename LieGroupBase<_Derived>::Jacobian;
MANIF_CHECK(m >= Scalar(1), "m >= 1 !");
Scalar interp_factor(t);
MANIF_CHECK(interp_factor >= Scalar(0) && interp_factor <= Scalar(1),
"s must be be in [0, 1].");
const auto phi = smoothing_phi(t, m);
// with lplus
// const auto r = mb.lplus(tb*(t-Scalar(1)));
// const auto l = ma.lplus(ta*t);
// with rplus
const auto r = mb.rplus(tb*(t-Scalar(1)));
const auto l = ma.rplus(ta*t);
const auto B = r.lminus(l);
return l.lplus(B*phi);
}
enum class INTERP_METHOD
{
SLERP,
CUBIC,
CNSMOOTH,
};
/**
* @brief A helper function for interpolation.
* @see interpolate_slerp.
* @see interpolate_cubic.
* @see interpolate_smooth.
*/
template <typename _Derived, typename _Scalar>
typename LieGroupBase<_Derived>::LieGroup
interpolate(const LieGroupBase<_Derived>& ma,
const LieGroupBase<_Derived>& mb,
const _Scalar t,
const INTERP_METHOD method = INTERP_METHOD::SLERP,
const typename LieGroupBase<_Derived>::Tangent& ta =
LieGroupBase<_Derived>::Tangent::Zero(),
const typename LieGroupBase<_Derived>::Tangent& tb =
LieGroupBase<_Derived>::Tangent::Zero()/*,
typename LieGroupBase<_Derived>::OptJacobianRef J_mc_ma =
LieGroupBase<_Derived>::_,
typename LieGroupBase<_Derived>::OptJacobianRef J_mc_mb =
LieGroupBase<_Derived>::_*/)
{
switch (method) {
case INTERP_METHOD::SLERP:
return interpolate_slerp(ma, mb, t/*, J_mc_ma, J_mc_mb*/);
case INTERP_METHOD::CUBIC:
return interpolate_cubic(ma, mb, t,
ta, tb/*,
J_mc_ma, J_mc_mb*/);
case INTERP_METHOD::CNSMOOTH:
return interpolate_smooth(ma, mb, t, 3,
ta, tb/*,
J_mc_ma, J_mc_mb*/);
default:
MANIF_THROW("Unknown interpolation method!");
break;
}
return typename LieGroupBase<_Derived>::LieGroup();
}
} /* namespace manif */
#endif /* _MANIF_MANIF_INTERPOLATION_H_ */

54
dependency/x86/third_party/mainif/0.0.5/include/manif/autodiff/autodiff.h vendored Normal file
View File

@ -0,0 +1,54 @@
#ifndef _MANIF_MANIF_AUTODIFF_AUTODIFF_H_
#define _MANIF_MANIF_AUTODIFF_AUTODIFF_H_
#include "manif/autodiff/constants.h"
#include "manif/autodiff/local_parameterization.h"
namespace manif::internal {
// @note: Unfortunately HigherOrderDual is a non-deducible context
// template <size_t N, typename T>
// struct is_ad<autodiff::HigherOrderDual<N, T>> : std::integral_constant<bool, true> { };
using dual0thf = autodiff::HigherOrderDual<0, float>;
using dual1stf = autodiff::HigherOrderDual<1, float>;
using dual2ndf = autodiff::HigherOrderDual<2, float>;
using dual3rdf = autodiff::HigherOrderDual<3, float>;
using dual4thf = autodiff::HigherOrderDual<4, float>;
template <> struct is_ad<autodiff::dual0th> : std::integral_constant<bool, true> { };
template <> struct is_ad<autodiff::dual1st> : std::integral_constant<bool, true> { };
template <> struct is_ad<autodiff::dual2nd> : std::integral_constant<bool, true> { };
template <> struct is_ad<autodiff::dual3rd> : std::integral_constant<bool, true> { };
template <> struct is_ad<autodiff::dual4th> : std::integral_constant<bool, true> { };
template <> struct is_ad<dual0thf> : std::integral_constant<bool, true> { };
template <> struct is_ad<dual1stf> : std::integral_constant<bool, true> { };
template <> struct is_ad<dual2ndf> : std::integral_constant<bool, true> { };
template <> struct is_ad<dual3rdf> : std::integral_constant<bool, true> { };
template <> struct is_ad<dual4thf> : std::integral_constant<bool, true> { };
template <size_t N, typename T>
struct is_ad<autodiff::Real<N, T>> : std::integral_constant<bool, true> { };
} // namespace manif
namespace autodiff::detail {
/// @brief VectorTraits specialization for Derived of LieGroupBase
template <typename T>
struct VectorTraits<T, EnableIf<manif::internal::is_manif_group<T>::value>> {
using ValueType = typename T::Scalar;
template <typename NewValueType>
using ReplaceValueType = typename T::template LieGroupTemplate<NewValueType>;
};
/// @brief VectorTraits specialization for Derived of TangentBase
template <typename T>
struct VectorTraits<T, EnableIf<manif::internal::is_manif_tangent<T>::value>> {
using ValueType = typename T::Scalar;
template <typename NewValueType>
using ReplaceValueType = typename T::template TangentTemplate<NewValueType>;
};
} // namespace autodiff::detail
#endif // _MANIF_MANIF_AUTODIFF_AUTODIFF_H_

42
dependency/x86/third_party/mainif/0.0.5/include/manif/autodiff/constants.h vendored Normal file
View File

@ -0,0 +1,42 @@
#ifndef _MANIF_MANIF_AUTODIFF_CONSTANTS_H_
#define _MANIF_MANIF_AUTODIFF_CONSTANTS_H_
namespace {
// size_t without includes.
using size_type = decltype(alignof(char));
}
namespace autodiff {
namespace detail {
template <typename T, typename G> struct Dual;
template <size_type N, typename T> class Real;
} // namespace detail
} // namespace autodiff
namespace manif {
/// @brief Specialize Constants traits for autodiff::Dual type
template <typename Scalar, typename G>
struct Constants<autodiff::detail::Dual<Scalar, G>> {
static const autodiff::detail::Dual<Scalar, G> eps;
};
template <typename Scalar, typename G>
const autodiff::detail::Dual<Scalar, G>
Constants<autodiff::detail::Dual<Scalar, G>>::eps =
autodiff::detail::Dual<Scalar, G>(Constants<Scalar>::eps);
/// @brief Specialize Constants traits for autodiff::Real type
template <size_type N, typename T>
struct Constants<autodiff::detail::Real<N, T>> {
static const autodiff::detail::Real<N, T> eps;
};
template <size_type N, typename T>
const autodiff::detail::Real<N, T>
Constants<autodiff::detail::Real<N, T>>::eps =
autodiff::detail::Real<N, T>(Constants<T>::eps);
} // namespace manif
#endif // _MANIF_MANIF_AUTODIFF_CONSTANTS_H_

37
dependency/x86/third_party/mainif/0.0.5/include/manif/autodiff/local_parameterization.h vendored Normal file
View File

@ -0,0 +1,37 @@
#ifndef _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_
#define _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_
namespace manif {
template <typename Ad, typename Derived>
Eigen::Matrix<typename Derived::Scalar, Derived::RepSize, Derived::DoF>
autodiffLocalParameterizationJacobian(const manif::LieGroupBase<Derived>& _state) {
using Scalar = typename Derived::Scalar;
using LieGroup = typename Derived::template LieGroupTemplate<Ad>;
using Tangent = typename Derived::Tangent::template TangentTemplate<Ad>;
using Jac = Eigen::Matrix<Scalar, Derived::RepSize, Derived::DoF>;
LieGroup state = _state.template cast<Ad>();
Tangent delta = Tangent::Zero();
LieGroup state_plus_delta;
auto f = [](const auto& s, const auto& t){
return s + t;
};
Jac J_so_t = autodiff::jacobian(
f, autodiff::wrt(delta), autodiff::at(state, delta), state_plus_delta
);
MANIF_ASSERT(state.isApprox(state_plus_delta));
MANIF_ASSERT(Derived::RepSize == J_so_t.rows());
MANIF_ASSERT(Derived::DoF == J_so_t.cols());
return J_so_t;
}
} // namespace manif
#endif // _MANIF_MANIF_AUTODIFF_LOCAL_PARAMETRIZATION_H_

81
dependency/x86/third_party/mainif/0.0.5/include/manif/ceres/ceres.h vendored Normal file
View File

@ -0,0 +1,81 @@
#ifndef _MANIF_MANIF_CERES_CERES_H_
#define _MANIF_MANIF_CERES_CERES_H_
#include "manif/ceres/constants.h"
#include "manif/ceres/constraint.h"
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
#include "manif/ceres/manifold.h"
#else
#include "manif/ceres/local_parametrization.h"
#endif
#include "manif/ceres/objective.h"
#include "manif/ceres/ceres_utils.h"
namespace manif {
namespace internal {
struct YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS{};
template <typename _Scalar, int _N>
struct is_ad<ceres::Jet<_Scalar, _N>> : std::integral_constant<bool, true> { };
} /* namespace internal */
#ifdef _MANIF_MANIF_SO2_H_
using CeresConstraintSO2 = CeresConstraintFunctor<SO2d>;
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
using CeresManifoldSO2 = CeresManifoldFunctor<SO2d>;
#else
using CeresLocalParameterizationSO2 = CeresLocalParameterizationFunctor<SO2d>;
#endif
using CeresObjectiveSO2 = CeresObjectiveFunctor<SO2d>;
#else
using CeresConstraintSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
using CeresManifoldSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
using CeresObjectiveSO2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
#endif
#ifdef _MANIF_MANIF_SO3_H_
using CeresConstraintSO3 = CeresConstraintFunctor<SO3d>;
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
using CeresManifoldSO3 = CeresManifoldFunctor<SO3d>;
#else
using CeresLocalParameterizationSO3 = CeresLocalParameterizationFunctor<SO3d>;
#endif
using CeresObjectiveSO3 = CeresObjectiveFunctor<SO3d>;
#else
using CeresConstraintSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
using CeresManifoldSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
using CeresObjectiveSO3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
#endif
#ifdef _MANIF_MANIF_SE2_H_
using CeresConstraintSE2 = CeresConstraintFunctor<SE2d>;
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
using CeresManifoldSE2 = CeresManifoldFunctor<SE2d>;
#else
using CeresLocalParameterizationSE2 = CeresLocalParameterizationFunctor<SE2d>;
#endif
using CeresObjectiveSE2 = CeresObjectiveFunctor<SE2d>;
#else
using CeresConstraintSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
using CeresManifoldSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
using CeresObjectiveSE2 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
#endif
#ifdef _MANIF_MANIF_SE3_H_
using CeresConstraintSE3 = CeresConstraintFunctor<SE3d>;
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
using CeresManifoldSE3 = CeresManifoldFunctor<SE3d>;
#else
using CeresLocalParameterizationSE3 = CeresLocalParameterizationFunctor<SE3d>;
#endif
using CeresObjectiveSE3 = CeresObjectiveFunctor<SE3d>;
#else
using CeresConstraintSE3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
using CeresManifoldSE3 = internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
using CeresObjectiveSE3= internal::YOU_MUST_INCLUDE_MANIF_BEFORE_CERES_HELPER_HEADERS;
#endif
} /* namespace manif */
#endif /* _MANIF_MANIF_CERES_CERES_H_ */

90
dependency/x86/third_party/mainif/0.0.5/include/manif/ceres/ceres_utils.h vendored Normal file
View File

@ -0,0 +1,90 @@
#ifndef _MANIF_MANIF_CERES_UTILS_H_
#define _MANIF_MANIF_CERES_UTILS_H_
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
#include "manif/ceres/manifold.h"
#else
#include "manif/ceres/local_parametrization.h"
#endif
#include "manif/ceres/objective.h"
#include "manif/ceres/constraint.h"
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
#include <ceres/autodiff_manifold.h>
#else
#include <ceres/autodiff_local_parameterization.h>
#endif
#include <ceres/autodiff_cost_function.h>
namespace manif {
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 2
/**
* @brief Helper function to create a Ceres Manifold parameterization wrapper.
* @see CeresManifoldFunctor
*/
template <typename _LieGroup>
std::shared_ptr<
ceres::AutoDiffManifold<CeresManifoldFunctor<_LieGroup>,
_LieGroup::RepSize, _LieGroup::DoF>>
make_manifold_autodiff()
{
return std::make_shared<
ceres::AutoDiffManifold<
CeresManifoldFunctor<_LieGroup>, _LieGroup::RepSize, _LieGroup::DoF>>();
}
#else
/**
* @brief Helper function to create a Ceres autodiff local parameterization wrapper.
* @see CeresLocalParameterizationFunctor
*/
template <typename _LieGroup>
std::shared_ptr<
ceres::AutoDiffLocalParameterization<CeresLocalParameterizationFunctor<_LieGroup>,
_LieGroup::RepSize, _LieGroup::DoF>>
make_local_parameterization_autodiff()
{
return std::make_shared<
ceres::AutoDiffLocalParameterization<
CeresLocalParameterizationFunctor<_LieGroup>, _LieGroup::RepSize, _LieGroup::DoF>>();
}
#endif
/**
* @brief Helper function to create a Ceres autodiff objective wrapper.
* @see CeresObjectiveFunctor
*/
template <typename _LieGroup, typename... Args>
std::shared_ptr<
ceres::AutoDiffCostFunction<
CeresObjectiveFunctor<_LieGroup>, 1, _LieGroup::RepSize>>
make_objective_autodiff(Args&&... args)
{
return std::make_shared<
ceres::AutoDiffCostFunction<CeresObjectiveFunctor<_LieGroup>, 1, _LieGroup::RepSize>>(
new CeresObjectiveFunctor<_LieGroup>(std::forward<Args>(args)...)
);
}
/**
* @brief Helper function to create a Ceres autodiff constraint wrapper.
* @see CeresConstraintFunctor
*/
template <typename _LieGroup, typename... Args>
std::shared_ptr<
ceres::AutoDiffCostFunction<
CeresConstraintFunctor<_LieGroup>, _LieGroup::DoF, _LieGroup::RepSize, _LieGroup::RepSize>>
make_constraint_autodiff(Args&&... args)
{
return std::make_shared<
ceres::AutoDiffCostFunction<
CeresConstraintFunctor<_LieGroup>,
_LieGroup::DoF,
_LieGroup::RepSize,
_LieGroup::RepSize>>(
new CeresConstraintFunctor<_LieGroup>(std::forward<Args>(args)...));
}
} /* namespace manif */
#endif /* _MANIF_MANIF_CERES_UTILS_H_ */

26
dependency/x86/third_party/mainif/0.0.5/include/manif/ceres/constants.h vendored Normal file
View File

@ -0,0 +1,26 @@
#ifndef _MANIF_MANIF_CERES_CONSTANTS_H_
#define _MANIF_MANIF_CERES_CONSTANTS_H_
#include "manif/constants.h"
#include <ceres/jet.h>
#include <ceres/version.h>
namespace manif {
/// @brief Specialize Constants traits
/// for the ceres::Jet type
template <typename _Scalar, int N>
struct Constants<ceres::Jet<_Scalar, N>>
{
static const ceres::Jet<_Scalar, N> eps;
};
template <typename _Scalar, int N>
const ceres::Jet<_Scalar, N>
Constants<ceres::Jet<_Scalar, N>>::eps =
ceres::Jet<_Scalar, N>(Constants<_Scalar>::eps);
} /* namespace manif */
#endif /* _MANIF_MANIF_CERES_CONSTANTS_H_ */

147
dependency/x86/third_party/mainif/0.0.5/include/manif/ceres/constraint.h vendored Normal file
View File

@ -0,0 +1,147 @@
#ifndef _MANIF_MANIF_CERES_CONSTRAINT_H_
#define _MANIF_MANIF_CERES_CONSTRAINT_H_
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
namespace manif {
template <typename _LieGroup>
class CeresConstraintFunctor
{
using LieGroup = _LieGroup;
using Tangent = typename _LieGroup::Tangent;
template <typename _Scalar>
using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>;
template <typename _Scalar>
using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND_TYPE(Tangent)
using Covariance = Eigen::Matrix<double, LieGroup::DoF, LieGroup::DoF>;
using InformationMatrix = Covariance;
template <typename... Args>
CeresConstraintFunctor(Args&&... args)
: measurement_(std::forward<Args>(args)...)
, measurement_covariance_(Covariance::Identity())
{
computeInformationMatrix();
}
template <typename... Args>
CeresConstraintFunctor(const Tangent& measurement,
const Covariance& measurement_covariance = Covariance::Identity())
: measurement_(measurement)
, measurement_covariance_(measurement_covariance)
{
computeInformationMatrix();
}
virtual ~CeresConstraintFunctor() = default;
template<typename T>
bool operator()(const T* const past_raw,
const T* const futur_raw,
T* residuals_raw) const
{
const Eigen::Map<const LieGroupTemplate<T>> state_past(past_raw);
const Eigen::Map<const LieGroupTemplate<T>> state_future(futur_raw);
Eigen::Map<TangentTemplate<T>> residuals(residuals_raw);
/// r = m - ( future (-) past )
residuals = measurement_.template cast<T>() - (state_future - state_past);
/// r = exp( log(m)^-1 . ( past^-1 . future ) )
// residuals =
// measurement_.exp().template cast<T>()
// .between(state_past.between(state_future)).log();
residuals.coeffs() = measurement_sqrt_info_upper_.template cast<T>() * residuals.coeffs();
return true;
}
Tangent getMeasurement() const;
void setMeasurement(const Tangent& measurement);
Covariance getMeasurementCovariance() const;
void setMeasurementCovariance(const Covariance covariance);
protected:
void computeInformationMatrix();
protected:
Tangent measurement_;
Covariance measurement_covariance_;
InformationMatrix measurement_sqrt_info_upper_;
};
template <typename _LieGroup>
typename CeresConstraintFunctor<_LieGroup>::Tangent
CeresConstraintFunctor<_LieGroup>::getMeasurement() const
{
return measurement_;
}
template <typename _LieGroup>
void CeresConstraintFunctor<_LieGroup>::setMeasurement(
const Tangent& measurement)
{
measurement_ = measurement;
}
template <typename _LieGroup>
typename CeresConstraintFunctor<_LieGroup>::Covariance
CeresConstraintFunctor<_LieGroup>::getMeasurementCovariance() const
{
return measurement_covariance_;
}
template <typename _LieGroup>
void CeresConstraintFunctor<_LieGroup>::setMeasurementCovariance(
const Covariance covariance)
{
// Ensuring symmetry
measurement_covariance_ = covariance.template selfadjointView<Eigen::Upper>();
computeInformationMatrix();
}
template <typename _LieGroup>
void CeresConstraintFunctor<_LieGroup>::computeInformationMatrix()
{
// compute square root information upper matrix
// ensuring symmetry
const InformationMatrix info =
measurement_covariance_.inverse().template selfadjointView<Eigen::Upper>();
// Normal Cholesky factorization
Eigen::LLT<InformationMatrix> llt_of_info(info);
InformationMatrix R = llt_of_info.matrixU();
// Factorization not good enough
if (! info.isApprox(R.transpose() * R, 1e-6))
{
Eigen::SelfAdjointEigenSolver<InformationMatrix> es(info);
Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(1e-6);
R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
}
measurement_sqrt_info_upper_ = R;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_CERES_CONSTRAINT_H_ */

46
dependency/x86/third_party/mainif/0.0.5/include/manif/ceres/local_parametrization.h vendored Normal file
View File

@ -0,0 +1,46 @@
#ifndef _MANIF_MANIF_CERES_LOCAL_PARAMETRIZATION_H_
#define _MANIF_MANIF_CERES_LOCAL_PARAMETRIZATION_H_
#include <Eigen/Core>
namespace manif {
/**
* @brief A wrapper for Ceres autodiff local parameterization.
*/
template <typename _LieGroup>
class CeresLocalParameterizationFunctor
{
using LieGroup = _LieGroup;
using Tangent = typename _LieGroup::Tangent;
template <typename _Scalar>
using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>;
template <typename _Scalar>
using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
public:
CeresLocalParameterizationFunctor() = default;
virtual ~CeresLocalParameterizationFunctor() = default;
template<typename T>
bool operator()(const T* state_raw,
const T* delta_raw,
T* state_plus_delta_raw) const
{
const Eigen::Map<const LieGroupTemplate<T>> state(state_raw);
const Eigen::Map<const TangentTemplate<T>> delta(delta_raw);
Eigen::Map<LieGroupTemplate<T>> state_plus_delta(state_plus_delta_raw);
state_plus_delta = state + delta;
return true;
}
};
} /* namespace manif */
#endif /* _MANIF_MANIF_CERES_LOCAL_PARAMETRIZATION_H_ */

61
dependency/x86/third_party/mainif/0.0.5/include/manif/ceres/manifold.h vendored Normal file
View File

@ -0,0 +1,61 @@
#ifndef _MANIF_MANIF_CERES_MANIFOLD_H_
#define _MANIF_MANIF_CERES_MANIFOLD_H_
#include <Eigen/Core>
namespace manif {
/**
* @brief A wrapper for Ceres autodiff local parameterization.
*/
template <typename _LieGroup>
class CeresManifoldFunctor
{
using LieGroup = _LieGroup;
using Tangent = typename _LieGroup::Tangent;
template <typename _Scalar>
using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>;
template <typename _Scalar>
using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>;
public:
CeresManifoldFunctor() = default;
virtual ~CeresManifoldFunctor() = default;
template <typename T>
bool Plus(const T* state_raw,
const T* delta_raw,
T* state_plus_delta_raw) const
{
const Eigen::Map<const LieGroupTemplate<T>> state(state_raw);
const Eigen::Map<const TangentTemplate<T>> delta(delta_raw);
Eigen::Map<LieGroupTemplate<T>> state_plus_delta(state_plus_delta_raw);
state_plus_delta = state + delta;
return true;
}
template <typename T>
bool Minus(const T* y_raw,
const T* x_raw,
T* y_minus_x_raw) const
{
const Eigen::Map<const LieGroupTemplate<T>> y(y_raw);
const Eigen::Map<const LieGroupTemplate<T>> x(x_raw);
Eigen::Map<TangentTemplate<T>> y_minus_x(y_minus_x_raw);
y_minus_x = y - x;
return true;
}
};
} /* namespace manif */
#endif /* _MANIF_MANIF_CERES_MANIFOLD_H_ */

96
dependency/x86/third_party/mainif/0.0.5/include/manif/ceres/objective.h vendored Normal file
View File

@ -0,0 +1,96 @@
#ifndef _MANIF_MANIF_CERES_OBJECTIVE_H_
#define _MANIF_MANIF_CERES_OBJECTIVE_H_
#include <Eigen/Core>
namespace manif {
template <typename _LieGroup>
class CeresObjectiveFunctor
{
using LieGroup = _LieGroup;
using Tangent = typename _LieGroup::Tangent;
template <typename _Scalar>
using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND_TYPE(LieGroup)
template <typename... Args>
CeresObjectiveFunctor(Args&&... args)
: target_state_(std::forward<Args>(args)...)
{
//
}
CeresObjectiveFunctor(const LieGroup& target_state,
const double weight = 1)
: weight_(weight)
, target_state_(target_state)
{
//
}
virtual ~CeresObjectiveFunctor() = default;
template <typename T>
bool operator()(const T* const state_raw, T* residuals_raw) const
{
const Eigen::Map<const LieGroupTemplate<T>> state(state_raw);
residuals_raw[0] = (target_state_.template cast<T>() - state).
coeffs().norm() * T(weight_);
/// @todo
///
/// Jacobian G = q.rjac().transpose() * q.rjac();
/// residual = (q.coeffs().transpose() * G * q.coeffs());
///
/// or
///
/// residual = (q.coeffs().transpose() * W * q.coeffs());
// std::cout << "State:"
// << state_raw[0] << "," << state_raw[1]
// << "\n";
// std::cout << "Target:"
// << target_state_.coeffs().transpose()
// << "\n";
// std::cout << "residual: " << residuals_raw[0] << "\n";
return true;
}
LieGroup getTargetState() const;
void setTargetState(const LieGroup& target_state) const;
inline void weight(const double weight) { weight_ = weight; }
inline double weight() const noexcept { return weight_; }
protected:
double weight_ = 1;
LieGroup target_state_;
};
template <typename _LieGroup>
typename CeresObjectiveFunctor<_LieGroup>::LieGroup
CeresObjectiveFunctor<_LieGroup>::getTargetState() const
{
return target_state_;
}
template <typename _LieGroup>
void CeresObjectiveFunctor<_LieGroup>::setTargetState(
const LieGroup& target_state) const
{
target_state_ = target_state;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_CERES_OBJECTIVE_H_ */

68
dependency/x86/third_party/mainif/0.0.5/include/manif/constants.h vendored Normal file
View File

@ -0,0 +1,68 @@
#ifndef _MANIF_MANIF_CONSTANTS_H_
#define _MANIF_MANIF_CONSTANTS_H_
#include <cmath>
#include <limits>
#define MANIF_PI 3.141592653589793238462643383279502884
#define MANIF_PI_2 1.570796326794896619231321691639751442
#define MANIF_PI_4 0.785398163397448309615660845819875721
namespace manif {
namespace internal {
/**
* Constexpr Newton-Raphson iterative algorithm for the sqrt aprrox.
*/
template <typename T>
T constexpr sqrtNewtonRaphson(T x, T curr, T prev)
{
return curr == prev
? curr
: sqrtNewtonRaphson(x, T(0.5) * (curr + x / curr), curr);
}
/**
* Constexpr version of the square root
* Return value:
* - For a finite and non-negative value of "x",
* returns an approximation for the square root of "x"
* - Otherwise, returns NaN
*
* credits : https://stackoverflow.com/a/34134071/9709397
*/
template <typename T>
T constexpr csqrt(T x)
{
return x >= T(0) && x < std::numeric_limits<T>::infinity()
? sqrtNewtonRaphson(x, x, T(0))
: std::numeric_limits<T>::quiet_NaN();
}
} /* namespace internal */
/**
* @brief Traits to define some constant scalar.
*/
template <typename _Scalar>
struct Constants
{
static constexpr _Scalar eps = std::numeric_limits<_Scalar>::epsilon()*_Scalar(100);
static constexpr _Scalar eps_sqrt = internal::csqrt(eps);
static constexpr _Scalar to_rad = _Scalar(MANIF_PI / 180.0);
static constexpr _Scalar to_deg = _Scalar(180.0 / MANIF_PI);
};
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::eps;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::eps_sqrt;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::to_rad;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::to_deg;
} /* namespace manif */
#endif /* _MANIF_MANIF_CONSTANTS_H_ */

230
dependency/x86/third_party/mainif/0.0.5/include/manif/functions.h vendored Normal file
View File

@ -0,0 +1,230 @@
#ifndef _MANIF_MANIF_FUNCTIONS_H_
#define _MANIF_MANIF_FUNCTIONS_H_
#include "manif/impl/lie_group_base.h"
namespace manif {
template <typename _Derived>
const typename _Derived::DataType&
coeffs(const LieGroupBase<_Derived>& lie_group)
{
return lie_group.coeffs();
}
template <typename _Derived>
const typename _Derived::DataType&
coeffs(const TangentBase<_Derived>& tangent)
{
return tangent.coeffs();
}
template <typename _Derived>
const typename _Derived::Scalar*
data(const LieGroupBase<_Derived>& lie_group)
{
return lie_group.data();
}
template <typename _Derived>
typename _Derived::Scalar*
data(LieGroupBase<_Derived>& lie_group)
{
return lie_group.data();
}
template <typename _Derived>
const typename _Derived::Scalar*
data(const TangentBase<_Derived>& tangent)
{
return tangent.data();
}
template <typename _Derived>
typename _Derived::Scalar*
data(TangentBase<_Derived>& tangent)
{
return tangent.data();
}
template <typename _Derived>
void
identity(LieGroupBase<_Derived>& lie_group)
{
lie_group.identity();
}
template <typename _LieGroup>
_LieGroup Identity()
{
return _LieGroup::Identity();
}
template <typename _Derived>
void
zero(TangentBase<_Derived>& tangent)
{
tangent.zero();
}
template <typename _Tangent>
_Tangent Zero()
{
return _Tangent::Zero();
}
template <typename _Derived>
void
random(LieGroupBase<_Derived>& lie_group)
{
lie_group.random();
}
template <typename _Type>
_Type Random()
{
return _Type::Random();
}
template <typename _Derived>
void
random(TangentBase<_Derived>& tangent)
{
tangent.random();
}
template <typename _Derived>
typename _Derived::LieGroup
inverse(const LieGroupBase<_Derived>& lie_group,
typename _Derived::OpJacobianRef J_minv_m = {})
{
return lie_group.inverse(J_minv_m);
}
template <typename _DerivedMan, typename _DerivedTan>
typename _DerivedMan::LieGroup
rplus(const LieGroupBase<_DerivedMan>& lie_group,
const TangentBase<_DerivedTan>& tangent,
typename _DerivedMan::OpJacobianRef J_mout_m = {},
typename _DerivedMan::OpJacobianRef J_mout_t = {})
{
return lie_group.rplus(tangent, J_mout_m, J_mout_t);
}
template <typename _DerivedMan, typename _DerivedTan>
typename _DerivedMan::LieGroup
lplus(const LieGroupBase<_DerivedMan>& lie_group,
const TangentBase<_DerivedTan>& tangent,
typename _DerivedMan::OpJacobianRef J_mout_m = {},
typename _DerivedMan::OpJacobianRef J_mout_t = {})
{
return lie_group.lplus(tangent, J_mout_m, J_mout_t);
}
template <typename _DerivedMan, typename _DerivedTan>
typename _DerivedMan::LieGroup
plus(const LieGroupBase<_DerivedMan>& lie_group,
const TangentBase<_DerivedTan>& tangent,
typename _DerivedMan::OpJacobianRef J_mout_m = {},
typename _DerivedMan::OpJacobianRef J_mout_t = {})
{
return lie_group.plus(tangent, J_mout_m, J_mout_t);
}
template <typename _Derived0, typename _Derived1>
typename _Derived0::Tangent
rminus(const LieGroupBase<_Derived0>& lie_group_lhs,
const LieGroupBase<_Derived1>& lie_group_rhs,
typename _Derived0::OptJacobianRef J_t_ma = {},
typename _Derived0::OptJacobianRef J_t_mb = {})
{
return lie_group_lhs.rminus(lie_group_rhs, J_t_ma, J_t_mb);
}
template <typename _Derived0, typename _Derived1>
typename _Derived0::Tangent
lminus(const LieGroupBase<_Derived0>& lie_group_lhs,
const LieGroupBase<_Derived1>& lie_group_rhs,
typename _Derived0::OptJacobianRef J_t_ma = {},
typename _Derived0::OptJacobianRef J_t_mb = {})
{
return lie_group_lhs.lminus(lie_group_rhs, J_t_ma, J_t_mb);
}
template <typename _Derived0, typename _Derived1>
typename _Derived0::Tangent
minus(const LieGroupBase<_Derived0>& lie_group_lhs,
const LieGroupBase<_Derived1>& lie_group_rhs,
typename _Derived0::OptJacobianRef J_t_ma = {},
typename _Derived0::OptJacobianRef J_t_mb = {})
{
return lie_group_lhs.minus(lie_group_rhs, J_t_ma, J_t_mb);
}
template <typename _Derived>
MANIF_DEPRECATED
typename _Derived::Tangent
lift(const LieGroupBase<_Derived>& lie_group,
typename _Derived::OptJacobianRef J_l_m = {})
{
return lie_group.log(J_l_m);
}
template <typename _Derived>
typename _Derived::Tangent
log(const LieGroupBase<_Derived>& lie_group,
typename _Derived::OptJacobianRef J_l_m = {})
{
return lie_group.log(J_l_m);
}
template <typename _Derived>
MANIF_DEPRECATED
typename _Derived::LieGroup
retract(const TangentBase<_Derived>& tangent,
typename _Derived::OptJacobianRef J_r_t = {})
{
return tangent.exp(J_r_t);
}
template <typename _Derived>
typename _Derived::LieGroup
exp(const TangentBase<_Derived>& tangent,
typename _Derived::OptJacobianRef J_e_t = {})
{
return tangent.exp(J_e_t);
}
template <typename _Derived0, typename _Derived1>
typename _Derived0::LieGroup
compose(const LieGroupBase<_Derived0>& lie_group_lhs,
const LieGroupBase<_Derived1>& lie_group_rhs,
typename _Derived0::OptJacobianRef J_mc_ma = {},
typename _Derived0::OptJacobianRef J_mc_mb = {})
{
return lie_group_lhs.compose(lie_group_rhs, J_mc_ma, J_mc_mb);
}
template <typename _Derived0, typename _Derived1>
typename _Derived0::LieGroup
between(const LieGroupBase<_Derived0>& lie_group_lhs,
const LieGroupBase<_Derived1>& lie_group_rhs,
typename _Derived0::OptJacobianRef J_mc_ma = {},
typename _Derived0::OptJacobianRef J_mc_mb = {})
{
return lie_group_lhs.between(lie_group_rhs, J_mc_ma, J_mc_mb);
}
template <typename _Derived>
typename _Derived::Vector
act(const LieGroupBase<_Derived>& lie_group,
typename _Derived::Vector v,
typename _Derived::OptJacobianRef J_vout_m = {},
typename _Derived::OptJacobianRef J_vout_v = {})
{
return lie_group.act(v, J_vout_m, J_vout_v);
}
} /* namespace manif */
#endif /* _MANIF_MANIF_FUNCTIONS_H_ */

298
dependency/x86/third_party/mainif/0.0.5/include/manif/gtest/gtest_eigen_utils.h vendored Normal file
View File

@ -0,0 +1,298 @@
#ifndef _MANIF_MANIF_GTEST_GTEST_EIGEN_UTILS_H_
#define _MANIF_MANIF_GTEST_GTEST_EIGEN_UTILS_H_
#include <gtest/gtest.h>
namespace manif {
namespace detail {
template <int... I> struct int_sequence
{
using type = int_sequence;
using value_type = int;
static constexpr unsigned int size() noexcept { return sizeof...(I); }
};
template <class, class, int> struct range_cat;
template <int... H, int... T, int Start>
struct range_cat<int_sequence<H...>, int_sequence<T...>, Start>
{
using type = int_sequence<H..., Start+T...>;
};
template <int Start, unsigned int N>
struct range_ : range_cat< typename range_<Start, N / 2>::type,
typename range_<Start, N - N / 2>::type,
N / 2 > { };
template <int Start> struct range_<Start, 1> { using type = int_sequence<Start>; };
template <int Start> struct range_<Start, 0> { using type = int_sequence<>; };
template <int End>
using make_int_sequence = typename range_<0, End + 1>::type;
template<typename F, class T, template <int...I> class S, int... I>
void call_for_each(F f, const T& t, const S<I...>&)
{
auto l = { (f(std::get<I>(/*std::forward<T>*/(t))), 0)... };
(void)(l);
}
template<typename F, template <typename...Ts> class C, typename... Ts>
void call_for_each(F&& f, const C<Ts...>& t)
{
call_for_each(std::forward<F>(f), t, make_int_sequence<sizeof...(Ts)-1>());
}
struct RowSizeGetter {
template <typename... Ts>
auto operator()(const Eigen::MatrixBase<Ts>&... ms)
-> decltype(std::make_tuple(ms.rows()...))
{ return std::make_tuple(ms.rows()...); }
static const char* dim() { constexpr static char dim_arr[] = "row"; return dim_arr; }
};
struct ColSizeGetter {
template <typename... Ts>
auto operator()(const Eigen::MatrixBase<Ts>&... ms)
-> decltype(std::make_tuple(ms.cols()...))
{ return std::make_tuple(ms.cols()...); }
static const char* dim() { constexpr static char dim_arr[] = "col"; return dim_arr; }
};
using EigenIndex = EIGEN_DEFAULT_DENSE_INDEX_TYPE;
} /* namespace detail */
/**
* @brief Gtest predicate function for matrice same dim.
* @param dim, the expected dim size
* @param ms, N Eigen::Matrix to be tested
* @note This function requires an extra template param helper DimGetter
* @see detail::RowSizeGetter
* @see detail::ColSizeGetter
* @see isEigenMatrixDimSize
* @see isEigenMatrixColSize
*/
template <typename DimGetter, typename... Ts>
inline ::testing::AssertionResult
isEigenMatrixDimSize(const detail::EigenIndex dim,
const Eigen::MatrixBase<Ts>&... ms)
{
static_assert(sizeof...(Ts)>=1, "No matrix passed !");
const auto sizes = DimGetter()(ms...);
bool result = true;
auto f = [&result, &dim](const detail::EigenIndex i){ result &= (dim == i);};
detail::call_for_each(f, sizes);
// cppcheck-suppress knownConditionTrueFalse
if (!result)
{
std::stringstream ss;
ss << dim;
auto p = [&ss, &dim](const detail::EigenIndex i)
{ ss << ((i==dim)?" == ":" != ") << i; };
detail::call_for_each(p, sizes);
return ::testing::AssertionFailure() << "Matrice have different "
<< DimGetter::dim()
<< " size ! " << ss.str();
}
return ::testing::AssertionSuccess();
}
/**
* @brief Gtest predicate function for testing expected matrice row dim.
* @note This is an helper function for isEigenMatrixDimSize
*/
template <typename... Ts>
inline ::testing::AssertionResult
isEigenMatrixRowSize(const detail::EigenIndex rows,
const Eigen::MatrixBase<Ts>&... ms)
{
return isEigenMatrixDimSize<detail::RowSizeGetter>(rows, ms...);
}
/**
* @brief Gtest predicate function for testing expected matrice col dim.
* @note This is an helper function for isEigenMatrixDimSize
*/
template <typename... Ts>
inline ::testing::AssertionResult
isEigenMatrixColSize(const detail::EigenIndex cols,
const Eigen::MatrixBase<Ts>&... ms)
{
return isEigenMatrixDimSize<detail::ColSizeGetter>(cols, ms...);
}
/**
* @brief Gtest predicate function for testing N matrice have the same row size.
*/
template <typename Derived, typename... Ts>
inline ::testing::AssertionResult
isEigenMatrixSameRowSize(const Eigen::MatrixBase<Derived>& m0,
const Eigen::MatrixBase<Ts>&... ms)
{
static_assert(sizeof...(Ts)>=1, "Only one matrix passed !\n"
"Please consider using isEigenMatrixRowSize instead.");
return isEigenMatrixRowSize(m0.rows(), ms...);
}
/**
* @brief Gtest predicate function for testing
* N matrice have the same col size.
*/
template <typename Derived, typename... Ts>
inline ::testing::AssertionResult
isEigenMatrixSameColSize(const Eigen::MatrixBase<Derived>& m0,
const Eigen::MatrixBase<Ts>&... ms)
{
static_assert(sizeof...(Ts)>=1, "Only one matrix passed !\n"
"Please consider using isEigenMatrixColSize instead.");
return isEigenMatrixColSize(m0.cols(), ms...);
}
/**
* @brief Gtest predicate function for testing
* N matrice have the same size.
*/
template <typename Derived, typename... Ts>
inline ::testing::AssertionResult
isEigenMatrixSameSize(const Eigen::MatrixBase<Derived>& m0,
const Eigen::MatrixBase<Ts>&... ms)
{
const ::testing::AssertionResult row_check =
isEigenMatrixSameRowSize(m0, ms...);
if (!row_check)
{
return row_check;
}
const ::testing::AssertionResult col_check =
isEigenMatrixSameColSize(m0, ms...);
if (!col_check)
{
return col_check;
}
return ::testing::AssertionSuccess();
}
/**
* @brief isZero() is not very suitable for comparing vectors which have norms
* significantly larger than 0, isApprox(), on the other hand, does not work
* with small norms.
* https://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#ae8443357b808cd393be1b51974213f9c
*/
template <class _DerivedA, class _DerivedB>
inline ::testing::AssertionResult isEigenMatrixNear(
const Eigen::MatrixBase<_DerivedA>& matrix_a,
const Eigen::MatrixBase<_DerivedB>& matrix_b,
const std::string& matrix_a_name = "matrix_a",
const std::string& matrix_b_name = "matrix_b",
typename _DerivedA::Scalar tolerance = (std::is_same<typename _DerivedA::Scalar, float>::value)? 1e-6 : 1e-8
)
{
const ::testing::AssertionResult size_check =
isEigenMatrixSameSize(matrix_a, matrix_b);
if (!size_check)
{
return size_check;
}
bool result = false;
if (std::min(matrix_a.norm(), matrix_b.norm()) < tolerance)
{
result = (matrix_a - matrix_b).isZero(tolerance);
}
else
{
result = (matrix_a.isApprox(matrix_b, tolerance));
}
return (result ? ::testing::AssertionSuccess()
: ::testing::AssertionFailure()
<< matrix_a_name << " != " << matrix_b_name << "\n"
<< matrix_a_name << ":\n" << matrix_a << "\n"
<< matrix_b_name << ":\n" << matrix_b << "\n"
<< "diff:\n" << (matrix_a - matrix_b) << "\n");
}
} /* namespace manif */
#define __GET_4TH_ARG(arg1,arg2,arg3,arg4, ...) arg4
#define EXPECT_EIGEN_NEAR_DEFAULT_TOL(A,B) \
EXPECT_TRUE(manif::isEigenMatrixNear(A, B, #A, #B))
#define EXPECT_EIGEN_NEAR_TOL(A,B,tol) \
EXPECT_TRUE(manif::isEigenMatrixNear(A, B, #A, #B, tol))
#define __EXPECT_EIGEN_NEAR_CHOOSER(...) \
__GET_4TH_ARG(__VA_ARGS__, EXPECT_EIGEN_NEAR_TOL, \
EXPECT_EIGEN_NEAR_DEFAULT_TOL, )
#define EXPECT_EIGEN_NEAR(...) \
__EXPECT_EIGEN_NEAR_CHOOSER(__VA_ARGS__)(__VA_ARGS__)
#define EXPECT_EIGEN_NOT_NEAR_DEFAULT_TOL(A,B) \
EXPECT_FALSE(manif::isEigenMatrixNear(A, B, #A, #B))
#define EXPECT_EIGEN_NOT_NEAR_TOL(A,B,tol) \
EXPECT_FALSE(manif::isEigenMatrixNear(A, B, #A, #B, tol))
#define __EXPECT_EIGEN_NOT_NEAR_CHOOSER(...) \
__GET_4TH_ARG(__VA_ARGS__, EXPECT_EIGEN_NOT_NEAR_TOL, \
EXPECT_EIGEN_NOT_NEAR_DEFAULT_TOL, )
#define EXPECT_EIGEN_NOT_NEAR(...) \
__EXPECT_EIGEN_NOT_NEAR_CHOOSER(__VA_ARGS__)(__VA_ARGS__)
#define ASSERT_EIGEN_NEAR_DEFAULT_TOL(A,B) \
ASSERT_TRUE(manif::isEigenMatrixNear(A, B, #A, #B))
#define ASSERT_EIGEN_NEAR_TOL(A,B,tol) \
ASSERT_TRUE(manif::isEigenMatrixNear(A, B, #A, #B, tol))
#define __ASSERT_EIGEN_NEAR_CHOOSER(...) \
__GET_4TH_ARG(__VA_ARGS__, ASSERT_EIGEN_NEAR_TOL, \
ASSERT_EIGEN_NEAR_DEFAULT_TOL, )
#define ASSERT_EIGEN_NEAR(...) \
__ASSERT_EIGEN_NEAR_CHOOSER(__VA_ARGS__)(__VA_ARGS__)
#define ASSERT_EIGEN_NOT_NEAR_DEFAULT_TOL(A,B) \
ASSERT_FALSE(manif::isEigenMatrixNear(A, B, #A, #B))
#define ASSERT_EIGEN_NOT_NEAR_TOL(A,B,tol) \
ASSERT_FALSE(manif::isEigenMatrixNear(A, B, #A, #B, tol))
#define __ASSERT_EIGEN_NOT_NEAR_CHOOSER(...) \
__GET_4TH_ARG(__VA_ARGS__, ASSERT_EIGEN_NOT_NEAR_TOL, \
ASSERT_EIGEN_NOT_NEAR_DEFAULT_TOL, )
#define ASSERT_EIGEN_NOT_NEAR(...) \
__ASSERT_EIGEN_NOT_NEAR_CHOOSER(__VA_ARGS__)(__VA_ARGS__)
/*
* E.g
EXPECT_TRUE(isEigenMatrixSameSize(Eigen::Vector2d::Zero(),
Eigen::Vector2d::Zero(),
Eigen::Vector3d::Zero(),
Eigen::Vector4d::Zero()));
*/
#endif /* _MANIF_MANIF_GTEST_GTEST_EIGEN_UTILS_H_ */

133
dependency/x86/third_party/mainif/0.0.5/include/manif/gtest/gtest_manif_utils.h vendored Normal file
View File

@ -0,0 +1,133 @@
#ifndef _MANIF_MANIF_GTEST_GTEST_MANIF_UTILS_H_
#define _MANIF_MANIF_GTEST_GTEST_MANIF_UTILS_H_
#include "manif/impl/lie_group_base.h"
#include "manif/impl/utils.h"
#include "gtest_eigen_utils.h"
#include <random>
#include <chrono>
#define MANIF_RUN_ALL_TEST \
int main(int argc, char** argv) { \
std::srand((unsigned int) time(0)); \
testing::InitGoogleTest(&argc, argv); \
return RUN_ALL_TESTS(); \
}
#define EXPECT_ANGLE_NEAR(e, a, eps) \
EXPECT_LT(pi2pi(e-a), eps)
// https://stackoverflow.com/questions/3046889/optional-parameters-with-c-macros
#define EXPECT_MANIF_NEAR_DEFAULT_TOL(A,B) \
EXPECT_TRUE(manif::isManifNear(A, B, #A, #B))
#define EXPECT_MANIF_NEAR_TOL(A,B,tol) \
EXPECT_TRUE(manif::isManifNear(A, B, #A, #B, tol))
#define EXPECT_MANIF_NOT_NEAR_DEFAULT_TOL(A,B) \
EXPECT_FALSE(manif::isManifNear(A, B, #A, #B))
#define EXPECT_MANIF_NOT_NEAR_TOL(A,B,tol) \
EXPECT_FALSE(manif::isManifNear(A, B, #A, #B, tol))
#define __EXPECT_MANIF_NEAR_CHOOSER(...) \
__GET_4TH_ARG(__VA_ARGS__, EXPECT_MANIF_NEAR_TOL, \
EXPECT_MANIF_NEAR_DEFAULT_TOL, )
#define __EXPECT_MANIF_NOT_NEAR_CHOOSER(...) \
__GET_4TH_ARG(__VA_ARGS__, EXPECT_MANIF_NOT_NEAR_TOL, \
EXPECT_MANIF_NOT_NEAR_DEFAULT_TOL, )
#define EXPECT_MANIF_NEAR(...) \
__EXPECT_MANIF_NEAR_CHOOSER(__VA_ARGS__)(__VA_ARGS__)
#define EXPECT_MANIF_NOT_NEAR(...) \
__EXPECT_MANIF_NOT_NEAR_CHOOSER(__VA_ARGS__)(__VA_ARGS__)
#define ASSERT_MANIF_NEAR_DEFAULT_TOL(A,B) \
ASSERT_TRUE(manif::isManifNear(A, B, #A, #B))
#define ASSERT_MANIF_NEAR_TOL(A,B,tol) \
ASSERT_TRUE(manif::isManifNear(A, B, #A, #B, tol))
#define __ASSERT_MANIF_NEAR_CHOOSER(...) \
__GET_4TH_ARG(__VA_ARGS__, ASSERT_MANIF_NEAR_TOL, \
ASSERT_MANIF_NEAR_DEFAULT_TOL, )
#define ASSERT_MANIF_NEAR(...) \
__ASSERT_MANIF_NEAR_CHOOSER(__VA_ARGS__)(__VA_ARGS__)
namespace manif {
template <class _DerivedA, class _DerivedB>
inline ::testing::AssertionResult
isManifNear(const LieGroupBase<_DerivedA>& manifold_a,
const LieGroupBase<_DerivedB>& manifold_b,
const std::string& manifold_a_name = "manifold_a",
const std::string& manifold_b_name = "manifold_b",
double tolerance = 1e-5)
{
auto result =
isEigenMatrixNear(LieGroupBase<_DerivedA>::Tangent::DataType::Zero(),
(manifold_a-manifold_b).coeffs(),
"", "", tolerance);
return (result ? ::testing::AssertionSuccess()
: ::testing::AssertionFailure()
<< manifold_a_name << " != " << manifold_b_name << "\n"
<< manifold_a_name << ":\n" << manifold_a.coeffs().transpose() << "\n"
<< manifold_b_name << ":\n" << manifold_b.coeffs().transpose() << "\n"
<< "rminus:\n" << (manifold_a - manifold_b) << "\n");
}
template <class _DerivedA, class _DerivedB>
inline ::testing::AssertionResult
isManifNear(const TangentBase<_DerivedA>& tangent_a,
const TangentBase<_DerivedB>& tangent_b,
const std::string& tangent_a_name = "tangent_a",
const std::string& tangent_b_name = "tangent_b",
double tolerance = 1e-5)
{
return isEigenMatrixNear(tangent_a.coeffs(), tangent_b.coeffs(),
tangent_a_name, tangent_b_name,
tolerance);
}
template <typename _Scalar = double>
class GaussianNoiseGenerator
{
using Clock = std::chrono::system_clock;
using Scalar = _Scalar;
public:
GaussianNoiseGenerator(const Scalar mean,
const Scalar std)
: re_(Clock::now().time_since_epoch().count())
, distr_(mean, std)
{
//
}
Scalar noise()
{
return distr_(re_);
}
Scalar operator()()
{
return noise();
}
protected:
std::default_random_engine re_;
std::normal_distribution<Scalar> distr_;
};
} /* namespace manif */
#endif /* _MANIF_MANIF_GTEST_GTEST_MANIF_UTILS_H_ */

24
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/assignment_assert.h vendored Normal file
View File

@ -0,0 +1,24 @@
#ifndef _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_
#define _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_
namespace manif {
namespace internal {
template <typename Derived>
struct AssignmentEvaluatorImpl
{
template <typename T> static void run_impl(const T&) { }
};
template <typename Derived>
struct AssignmentEvaluator : AssignmentEvaluatorImpl<Derived>
{
using Base = AssignmentEvaluatorImpl<Derived>;
template <typename T> void run(T&& t) { Base::run_impl(std::forward<T>(t)); }
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_

35
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/bracket.h vendored Normal file
View File

@ -0,0 +1,35 @@
#ifndef _MANIF_MANIF_IMPL_BRACKET_H_
#define _MANIF_MANIF_IMPL_BRACKET_H_
namespace manif {
namespace internal {
template <typename Derived>
struct BracketEvaluatorImpl {
template <typename TL, typename TR>
static typename Derived::Tangent run(const TL& a, const TR& b) {
return a.smallAdj() * b;
}
};
template <typename Derived, typename DerivedOther>
struct BracketEvaluator : BracketEvaluatorImpl<Derived> {
using Base = BracketEvaluatorImpl<Derived>;
BracketEvaluator(const Derived& xptr, const DerivedOther& xptr_o)
: xptr_(xptr), xptr_o_(xptr_o) {}
typename Derived::Tangent run() {
return Base::run(xptr_, xptr_o_);
}
protected:
const Derived& xptr_;
const DerivedOther& xptr_o_;
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_IMPL_BRACKET_H_

208
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/bundle/Bundle.h vendored Normal file
View File

@ -0,0 +1,208 @@
#ifndef _MANIF_MANIF_BUNDLE_H_
#define _MANIF_MANIF_BUNDLE_H_
#include "manif/impl/bundle/Bundle_base.h"
#include "manif/impl/traits.h"
namespace manif {
// Forward declare for type traits specialization
template <typename _Scalar, template<typename> class ... _T> struct Bundle;
template <typename _Scalar, template<typename> class ... _T> struct BundleTangent;
namespace internal {
//! Traits specialization
template <typename _Scalar, template<typename> class ... _T>
struct traits<Bundle<_Scalar, _T ...>>
{
// Bundle-specific traits
static constexpr std::size_t BundleSize = sizeof...(_T);
using Elements = std::tuple<_T<_Scalar>...>;
template <int _N>
using Element = typename std::tuple_element<_N, Elements>::type;
template <int _N>
using MapElement = Eigen::Map<Element<_N>>;
template <int _N>
using MapConstElement = Eigen::Map<const Element<_N>>;
static constexpr std::array<int, sizeof...(_T)> DimIdx = compute_indices<_T<_Scalar>::Dim ...>();
static constexpr std::array<int, sizeof...(_T)> DoFIdx = compute_indices<_T<_Scalar>::DoF ...>();
static constexpr std::array<int, sizeof...(_T)> RepSizeIdx = compute_indices<_T<_Scalar>::RepSize ...>();
static constexpr std::array<int, sizeof...(_T)> TraIdx = compute_indices<_T<_Scalar>::Transformation::RowsAtCompileTime ...>();
// Regular traits
using Scalar = _Scalar;
using LieGroup = Bundle<_Scalar, _T ...>;
using Tangent = BundleTangent<_Scalar, _T ...>;
using Base = BundleBase<Bundle<_Scalar, _T ...>>;
static constexpr int Dim = accumulate(int(_T<_Scalar>::Dim) ...);
static constexpr int DoF = accumulate(int(_T<_Scalar>::DoF) ...);
static constexpr int RepSize = accumulate(int(_T<_Scalar>::RepSize) ...);
using DataType = Eigen::Matrix<_Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<_Scalar, DoF, DoF>;
using Transformation = SquareMatrix<
_Scalar,
accumulate(int(_T<_Scalar>::Transformation::RowsAtCompileTime) ...)
>;
using Vector = Eigen::Matrix<_Scalar, Dim, 1>;
};
template <typename _Scalar, template<typename> class ... _T>
const constexpr std::array<int, sizeof...(_T)> traits<Bundle<_Scalar, _T ...>>::DimIdx;
template <typename _Scalar, template<typename> class ... _T>
const constexpr std::array<int, sizeof...(_T)> traits<Bundle<_Scalar, _T ...>>::DoFIdx;
template <typename _Scalar, template<typename> class ... _T>
const constexpr std::array<int, sizeof...(_T)> traits<Bundle<_Scalar, _T ...>>::RepSizeIdx;
template <typename _Scalar, template<typename> class ... _T>
const constexpr std::array<int, sizeof...(_T)> traits<Bundle<_Scalar, _T ...>>::TraIdx;
template <typename _Scalar, template<typename> class ... _T>
const constexpr int traits<Bundle<_Scalar, _T ...>>::Dim;
template <typename _Scalar, template<typename> class ... _T>
const constexpr int traits<Bundle<_Scalar, _T ...>>::DoF;
template <typename _Scalar, template<typename> class ... _T>
const constexpr int traits<Bundle<_Scalar, _T ...>>::RepSize;
} // namespace internal
//
// Bundle LieGroup
//
/**
* @brief Represents a Bundle (or Composite) element as
* described in Section IV of the reference paper
* (see also Example 7).
*
* A Bundle <G1, ..., Gn> of Lie groups can be utilized as
* a single group with element-wise operations. This can be
* convenient when working with aggregate states that consist of
* multiple Lie group sub-states, like the example in Section VIIb
* of the reference paper.
*
* Example: create an element of the composite <SO3, E3, E3>
* using double as the scalar type.
*
* > Bundle<double, SO3, R3, R3> element;
*/
template<typename _Scalar, template<typename> class ... _T>
struct Bundle : BundleBase<Bundle<_Scalar, _T ...>>
{
private:
static_assert(sizeof...(_T) > 0, "Must have at least one element in Bundle !");
using Base = BundleBase<Bundle<_Scalar, _T...>>;
using Type = Bundle<_Scalar, _T...>;
protected:
using Base::derived;
public:
template <int Idx> using Element = typename Base::template Element<Idx>;
using Base::BundleSize;
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
Bundle() = default;
~Bundle() = default;
MANIF_COPY_CONSTRUCTOR(Bundle)
MANIF_MOVE_CONSTRUCTOR(Bundle)
// Copy constructor
template<typename _DerivedOther>
Bundle(const LieGroupBase<_DerivedOther> & o);
MANIF_GROUP_ASSIGN_OP(Bundle)
// LieGroup common API
/**
* @brief Get a reference to the underlying DataType.
* @param[out] a reference to the underlying Eigen vector
*/
DataType & coeffs();
/**
* @brief Get a const reference to the underlying DataType.
* @param[out] a const reference to the underlying Eigen vector
*/
const DataType & coeffs() const;
// Bundle specific API
/**
* @brief Construct from Bundle elements
*/
Bundle(const _T<_Scalar> & ... elements);
protected:
// Helper for the elements constructor
template <int ... _Idx>
Bundle(internal::intseq<_Idx...>, const _T<_Scalar> & ... elements);
protected:
//! Underlying data (Eigen) vector
DataType data_;
};
template<typename _Scalar, template<typename> class ... _T>
template<typename _DerivedOther>
Bundle<_Scalar, _T...>::Bundle(const LieGroupBase<_DerivedOther> & o)
: Bundle(o.coeffs())
{}
template<typename _Scalar, template<typename> class ... _T>
Bundle<_Scalar, _T...>::Bundle(const _T<_Scalar> & ... elements)
: Bundle(internal::make_intseq_t<BundleSize>{}, elements ...)
{}
template<typename _Scalar, template<typename> class ... _T>
template<int ... _Idx>
Bundle<_Scalar, _T...>::Bundle(
internal::intseq<_Idx...>, const _T<_Scalar> & ... elements
)
{
// c++11 "fold expression"
auto l = {((data_.template segment<Element<_Idx>::RepSize>(
std::get<_Idx>(internal::traits<Type>::RepSizeIdx)
) = elements.coeffs()), 0) ...};
static_cast<void>(l); // compiler warning
}
template<typename _Scalar, template<typename> class ... _T>
typename Bundle<_Scalar, _T...>::DataType &
Bundle<_Scalar, _T...>::coeffs()
{
return data_;
}
template<typename _Scalar, template<typename> class ... _T>
const typename Bundle<_Scalar, _T...>::DataType &
Bundle<_Scalar, _T...>::coeffs() const
{
return data_;
}
} // namespace manif
#endif // _MANIF_MANIF_BUNDLE_H_

191
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/bundle/BundleTangent.h vendored Normal file
View File

@ -0,0 +1,191 @@
#ifndef _MANIF_MANIF_BUNDLETANGENT_H_
#define _MANIF_MANIF_BUNDLETANGENT_H_
#include "manif/impl/bundle/BundleTangent_base.h"
#include "manif/impl/traits.h"
namespace manif {
// Forward declare for type traits specialization
template<typename _Scalar, template<typename> class ... _T> struct Bundle;
template<typename _Scalar, template<typename> class ... _T> struct BundleTangent;
namespace internal {
//! Traits specialization
template<typename _Scalar, template<typename> class ... _T>
struct traits<BundleTangent<_Scalar, _T...>>
{
// BundleTangent-specific traits
static constexpr std::size_t BundleSize = sizeof...(_T);
using Elements = std::tuple<typename _T<_Scalar>::Tangent...>;
template <int _N>
using Element = typename std::tuple_element<_N, Elements>::type;
template <int _N>
using MapElement = Eigen::Map<Element<_N>>;
template <int _N>
using MapConstElement = Eigen::Map<const Element<_N>>;
static constexpr std::array<int, sizeof...(_T)> DoFIdx = compute_indices<_T<_Scalar>::Tangent::DoF ...>();
static constexpr std::array<int, sizeof...(_T)> RepSizeIdx = compute_indices<_T<_Scalar>::Tangent::RepSize ...>();
static constexpr std::array<int, sizeof...(_T)> AlgIdx = compute_indices<_T<_Scalar>::Tangent::LieAlg::RowsAtCompileTime ...>();
// Regular traits
using Scalar = _Scalar;
using LieGroup = Bundle<_Scalar, _T...>;
using Tangent = BundleTangent<_Scalar, _T...>;
using Base = BundleTangentBase<Tangent>;
static constexpr int Dim = accumulate(int(_T<_Scalar>::Tangent::Dim) ...);
static constexpr int DoF = accumulate(int(_T<_Scalar>::Tangent::DoF) ...);
static constexpr int RepSize = accumulate(int(_T<_Scalar>::Tangent::RepSize) ...);
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using LieAlg = SquareMatrix<
Scalar,
accumulate(int(_T<_Scalar>::Tangent::LieAlg::RowsAtCompileTime) ...)
>;
};
template <typename _Scalar, template<typename> class ... _T>
const constexpr std::array<int, sizeof...(_T)> traits<BundleTangent<_Scalar, _T ...>>::DoFIdx;
template <typename _Scalar, template<typename> class ... _T>
const constexpr std::array<int, sizeof...(_T)> traits<BundleTangent<_Scalar, _T ...>>::RepSizeIdx;
template <typename _Scalar, template<typename> class ... _T>
const constexpr std::array<int, sizeof...(_T)> traits<BundleTangent<_Scalar, _T ...>>::AlgIdx;
template <typename _Scalar, template<typename> class ... _T>
const constexpr int traits<BundleTangent<_Scalar, _T ...>>::Dim;
template <typename _Scalar, template<typename> class ... _T>
const constexpr int traits<BundleTangent<_Scalar, _T ...>>::DoF;
template <typename _Scalar, template<typename> class ... _T>
const constexpr int traits<BundleTangent<_Scalar, _T ...>>::RepSize;
} // namespace internal
//
// BundleTangent
//
/**
* @brief Represents a BundleTangent element.
*/
template<typename _Scalar, template<typename> class ... _T>
struct BundleTangent : BundleTangentBase<BundleTangent<_Scalar, _T...>>
{
private:
static_assert(sizeof...(_T) > 0, "Must have at least one element in BundleTangent !");
using Base = BundleTangentBase<BundleTangent<_Scalar, _T...>>;
using Type = BundleTangent<_Scalar, _T...>;
protected:
using Base::derived;
public:
template <int Idx> using Element = typename Base::template Element<Idx>;
using Base::BundleSize;
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
BundleTangent() = default;
~BundleTangent() = default;
MANIF_COPY_CONSTRUCTOR(BundleTangent)
MANIF_MOVE_CONSTRUCTOR(BundleTangent)
// Copy constructors given base
template<typename _DerivedOther>
BundleTangent(const TangentBase<_DerivedOther> & o);
MANIF_TANGENT_ASSIGN_OP(BundleTangent)
// Tangent common API
/**
* @brief Get a reference to the underlying DataType.
*/
DataType & coeffs();
/**
* @brief Get a const reference to the underlying DataType.
*/
const DataType & coeffs() const;
// BundleTangent specific API
/**
* @brief Construct from BundleTangent elements
*/
BundleTangent(const typename _T<_Scalar>::Tangent & ... elements);
protected:
// Helper for the elements constructor
template <int ... _Idx>
BundleTangent(
internal::intseq<_Idx...>,
const typename _T<_Scalar>::Tangent & ... elements
);
protected:
DataType data_;
};
template<typename _Scalar, template<typename> class ... _T>
template<typename _DerivedOther>
BundleTangent<_Scalar, _T...>::BundleTangent(const TangentBase<_DerivedOther> & o)
: data_(o.coeffs())
{}
template<typename _Scalar, template<typename> class ... _T>
BundleTangent<_Scalar, _T...>::BundleTangent(const typename _T<_Scalar>::Tangent & ... elements)
: BundleTangent(internal::make_intseq_t<BundleSize>{}, elements ...)
{}
template<typename _Scalar, template<typename> class ... _T>
template<int ... _Idx>
BundleTangent<_Scalar, _T...>::BundleTangent(
internal::intseq<_Idx...>,
const typename _T<_Scalar>::Tangent & ... elements
) {
// c++11 "fold expression"
auto l = {((data_.template segment<Element<_Idx>::RepSize>(
std::get<_Idx>(internal::traits<Type>::RepSizeIdx)
) = elements.coeffs()), 0) ...};
static_cast<void>(l); // compiler warning
}
template<typename _Scalar, template<typename> class ... _T>
typename BundleTangent<_Scalar, _T...>::DataType &
BundleTangent<_Scalar, _T...>::coeffs()
{
return data_;
}
template<typename _Scalar, template<typename> class ... _T>
const typename BundleTangent<_Scalar, _T...>::DataType &
BundleTangent<_Scalar, _T...>::coeffs() const
{
return data_;
}
} // namespace manif
#endif // _MANIF_MANIF_BUNDLETANGENT_H_

439
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/bundle/BundleTangent_base.h vendored Normal file
View File

@ -0,0 +1,439 @@
#ifndef _MANIF_MANIF_BUNDLETANGENT_BASE_H_
#define _MANIF_MANIF_BUNDLETANGENT_BASE_H_
#include "manif/impl/tangent_base.h"
#include "manif/impl/traits.h"
namespace manif {
/**
* @brief The base class of the Bundle tangent.
*/
template<typename _Derived>
struct BundleTangentBase : TangentBase<_Derived>
{
private:
using Base = TangentBase<_Derived>;
using Type = BundleTangentBase<_Derived>;
public:
/**
* @brief Number of elements in the BundleTangent
*/
static constexpr std::size_t BundleSize = internal::traits<_Derived>::BundleSize;
using Elements = typename internal::traits<_Derived>::Elements;
template <int Idx>
using Element = typename internal::traits<_Derived>::template Element<Idx>;
template <int Idx>
using MapElement = typename internal::traits<_Derived>::template MapElement<Idx>;
template <int Idx>
using MapConstElement = typename internal::traits<_Derived>::template MapConstElement<Idx>;
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
using Base::data;
using Base::coeffs;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(BundleTangentBase)
public:
MANIF_TANGENT_ML_ASSIGN_OP(BundleTangentBase)
// Tangent common API
/**
* @brief Hat operator.
* @return An element of the Lie algebra.
*/
LieAlg hat() const;
/**
* @brief Exponential operator.
* @return An element of the Lie Group.
*/
LieGroup exp(OptJacobianRef J_m_t = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t = {}) const;
/**
* @brief Get the right Jacobian.
*/
Jacobian rjac() const;
/**
* @brief Get the left Jacobian.
*/
Jacobian ljac() const;
/**
* @brief Get the inverse of the right Jacobian.
*/
Jacobian rjacinv() const;
/**
* @brief Get the inverse of the left Jacobian.
*/
Jacobian ljacinv() const;
/**
* @brief
*/
Jacobian smallAdj() const;
// BundleTangent specific API
/**
* @brief Access BundleTangent element as Map
* @tparam _Idx element index
*/
template<int _Idx>
MapElement<_Idx> element();
/**
* @brief Access BundleTangent element as Map to const
* @tparam _Idx element index
*/
template<int _Idx>
MapConstElement<_Idx> element() const;
protected:
template <int ... _Idx>
LieAlg hat_impl(internal::intseq<_Idx...>) const;
template <int ... _Idx>
LieGroup exp_impl(OptJacobianRef J_m_t, internal::intseq<_Idx...>) const;
template <int ... _Idx>
Jacobian rjac_impl(internal::intseq<_Idx...>) const;
template <int ... _Idx>
Jacobian ljac_impl(internal::intseq<_Idx...>) const;
template <int ... _Idx>
Jacobian rjacinv_impl(internal::intseq<_Idx...>) const;
template <int ... _Idx>
Jacobian ljacinv_impl(internal::intseq<_Idx...>) const;
template <int ... _Idx>
Jacobian smallAdj_impl(internal::intseq<_Idx...>) const;
};
template<typename _Derived>
typename BundleTangentBase<_Derived>::LieAlg
BundleTangentBase<_Derived>::hat() const
{
return hat_impl(internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<int ... _Idx>
typename BundleTangentBase<_Derived>::LieAlg
BundleTangentBase<_Derived>::hat_impl(internal::intseq<_Idx...>) const
{
LieAlg ret = LieAlg::Zero();
// c++11 "fold expression"
auto l = {((ret.template block<
Element<_Idx>::LieAlg::RowsAtCompileTime,
Element<_Idx>::LieAlg::RowsAtCompileTime
>(
std::get<_Idx>(internal::traits<_Derived>::AlgIdx),
std::get<_Idx>(internal::traits<_Derived>::AlgIdx)
) = element<_Idx>().hat()), 0) ...};
static_cast<void>(l); // compiler warning
return ret;
}
template<typename _Derived>
typename BundleTangentBase<_Derived>::LieGroup
BundleTangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
if (J_m_t) {
J_m_t->setZero();
}
return exp_impl(J_m_t, internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<int ... _Idx>
typename BundleTangentBase<_Derived>::LieGroup
BundleTangentBase<_Derived>::exp_impl(
OptJacobianRef J_m_t, internal::intseq<_Idx...>
) const
{
if (J_m_t) {
return LieGroup(
element<_Idx>().exp(
J_m_t->template block<
Element<_Idx>::DoF, Element<_Idx>::DoF
>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
)
) ...
);
}
return LieGroup(element<_Idx>().exp() ...);
}
template<typename _Derived>
typename BundleTangentBase<_Derived>::LieGroup
BundleTangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
return exp(J_m_t);
}
template<typename _Derived>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::rjac() const
{
return rjac_impl(internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::ljac() const
{
return ljac_impl(internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::rjacinv() const
{
return rjacinv_impl(internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::ljacinv() const
{
return ljacinv_impl(internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::smallAdj() const
{
return smallAdj_impl(internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<int ... _Idx>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::rjac_impl(internal::intseq<_Idx...>) const
{
Jacobian Jr = Jacobian::Zero();
// c++11 "fold expression"
auto l = {((Jr.template block<Element<_Idx>::DoF, Element<_Idx>::DoF>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) = element<_Idx>().rjac() ), 0) ...};
static_cast<void>(l); // compiler warning
return Jr;
}
template<typename _Derived>
template<int ... _Idx>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::ljac_impl(internal::intseq<_Idx...>) const
{
Jacobian Jr = Jacobian::Zero();
// c++11 "fold expression"
auto l = {((Jr.template block<Element<_Idx>::DoF, Element<_Idx>::DoF>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) = element<_Idx>().ljac()), 0) ...};
static_cast<void>(l); // compiler warning
return Jr;
}
template<typename _Derived>
template<int ... _Idx>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::rjacinv_impl(internal::intseq<_Idx...>) const
{
Jacobian Jr = Jacobian::Zero();
// c++11 "fold expression"
auto l = {
((Jr.template block<Element<_Idx>::DoF, Element<_Idx>::DoF>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) = element<_Idx>().rjacinv()), 0) ...
};
static_cast<void>(l); // compiler warning
return Jr;
}
template<typename _Derived>
template<int ... _Idx>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::ljacinv_impl(internal::intseq<_Idx...>) const
{
Jacobian Jr = Jacobian::Zero();
// c++11 "fold expression"
auto l = {
((Jr.template block<Element<_Idx>::DoF, Element<_Idx>::DoF>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) = element<_Idx>().ljacinv()), 0) ...
};
static_cast<void>(l); // compiler warning
return Jr;
}
template<typename _Derived>
template<int ... _Idx>
typename BundleTangentBase<_Derived>::Jacobian
BundleTangentBase<_Derived>::smallAdj_impl(internal::intseq<_Idx...>) const
{
Jacobian Jr = Jacobian::Zero();
// c++11 "fold expression"
auto l = {
((Jr.template block<Element<_Idx>::DoF, Element<_Idx>::DoF>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) = element<_Idx>().smallAdj()), 0) ...
};
static_cast<void>(l); // compiler warning
return Jr;
}
template<typename _Derived>
template<int _Idx>
auto BundleTangentBase<_Derived>::element() -> MapElement<_Idx>
{
return MapElement<_Idx>(
static_cast<_Derived &>(*this).coeffs().data() +
std::get<_Idx>(internal::traits<_Derived>::RepSizeIdx)
);
}
template<typename _Derived>
template<int _Idx>
auto BundleTangentBase<_Derived>::element() const -> MapConstElement<_Idx>
{
return MapConstElement<_Idx>(
static_cast<const _Derived &>(*this).coeffs().data() +
std::get<_Idx>(internal::traits<_Derived>::RepSizeIdx)
);
}
namespace internal {
/**
* @brief Generator specialization for BundleTangentBase objects.
*/
template<typename Derived>
struct GeneratorEvaluator<BundleTangentBase<Derived>>
{
static typename BundleTangentBase<Derived>::LieAlg
run(const unsigned int i)
{
MANIF_CHECK(
i < BundleTangentBase<Derived>::DoF,
"Index i must less than DoF!",
invalid_argument
);
return run(i, make_intseq_t<Derived::BundleSize>{});
}
template<int ... _Idx>
static typename BundleTangentBase<Derived>::LieAlg
run(const unsigned int i, intseq<_Idx...>)
{
using LieAlg = typename BundleTangentBase<Derived>::LieAlg;
LieAlg Ei = LieAlg::Zero();
// c++11 "fold expression"
auto l = {((Ei.template block<
Derived::template Element<_Idx>::LieAlg::RowsAtCompileTime,
Derived::template Element<_Idx>::LieAlg::RowsAtCompileTime
>(
std::get<_Idx>(internal::traits<Derived>::AlgIdx),
std::get<_Idx>(internal::traits<Derived>::AlgIdx)
) = (
static_cast<int>(i) >= std::get<_Idx>(internal::traits<Derived>::DoFIdx) &&
static_cast<int>(i) < std::get<_Idx>(internal::traits<Derived>::DoFIdx) + Derived::template Element<_Idx>::DoF
) ?
Derived::template Element<_Idx>::Generator(
static_cast<int>(i) - std::get<_Idx>(internal::traits<Derived>::DoFIdx)
) :
Derived::template Element<_Idx>::LieAlg::Zero()
), 0) ...};
static_cast<void>(l); // compiler warning
return Ei;
}
};
/**
* @brief Random specialization for BundleTangent objects.
*/
template<typename Derived>
struct RandomEvaluatorImpl<BundleTangentBase<Derived>>
{
static void run(BundleTangentBase<Derived> & m)
{
run(m, make_intseq_t<Derived::BundleSize>{});
}
template<int ... _Idx>
static void run(BundleTangentBase<Derived> & m, intseq<_Idx...>)
{
m = typename BundleTangentBase<Derived>::Tangent(
Derived::template Element<_Idx>::Random() ...
);
}
};
//! @brief Vee specialization for BundleTangentBase objects.
template <typename Derived>
struct VeeEvaluatorImpl<BundleTangentBase<Derived>> {
template <typename TL, typename TR>
static void run(TL& t, const TR& v) {
return vee_impl(
t, v, internal::make_intseq_t<BundleTangentBase<Derived>::BundleSize>{}
);
}
};
template <typename TL, typename TR, int ... _Idx>
void vee_impl(TL& t, const TR& v, internal::intseq<_Idx...>) {
// c++11 "fold expression"
auto l = {((t.template element<_Idx>().setVee(
v.template block<
TL::template Element<_Idx>::LieAlg::RowsAtCompileTime,
TL::template Element<_Idx>::LieAlg::RowsAtCompileTime
>(
std::get<_Idx>(internal::traits<typename TL::Tangent>::AlgIdx),
std::get<_Idx>(internal::traits<typename TL::Tangent>::AlgIdx)
))), 0) ...};
static_cast<void>(l); // compiler warning
}
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_BUNDLETANGENT_BASE_H_

99
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/bundle/BundleTangent_map.h vendored Normal file
View File

@ -0,0 +1,99 @@
#ifndef _MANIF_MANIF_BUNDLETANGENT_MAP_H_
#define _MANIF_MANIF_BUNDLETANGENT_MAP_H_
#include "manif/impl/bundle/BundleTangent.h"
namespace manif {
namespace internal {
/**
* @brief traits specialization for Eigen Map
*/
template<typename _Scalar, template<typename> class ... T>
struct traits<Eigen::Map<BundleTangent<_Scalar, T...>, 0>>
: public traits<BundleTangent<_Scalar, T...>>
{
using typename traits<BundleTangent<_Scalar, T...>>::Scalar;
using traits<BundleTangent<Scalar, T...>>::DoF;
using Base = BundleTangentBase<Eigen::Map<BundleTangent<Scalar, T...>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, DoF, 1>, 0>;
};
/**
* @brief traits specialization for Eigen const Map
*/
template<typename _Scalar, template<typename> class ... T>
struct traits<Eigen::Map<const BundleTangent<_Scalar, T...>, 0>>
: public traits<const BundleTangent<_Scalar, T...>>
{
using typename traits<const BundleTangent<_Scalar, T...>>::Scalar;
using traits<const BundleTangent<Scalar, T...>>::DoF;
using Base = BundleTangentBase<Eigen::Map<const BundleTangent<Scalar, T...>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, DoF, 1>, 0>;
};
} // namespace internal
} // namespace manif
namespace Eigen {
/**
* @brief Specialization of Map for manif::Bundle
*/
template<class _Scalar, template<typename> class ... T>
class Map<manif::BundleTangent<_Scalar, T...>, 0>
: public manif::BundleTangentBase<Map<manif::BundleTangent<_Scalar, T...>, 0>>
{
using Base = manif::BundleTangentBase<Map<manif::BundleTangent<_Scalar, T...>, 0>>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
using Base::BundleSize;
Map(Scalar * coeffs) : data_(coeffs) { }
MANIF_TANGENT_MAP_ASSIGN_OP(BundleTangent)
DataType & coeffs() {return data_;}
const DataType & coeffs() const {return data_;}
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::BundleTangent
*/
template<class _Scalar, template<typename> class ... T>
class Map<const manif::BundleTangent<_Scalar, T...>, 0>
: public manif::BundleTangentBase<Map<const manif::BundleTangent<_Scalar, T...>, 0>>
{
using Base = manif::BundleTangentBase<Map<const manif::BundleTangent<_Scalar, T...>, 0>>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
using Base::BundleSize;
Map(const Scalar * coeffs) : data_(coeffs) { }
const DataType & coeffs() const {return data_;}
protected:
const DataType data_;
};
} // namespace Eigen
#endif // _MANIF_MANIF_BUNDLETANGENT_MAP_H_

442
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/bundle/Bundle_base.h vendored Normal file
View File

@ -0,0 +1,442 @@
#ifndef _MANIF_MANIF_BUNDLE_BASE_H_
#define _MANIF_MANIF_BUNDLE_BASE_H_
#include "manif/impl/lie_group_base.h"
#include "manif/impl/traits.h"
namespace manif {
/**
* @brief The base class of the Bundle group.
*/
template<typename _Derived>
struct BundleBase : LieGroupBase<_Derived>
{
private:
using Base = LieGroupBase<_Derived>;
using Type = BundleBase<_Derived>;
public:
/**
* @brief Number of elements in bundle
*/
static constexpr std::size_t BundleSize = internal::traits<_Derived>::BundleSize;
using Elements = typename internal::traits<_Derived>::Elements;
template <int Idx>
using Element = typename internal::traits<_Derived>::template Element<Idx>;
template <int Idx>
using MapElement = typename internal::traits<_Derived>::template MapElement<Idx>;
template <int Idx>
using MapConstElement = typename internal::traits<_Derived>::template MapConstElement<Idx>;
MANIF_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_AUTO_API
MANIF_INHERIT_GROUP_OPERATOR
using Base::coeffs;
using Transformation = typename internal::traits<_Derived>::Transformation;
// LieGroup common API
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(BundleBase)
public:
MANIF_GROUP_ML_ASSIGN_OP(BundleBase)
/**
* @brief Get the inverse of this.
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this.
*/
LieGroup inverse(OptJacobianRef J_minv_m = {}) const;
/**
* @brief Get the corresponding Lie algebra element.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt to this.
* @return The tangent of this.
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please consider using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another Bundle element.
* @param[in] m Another Bundle element.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
*/
template<typename _DerivedOther>
LieGroup compose(
const LieGroupBase<_DerivedOther> & m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}
) const;
/**
* @brief Bundle group action
* @param v vector.
* @param[out] -optional- J_vout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object.
* @return The translated vector.
*/
Vector act(
const Vector & v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v = {}
) const;
/**
* @brief Get the adjoint matrix at this.
*/
Jacobian adj() const;
// Bundle-specific API
/**
* @brief Get the element-diagonal transformation matrix
*/
Transformation transform() const;
/**
* @brief Access Bundle element as Map
* @tparam _Idx element index
*/
template<int _Idx>
MapElement<_Idx> element();
/**
* @brief Access Bundle element as Map to const
* @tparam _Idx element index
*/
template<int _Idx>
MapConstElement<_Idx> element() const;
protected:
template <int ... _Idx>
LieGroup inverse_impl(OptJacobianRef, internal::intseq<_Idx...>) const;
template <int ... _Idx>
Tangent log_impl(OptJacobianRef, internal::intseq<_Idx...>) const;
template<typename _DerivedOther, int ... _Idx>
LieGroup compose_impl(
const LieGroupBase<_DerivedOther> & m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb,
internal::intseq<_Idx...>
) const;
template<int ... _Idx>
Vector act_impl(
const Vector & v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v,
internal::intseq<_Idx...>
) const;
template <int ... _Idx>
Jacobian adj_impl(internal::intseq<_Idx...>) const;
template <int ... _Idx>
Transformation
transform_impl(internal::intseq<_Idx...>) const;
};
template<typename _Derived>
typename BundleBase<_Derived>::Transformation
BundleBase<_Derived>::transform() const
{
return transform_impl(internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<int ... _Idx>
typename BundleBase<_Derived>::Transformation
BundleBase<_Derived>::transform_impl(
internal::intseq<_Idx...>
) const {
Transformation ret = Transformation::Zero();
// cxx11 "fold expression"
auto l =
{((ret.template element<
Element<_Idx>::Dim+1, Element<_Idx>::Dim+1
>(
std::get<_Idx>(internal::traits<_Derived>::TraIdx),
std::get<_Idx>(internal::traits<_Derived>::TraIdx)
) = element<_Idx>().transform()), 0) ...};
static_cast<void>(l); // compiler warning
return ret;
}
template<typename _Derived>
typename BundleBase<_Derived>::LieGroup
BundleBase<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
if (J_minv_m) {
J_minv_m->setZero();
}
return inverse_impl(J_minv_m, internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<int ... _Idx>
typename BundleBase<_Derived>::LieGroup
BundleBase<_Derived>::inverse_impl(
OptJacobianRef J_minv_m, internal::intseq<_Idx...>
) const {
if (J_minv_m) {
return LieGroup(
element<_Idx>().inverse(
J_minv_m->template block<
Element<_Idx>::DoF,
Element<_Idx>::DoF
>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
)
) ...
);
}
return LieGroup(element<_Idx>().inverse() ...);
}
template<typename _Derived>
typename BundleBase<_Derived>::Tangent
BundleBase<_Derived>::log(OptJacobianRef J_t_m) const
{
if (J_t_m) {
J_t_m->setZero();
}
return log_impl(J_t_m, internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<int ... _Idx>
typename BundleBase<_Derived>::Tangent
BundleBase<_Derived>::log_impl(
OptJacobianRef J_minv_m,
internal::intseq<_Idx...>
) const {
if (J_minv_m) {
return Tangent(
element<_Idx>().log(
J_minv_m->template block<
Element<_Idx>::DoF,
Element<_Idx>::DoF
>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
)
)...
);
}
return Tangent(element<_Idx>().log() ...);
}
template<typename _Derived>
typename BundleBase<_Derived>::Tangent
BundleBase<_Derived>::lift(OptJacobianRef J_t_m) const
{
return log(J_t_m);
}
template<typename _Derived>
template<typename _DerivedOther>
typename BundleBase<_Derived>::LieGroup
BundleBase<_Derived>::compose(
const LieGroupBase<_DerivedOther> & m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb
) const {
if (J_mc_ma) {
J_mc_ma->setZero();
}
if (J_mc_mb) {
J_mc_mb->setZero();
}
return compose_impl(m, J_mc_ma, J_mc_mb, internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<typename _DerivedOther, int ... _Idx>
typename BundleBase<_Derived>::LieGroup
BundleBase<_Derived>::compose_impl(
const LieGroupBase<_DerivedOther> & m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb,
internal::intseq<_Idx...>
) const {
return LieGroup(
element<_Idx>().compose(
static_cast<const _DerivedOther &>(m).template element<_Idx>(),
J_mc_ma ?
J_mc_ma->template block<
Element<_Idx>::DoF, Element<_Idx>::DoF
>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) :
tl::optional<
Eigen::Ref<Eigen::Matrix<Scalar, Element<_Idx>::DoF, Element<_Idx>::DoF>>
>{},
J_mc_mb ?
J_mc_mb->template block<
Element<_Idx>::DoF, Element<_Idx>::DoF
>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) :
tl::optional<
Eigen::Ref<Eigen::Matrix<Scalar, Element<_Idx>::DoF, Element<_Idx>::DoF>>
>{}
) ...
);
}
template<typename _Derived>
typename BundleBase<_Derived>::Vector
BundleBase<_Derived>::act(
const typename BundleBase<_Derived>::Vector & v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v) const
{
if (J_vout_m) {
J_vout_m->setZero();
}
if (J_vout_v) {
J_vout_v->setZero();
}
return act_impl(v, J_vout_m, J_vout_v, internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<int ... _Idx>
typename BundleBase<_Derived>::Vector
BundleBase<_Derived>::act_impl(
const typename BundleBase<_Derived>::Vector & v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v,
internal::intseq<_Idx...>
) const {
Vector ret;
// cxx11 "fold expression"
auto l = {((ret.template segment<Element<_Idx>::Dim>(
std::get<_Idx>(internal::traits<_Derived>::DimIdx)
) = element<_Idx>().act(
v.template segment<Element<_Idx>::Dim>(
std::get<_Idx>(internal::traits<_Derived>::DimIdx)
),
J_vout_m ?
J_vout_m->template block<Element<_Idx>::Dim, Element<_Idx>::DoF>(
std::get<_Idx>(internal::traits<_Derived>::DimIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) :
tl::optional<
Eigen::Ref<Eigen::Matrix<Scalar, Element<_Idx>::Dim, Element<_Idx>::DoF>>
>{},
J_vout_v ?
J_vout_v->template block<Element<_Idx>::Dim, Element<_Idx>::Dim>(
std::get<_Idx>(internal::traits<_Derived>::DimIdx),
std::get<_Idx>(internal::traits<_Derived>::DimIdx)
) :
tl::optional<
Eigen::Ref<Eigen::Matrix<Scalar, Element<_Idx>::Dim, Element<_Idx>::Dim>>
>{}
)
), 0) ...};
static_cast<void>(l); // compiler warning
return ret;
}
template<typename _Derived>
typename BundleBase<_Derived>::Jacobian
BundleBase<_Derived>::adj() const
{
return adj_impl(internal::make_intseq_t<BundleSize>{});
}
template<typename _Derived>
template<int ... _Idx>
typename BundleBase<_Derived>::Jacobian
BundleBase<_Derived>::adj_impl(internal::intseq<_Idx...>) const
{
Jacobian adj = Jacobian::Zero();
// cxx11 "fold expression"
auto l = {((adj.template block<
Element<_Idx>::DoF, Element<_Idx>::DoF
>(
std::get<_Idx>(internal::traits<_Derived>::DoFIdx),
std::get<_Idx>(internal::traits<_Derived>::DoFIdx)
) = element<_Idx>().adj()), 0) ...};
static_cast<void>(l); // compiler warning
return adj;
}
template<typename _Derived>
template<int _Idx>
auto BundleBase<_Derived>::element() -> MapElement<_Idx>
{
return MapElement<_Idx>(
static_cast<_Derived &>(*this).coeffs().data() +
std::get<_Idx>(internal::traits<_Derived>::RepSizeIdx)
);
}
template<typename _Derived>
template<int _Idx>
auto BundleBase<_Derived>::element() const -> MapConstElement<_Idx>
{
return MapConstElement<_Idx>(
static_cast<const _Derived &>(*this).coeffs().data() +
std::get<_Idx>(internal::traits<_Derived>::RepSizeIdx)
);
}
namespace internal {
/**
* @brief Random specialization for Bundle objects.
*/
template<typename Derived>
struct RandomEvaluatorImpl<BundleBase<Derived>>
{
static void run(BundleBase<Derived> & m)
{
run(m, internal::make_intseq_t<Derived::BundleSize>{});
}
template <int ... _Idx>
static void run(BundleBase<Derived> & m, internal::intseq<_Idx...>)
{
m = typename BundleBase<Derived>::LieGroup(
BundleBase<Derived>::template Element<_Idx>::Random() ...
);
}
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_BUNDLE_BASE_H_

97
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/bundle/Bundle_map.h vendored Normal file
View File

@ -0,0 +1,97 @@
#ifndef _MANIF_MANIF_BUNDLE_MAP_H_
#define _MANIF_MANIF_BUNDLE_MAP_H_
#include "manif/impl/bundle/Bundle.h"
namespace manif {
namespace internal {
/**
* @brief traits specialization for Eigen Map
*/
template<typename _Scalar, template<typename> class ... T>
struct traits<Eigen::Map<Bundle<_Scalar, T...>, 0>>
: public traits<Bundle<_Scalar, T...>>
{
using typename traits<Bundle<_Scalar, T...>>::Scalar;
using traits<Bundle<Scalar, T...>>::RepSize;
using Base = BundleBase<Eigen::Map<Bundle<Scalar, T...>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
/**
* @brief traits specialization for Eigen const Map
*/
template<typename _Scalar, template<typename> class ... T>
struct traits<Eigen::Map<const Bundle<_Scalar, T...>, 0>>
: public traits<const Bundle<_Scalar, T...>>
{
using typename traits<const Bundle<_Scalar, T...>>::Scalar;
using traits<const Bundle<Scalar, T...>>::RepSize;
using Base = BundleBase<Eigen::Map<const Bundle<Scalar, T...>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
} // namespace internal
} // namespace manif
namespace Eigen {
/**
* @brief Specialization of Map for manif::Bundle
*/
template<class _Scalar, template<typename> class ... T>
class Map<manif::Bundle<_Scalar, T...>, 0>
: public manif::BundleBase<Map<manif::Bundle<_Scalar, T...>, 0>>
{
using Base = manif::BundleBase<Map<manif::Bundle<_Scalar, T...>, 0>>;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::BundleSize;
Map(Scalar * coeffs) : data_(coeffs) { }
MANIF_GROUP_MAP_ASSIGN_OP(Bundle)
DataType & coeffs() {return data_;}
const DataType & coeffs() const {return data_;}
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::Bundle
*/
template<class _Scalar, template<typename> class ... T>
class Map<const manif::Bundle<_Scalar, T...>, 0>
: public manif::BundleBase<Map<const manif::Bundle<_Scalar, T...>, 0>>
{
using Base = manif::BundleBase<Map<const manif::Bundle<_Scalar, T...>, 0>>;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::BundleSize;
Map(const Scalar * coeffs) : data_(coeffs) { }
const DataType & coeffs() const {return data_;}
protected:
const DataType data_;
};
} // namespace Eigen
#endif // _MANIF_MANIF_BUNDLE_MAP_H_

33
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/bundle/Bundle_properties.h vendored Normal file
View File

@ -0,0 +1,33 @@
#ifndef _MANIF_MANIF_BUNDLE_PROPERTIES_H_
#define _MANIF_MANIF_BUNDLE_PROPERTIES_H_
#include "manif/impl/traits.h"
namespace manif {
// Forward declaration for type traits specialization
template<typename _Derived> struct BundleBase;
template<typename _Derived> struct BundleTangentBase;
namespace internal {
//! traits specialization
template<typename _Derived>
struct LieGroupProperties<BundleBase<_Derived>>
{
static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension
static constexpr int DoF = traits<_Derived>::DoF; /// @brief Degrees of freedom
};
//! traits specialization
template<typename _Derived>
struct LieGroupProperties<BundleTangentBase<_Derived>>
{
static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension
static constexpr int DoF = traits<_Derived>::DoF; /// @brief Degrees of freedom
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_BUNDLE_PROPERTIES_H_

35
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/cast.h vendored Normal file
View File

@ -0,0 +1,35 @@
#ifndef _MANIF_MANIF_IMPL_CAST_H_
#define _MANIF_MANIF_IMPL_CAST_H_
namespace manif {
namespace internal {
template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl {
template <typename T>
static auto run(const T& o) -> typename T::template LieGroupTemplate<NewScalar> {
return typename T::template LieGroupTemplate<NewScalar>(
o.coeffs().template cast<NewScalar>()
);
}
};
template <typename Derived, typename NewScalar>
struct CastEvaluator : CastEvaluatorImpl<Derived, NewScalar> {
using Base = CastEvaluatorImpl<Derived, NewScalar>;
CastEvaluator(const Derived& xptr) : xptr_(xptr) {}
auto run() const -> typename Derived::template LieGroupTemplate<NewScalar> {
return Base::run(xptr_);
}
protected:
const Derived& xptr_;
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_IMPL_CAST_H_

254
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/eigen.h vendored Normal file
View File

@ -0,0 +1,254 @@
#ifndef _MANIF_MANIF_EIGEN_H_
#define _MANIF_MANIF_EIGEN_H_
#include <cassert>
#include <Eigen/Core>
#include <Eigen/LU> // for mat.inverse()
#include <Eigen/Geometry>
#include <manif/constants.h>
/**
* @note static_cast<int> to avoid -Wno-enum-compare
*/
//
// Static Asserts
//
// Define some custom static_assert macros
#define static_assert_rows_dim(x, dim) \
static_assert(static_cast<int>(std::decay<decltype(x)>::type::RowsAtCompileTime) == dim, \
"x.rows != "#dim" .");
#define static_assert_cols_dim(x, dim) \
static_assert(static_cast<int>(std::decay<decltype(x)>::type::ColsAtCompileTime) == dim, \
"x.cols != "#dim" .");
#define static_assert_dim(x, rows, cols) \
static_assert_rows_dim(x, rows); \
static_assert_cols_dim(x, cols);
#define static_assert_dim_eq(l,r) \
static_assert(static_cast<int>(std::decay<decltype(l)>::type::ColsAtCompileTime) == \
static_cast<int>(std::decay<decltype(r)>::type::ColsAtCompileTime), \
"lhs.cols != rhs.cols !"); \
static_assert(static_cast<int>(std::decay<decltype(l)>::type::RowsAtCompileTime) == \
static_cast<int>(std::decay<decltype(r)>::type::RowsAtCompileTime), \
"lhs.rows != rhs.rows !");
#define static_assert_is_vector(x) \
static_assert_cols_dim(x, 1);
#define static_assert_vector_dim(x, dim) \
static_assert_is_vector(x); \
static_assert_rows_dim(x, dim);
#define static_assert_is_colmajor_vector(x) \
static_assert_rows_dim(x, 1);
#define static_assert_colmajor_vector_dim(x, dim) \
static_assert_is_colmajor_vector(x); \
static_assert_cols_dim(x, dim);
//
// Asserts
//
// Define some custom assert macros
#define assert_rows_dim(x, dim) \
static_assert(static_cast<int>(std::decay<decltype(x)>::type::RowsAtCompileTime) == dim || \
std::decay<decltype(x)>::type::RowsAtCompileTime == Eigen::Dynamic, \
"x.rows != "#dim" ."); \
assert(x.rows() == dim && "x.rows != "#dim" .");
#define assert_cols_dim(x, dim) \
static_assert(static_cast<int>(std::decay<decltype(x)>::type::ColsAtCompileTime) == dim || \
std::decay<decltype(x)>::type::ColsAtCompileTime == Eigen::Dynamic, \
"x.cols != "#dim" ."); \
assert(x.cols() == dim && "x.cols != "#dim" .");
#define assert_dim(x, rows, cols) \
assert_rows_dim(x, rows); \
assert_cols_dim(x, cols);
#define assert_dim_eq(l,r) \
static_assert(static_cast<int>(std::decay<decltype(l)>::type::ColsAtCompileTime) == \
static_cast<int>(std::decay<decltype(r)>::type::ColsAtCompileTime) || \
std::decay<decltype(l)>::type::ColsAtCompileTime == Eigen::Dynamic || \
std::decay<decltype(r)>::type::ColsAtCompileTime == Eigen::Dynamic, \
"lhs.cols != rhs.cols !"); \
static_assert(static_cast<int>(std::decay<decltype(l)>::type::RowsAtCompileTime) == \
static_cast<int>(std::decay<decltype(r)>::type::RowsAtCompileTime) || \
std::decay<decltype(l)>::type::RowsAtCompileTime == Eigen::Dynamic || \
std::decay<decltype(r)>::type::RowsAtCompileTime == Eigen::Dynamic, \
"lhs.rows != rhs.rows !"); \
assert(l.rows() == r.rows() && "lhs.rows != rhs.rows !"); \
assert(l.cols() == r.cols() && "lhs.cols != rhs.cols !"); \
#define assert_is_vector(x) \
static_assert(std::decay<decltype(x)>::type::ColsAtCompileTime == 1 || \
std::decay<decltype(x)>::type::ColsAtCompileTime == Eigen::Dynamic, \
"Expected a vector !"); \
assert(x.cols() == 1 && "Expected a vector !"); \
#define assert_vector_dim(x, dim) \
assert_is_vector(x); \
assert_rows_dim(x, dim);
#define assert_is_colmajor_vector(x) \
static_assert(std::decay<decltype(x)>::type::RowsAtCompileTime == 1 || \
std::decay<decltype(x)>::type::RowsAtCompileTime == Eigen::Dynamic, \
"Expected a column-major vector !"); \
assert(x.rows() == 1 && "Expected a column-major vector !"); \
#define assert_colmajor_vector_dim(x, dim) \
assert_is_colmajor_vector(x); \
assert_cols_dim(x, dim);
namespace manif {
template <typename Scalar, int S>
using SquareMatrix = Eigen::Matrix<Scalar, S, S>;
namespace internal {
template< class Base, class Derived >
constexpr bool is_base_of_v()
{
return std::is_base_of<Base, Derived>::value;
}
/**
* @brief traitscast specialization that come handy when writing thing like
* using Matrix3f = typename traitscast<Matrix3d, float>::cast;
*/
template <typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols, typename NewScalar>
struct traitscast<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, NewScalar>
{
using cast = Eigen::Matrix<NewScalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>;
};
} /* namespace internal */
/**
* @brief Return a 2x2 skew matrix given a scalar.
* @note [x] = | 0 -x |
* | x 0 |
*/
template <typename _Scalar>
typename std::enable_if<std::is_arithmetic<_Scalar>::value || internal::is_ad<_Scalar>::value,
Eigen::Matrix<_Scalar, 2, 2>>::type
skew(const _Scalar v)
{
return (Eigen::Matrix<_Scalar, 2, 2>() <<
_Scalar(0.), -v,
v, _Scalar(0.) ).finished();
}
/**
* @brief Return a 3x3 skew matrix given 3-vector.
* @note [v] = | 0 -v(2) +v(1) |
* | +v(2) 0 -v(0) |
* | -v(1) +v(0) 0 |
*/
template <typename _Derived>
typename std::enable_if<(internal::is_base_of_v<Eigen::MatrixBase<_Derived>, _Derived>()
&& _Derived::RowsAtCompileTime == 3),
Eigen::Matrix<typename _Derived::Scalar, 3, 3>>::type
skew(const Eigen::MatrixBase<_Derived>& v)
{
assert_vector_dim(v, 3);
using T = typename _Derived::Scalar;
return (Eigen::Matrix<T, 3, 3>() <<
T(0.), -v(2), +v(1),
+v(2), T(0.), -v(0),
-v(1), +v(0), T(0.) ).finished();
}
#if !defined(DOXYGEN_SHOULD_SKIP_THIS)
/**
* @brief Return either a 2x2 or a 3x3 skew matrix given a scalar or a 3-vector.
*/
template <typename _Derived>
typename std::enable_if<(internal::is_base_of_v<Eigen::MatrixBase<_Derived>, _Derived>()
&& _Derived::RowsAtCompileTime == Eigen::Dynamic),
Eigen::Matrix<typename _Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic>>::type
skew(const Eigen::MatrixBase<_Derived>& v)
{
using T = typename _Derived::Scalar;
if (v.rows() == 1) {
return skew(v(0));
} else if (v.rows() == 3) {
return skew(Eigen::Ref<const Eigen::Matrix<T, 3, 1>>(v));
} else {
MANIF_THROW("Unexpected vector size in function skew.");
}
}
#endif // DOXYGEN_SHOULD_SKIP_THIS
template <typename Scalar>
Eigen::Matrix<Scalar, 3, 1> randPointInBall(Scalar radius)
{
// See https://stackoverflow.com/a/5408843/9709397
using std::acos;
using std::sin;
using std::cos;
using std::cbrt;
// random(0, 2pi)
Scalar phi = static_cast<Scalar>(rand()) / (static_cast<Scalar>(RAND_MAX / (Scalar(2) * MANIF_PI)));
// random(-1, 1)
Scalar costheta = Scalar(-1) + static_cast<Scalar>(rand()) / (static_cast<Scalar>(RAND_MAX / Scalar(2)));
// random(0, 1)
Scalar u = static_cast<Scalar>(rand()) / static_cast<Scalar>(RAND_MAX);
Scalar theta = acos(costheta);
Scalar r = radius * cbrt(u);
Scalar rsintheta = r * sin(theta);
return Eigen::Matrix<Scalar, 3, 1>(
rsintheta * cos(phi),
rsintheta * sin(phi),
r * costheta
);
}
template <typename Scalar>
Eigen::Quaternion<Scalar> randQuat()
{
#if EIGEN_VERSION_AT_LEAST(3,3,0)
return Eigen::Quaternion<Scalar>::UnitRandom();
#else
// @note:
// Quaternion::UnitRandom is not available in Eigen 3.3-beta1
// which is the default version in Ubuntu 16.04
// So we copy its implementation here.
using std::sqrt;
using std::sin;
using std::cos;
const Scalar u1 = Eigen::internal::random<Scalar>(0, 1),
u2 = Eigen::internal::random<Scalar>(0, 2.*EIGEN_PI),
u3 = Eigen::internal::random<Scalar>(0, 2.*EIGEN_PI);
const Scalar a = sqrt(1. - u1),
b = sqrt(u1);
return Eigen::Quaternion<Scalar>(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3));
#endif
}
} /* namespace manif */
#endif /* _MANIF_MANIF_EIGEN_H_ */

47
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/generator.h vendored Normal file
View File

@ -0,0 +1,47 @@
#ifndef _MANIF_MANIF_GENERATOR_H_
#define _MANIF_MANIF_GENERATOR_H_
namespace manif {
namespace internal {
template <typename Derived>
struct GeneratorEvaluator
{
static typename Derived::LieAlg
run(const unsigned int)
{
/// @todo print actual Derived type
static_assert(constexpr_false<Derived>(),
"GeneratorEvaluator not overloaded for Derived type!");
}
};
template <typename Derived>
struct InnerWeightsEvaluator
{
static typename Derived::InnerWeightsMatrix
run()
{
using InnerWeightsMatrix = typename Derived::InnerWeightsMatrix;
auto computeW = []()
{
InnerWeightsMatrix W = InnerWeightsMatrix::Zero();
for (int r = 0; r <Derived::DoF; ++r)
for (int c = 0; c < Derived::DoF; ++c)
W(r,c) = (Derived::Generator(r) * Derived::Generator(c).transpose()).trace();
return W;
};
const static InnerWeightsMatrix W = computeW();
return W;
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_GENERATOR_H_ */

735
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/lie_group_base.h vendored Normal file
View File

@ -0,0 +1,735 @@
#ifndef _MANIF_MANIF_LIE_GROUP_BASE_H_
#define _MANIF_MANIF_LIE_GROUP_BASE_H_
#include "manif/impl/macro.h"
#include "manif/impl/traits.h"
#include "manif/impl/eigen.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/assignment_assert.h"
#include "manif/impl/cast.h"
#include "manif/constants.h"
#include <tl/optional.hpp>
namespace manif {
/**
* @brief Base class for Lie groups.
* Defines the minimum common API.
* @see TangentBase.
*/
template <class _Derived>
struct LieGroupBase
{
static constexpr int Dim = internal::traits<_Derived>::Dim;
static constexpr int DoF = internal::traits<_Derived>::DoF;
static constexpr int RepSize = internal::traits<_Derived>::RepSize;
using Scalar = typename internal::traits<_Derived>::Scalar;
using LieGroup = typename internal::traits<_Derived>::LieGroup;
using DataType = typename internal::traits<_Derived>::DataType;
using Tangent = typename internal::traits<_Derived>::Tangent;
using Jacobian = typename internal::traits<_Derived>::Jacobian;
using Vector = typename internal::traits<_Derived>::Vector;
using OptJacobianRef = tl::optional<Eigen::Ref<Jacobian>>;
template <typename _Scalar>
using LieGroupTemplate = typename internal::traitscast<LieGroup, _Scalar>::cast;
public:
//! @brief Helper for skipping an optional parameter.
static const OptJacobianRef _;
protected:
MANIF_DEFAULT_CONSTRUCTOR(LieGroupBase)
public:
/**
* @brief Assignment operator.
* @param[in] An element of the Lie group.
* @return A reference to this.
* @note This is a special case of the templated operator=. Its purpose is to
* prevent a default operator= from hiding the templated operator=.
*/
_Derived& operator =(const LieGroupBase& m);
/**
* @brief Assignment operator.
* @param[in] An element of the Lie group.
* @return A reference to this.
*/
template <typename _DerivedOther>
_Derived& operator =(const LieGroupBase<_DerivedOther>& m);
/**
* @brief Assignment operator given Eigen object.
* @param[in] An element of the Lie group.
* @return A reference to this.
*/
template <typename _EigenDerived>
_Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& data);
//! @brief Access the underlying data by const reference
DataType& coeffs();
//! @brief Access the underlying data by const reference
const DataType& coeffs() const;
//! @brief Access the underlying data by pointer
Scalar* data();
//! @brief Access the underlying data by const pointer
const Scalar* data() const;
//! @brief Cast the LieGroup object to a copy
//! of a different scalar type
template <class _NewScalar>
LieGroupTemplate<_NewScalar> cast() const;
/// @todo 'cast' across groups
/// SO3 so3 = so2.as<SO3>()
// template <class _DerivedOther>
// LieGroupTemplate<_DerivedOther> as() const;
/**
* @brief Set the LieGroup object this to Identity.
* @return A reference to this.
* @see Eq. (2).
*/
_Derived& setIdentity();
/**
* @brief Set the LieGroup object this to a random value.
* @return A reference to this.
* @note Randomization happens in the tangent space so that
* M = Log(tau.random)
*/
_Derived& setRandom();
// Minimum API
// Those functions must be implemented in the Derived class !
/**
* @brief Get the inverse of the LieGroup object this.
* @param[out] -optional- J_m_t Jacobian of the inverse wrt this.
* @return The Inverse of this.
* @note See Eq. (3).
* @see TangentBase.
*/
LieGroup inverse(OptJacobianRef J_m_t = {}) const;
/**
* @brief Get the corresponding Lie algebra element in vector form.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt this.
* @return The tangent element in vector form.
* @note This is the log() map in vector form.
* @see Eq. (24).
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another element of the same Lie group.
* @param[in] m Another element of the same Lie group.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
* @note See Eqs. (1,2,3,4).
*/
template <typename _DerivedOther>
LieGroup compose(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}) const;
/**
* @brief Get the action of the Lie group object on a point.
* @param[in] v A point.
* @param[out] -optional- J_vout_m Jacobian of the new object wrt this.
* @param[out] -optional- J_vout_v Jacobian of the new object wrt input object.
* @return A point acted upon by the object.
*/
template <typename _EigenDerived>
Vector act(const Eigen::MatrixBase<_EigenDerived>& v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v = {}) const;
/**
* @brief Get the Adjoint of the Lie group element this.
* @note See Eq. (29).
*/
Jacobian adj() const;
// Deduced API
/**
* @brief Right oplus operation of the Lie group.
* @param[in] t An element of the tangent of the Lie group.
* @param[out] -optional- J_mout_m Jacobian of the oplus operation wrt this.
* @param[out] -optional- J_mout_t Jacobian of the oplus operation wrt the tangent element.
* @return An element of the Lie group.
* @note See Eq. (25).
*/
template <typename _DerivedOther>
LieGroup rplus(const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_m = {},
OptJacobianRef J_mout_t = {}) const;
/**
* @brief Left oplus operation of the Lie group.
* @param[in] t An element of the tangent of the Lie group.
* @param[out] -optional- J_mout_m Jacobian of the oplus operation wrt this.
* @param[out] -optional- J_mout_t Jacobian of the oplus operation wrt the tangent element.
* @return An element of the Lie group.
* @note See Eq. (27).
*/
template <typename _DerivedOther>
LieGroup lplus(const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_m = {},
OptJacobianRef J_mout_t = {}) const;
/**
* @brief An alias for the right oplus operation.
* @see rplus
*/
template <typename _DerivedOther>
LieGroup plus(const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_m = {},
OptJacobianRef J_mout_t = {}) const;
/**
* @brief Right ominus operation of the Lie group.
* @param[in] m Another element of the same Lie group.
* @param[out] -optional- J_t_ma Jacobian of the ominus operation wrt this.
* @param[out] -optional- J_t_mb Jacobian of the ominus operation wrt the other element.
* @return An element of the tangent space of the Lie group.
* @note See Eq. (26).
*/
template <typename _DerivedOther>
Tangent rminus(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_t_ma = {},
OptJacobianRef J_t_mb = {}) const;
/**
* @brief Left ominus operation of the Lie group.
* @param[in] m Another element of the same Lie group.
* @param[out] -optional- J_t_ma Jacobian of the ominus operation wrt this.
* @param[out] -optional- J_t_mb Jacobian of the ominus operation wrt the other element.
* @return An element of the tangent space of the Lie group.
* @note See Eq. (28).
*/
template <typename _DerivedOther>
Tangent lminus(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_t_ma = {},
OptJacobianRef J_t_mb = {}) const;
/**
* @brief An alias for the right ominus operation.
* @see rminus
*/
template <typename _DerivedOther>
Tangent minus(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_t_ma = {},
OptJacobianRef J_t_mb = {}) const;
/**
* @brief
* @param[in] m [description]
* @param[out] -optional- J_mc_ma [description]
* @param[out] -optional- J_mc_mb [description]
* @return [description]
*/
template <typename _DerivedOther>
LieGroup between(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}) const;
/**
* @brief Evaluate whether this and m are 'close'.
* @param[in] m An element of the same Lie Group.
* @param[in] eps Threshold for equality comparison.
* @return true if the Lie group element m is 'close' to this,
* false otherwise.
* @see TangentBase::isApprox
*/
template <typename _DerivedOther>
bool isApprox(const LieGroupBase<_DerivedOther>& m,
const Scalar eps = Constants<Scalar>::eps) const;
// Some operators
/**
* @brief Equality operator.
* @param[in] An element of the same Lie group.
* @return true if the Lie group element m is 'close' to this,
* false otherwise.
* @see isApprox.
*/
template <typename _DerivedOther>
bool operator ==(const LieGroupBase<_DerivedOther>& m) const;
/**
* @brief Inequality operator.
* @param[in] An element of the same Lie group.
* @return false if the Lie group element m is 'close' to this,
* true otherwise.
* @see operator==.
*/
template <typename _DerivedOther>
bool operator!=(
const LieGroupBase<_DerivedOther> &m) const {
return !(*this == m);
}
/**
* @brief Right oplus operator.
* @see rplus.
*/
template <typename _DerivedOther>
LieGroup operator +(const TangentBase<_DerivedOther>& t) const;
/**
* @brief Right in-place oplus operator.
* @see rplus.
*/
template <typename _DerivedOther>
_Derived& operator +=(const TangentBase<_DerivedOther>& t);
/**
* @brief Right ominus operator.
* @see rminus.
*/
template <typename _DerivedOther>
Tangent operator -(const LieGroupBase<_DerivedOther>& m) const;
/**
* @brief Lie group composition operator.
* @see compose.
*/
template <typename _DerivedOther>
LieGroup operator *(const LieGroupBase<_DerivedOther>& m) const;
/**
* @brief Lie group in-place composition operator.
* @see compose.
*/
template <typename _DerivedOther>
_Derived& operator *=(const LieGroupBase<_DerivedOther>& m);
//! Access the ith coeffs
auto operator [](const unsigned int i) const -> decltype(coeffs()[i]){
return coeffs()[i];
}
//! Access the ith coeffs
auto operator [](const unsigned int i) -> decltype(coeffs()[i]){
return coeffs()[i];
}
//! @brief The size of the underlying vector
constexpr unsigned int size() const {
return RepSize;
}
// Some static helpers
//! Static helper to create a Lie group object set at Identity.
static LieGroup Identity();
//! Static helper to create a random object of the Lie group.
static LieGroup Random();
protected:
inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); }
inline const _Derived& derived() const & noexcept { return *static_cast< const _Derived* >(this); }
};
template <typename _Derived>
constexpr int LieGroupBase<_Derived>::Dim;
template <typename _Derived>
constexpr int LieGroupBase<_Derived>::DoF;
template <typename _Derived>
constexpr int LieGroupBase<_Derived>::RepSize;
template <typename _Derived>
const typename LieGroupBase<_Derived>::OptJacobianRef
LieGroupBase<_Derived>::_ = {};
// Copy
template <typename _Derived>
_Derived&
LieGroupBase<_Derived>::operator =(const LieGroupBase& m)
{
derived().coeffs() = m.coeffs();
return derived();
}
template <typename _Derived>
template <typename _DerivedOther>
_Derived&
LieGroupBase<_Derived>::operator =(const LieGroupBase<_DerivedOther>& m)
{
derived().coeffs() = m.coeffs();
return derived();
}
template <typename _Derived>
template <typename _EigenDerived>
_Derived&
LieGroupBase<_Derived>::operator =(const Eigen::MatrixBase<_EigenDerived>& data)
{
internal::AssignmentEvaluator<
typename internal::traits<_Derived>::Base>().run(data);
derived().coeffs() = data;
return derived();
}
template <typename _Derived>
typename LieGroupBase<_Derived>::DataType&
LieGroupBase<_Derived>::coeffs()
{
return derived().coeffs();
}
template <typename _Derived>
const typename LieGroupBase<_Derived>::DataType&
LieGroupBase<_Derived>::coeffs() const
{
return derived().coeffs();
}
template <typename _Derived>
typename LieGroupBase<_Derived>::Scalar*
LieGroupBase<_Derived>::data()
{
return derived().coeffs().data();
}
template <typename _Derived>
const typename LieGroupBase<_Derived>::Scalar*
LieGroupBase<_Derived>::data() const
{
return derived().coeffs().data();
}
template <typename _Derived>
template <class _NewScalar>
typename LieGroupBase<_Derived>::template LieGroupTemplate<_NewScalar>
LieGroupBase<_Derived>::cast() const
{
return internal::CastEvaluator<
typename internal::traits<_Derived>::Base, _NewScalar
>(derived()).run();
}
template <typename _Derived>
_Derived&
LieGroupBase<_Derived>::setIdentity()
{
const static Tangent zero = Tangent::Zero();
derived() = zero.exp();
return derived();
}
template <typename _Derived>
_Derived&
LieGroupBase<_Derived>::setRandom()
{
internal::RandomEvaluator<
typename internal::traits<_Derived>::Base>(
derived()).run();
return derived();
}
template <typename _Derived>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::inverse(OptJacobianRef J_m_t) const
{
return derived().inverse(J_m_t);
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::rplus(
const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_m,
OptJacobianRef J_mout_t) const
{
if (J_mout_t)
{
(*J_mout_t) = t.rjac();
}
return compose(t.exp(), J_mout_m, _);
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::lplus(
const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_m,
OptJacobianRef J_mout_t) const
{
if (J_mout_t)
{
J_mout_t->noalias() = inverse().adj() * t.rjac();
}
if (J_mout_m)
{
J_mout_m->setIdentity();
}
return t.exp().compose(derived());
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::plus(
const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_m,
OptJacobianRef J_mout_t) const
{
return derived().rplus(t, J_mout_m, J_mout_t);
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::rminus(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_t_ma,
OptJacobianRef J_t_mb) const
{
const Tangent t = m.inverse().compose(derived()).log();
if (J_t_ma)
{
(*J_t_ma) = t.rjacinv();
}
if (J_t_mb)
{
(*J_t_mb) = -(-t).rjacinv();
}
return t;
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::lminus(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_t_ma,
OptJacobianRef J_t_mb) const
{
const Tangent t = compose(m.inverse()).log();
if (J_t_ma)
{
J_t_ma->noalias() = t.rjacinv() * m.adj();
if (J_t_mb)
{
*J_t_mb = -(*J_t_ma);
}
}
else if (J_t_mb)
{
J_t_mb->noalias() = -(t.rjacinv() * m.adj());
}
return t;
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::minus(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_t_ma,
OptJacobianRef J_t_mb) const
{
return derived().rminus(m, J_t_ma, J_t_mb);
}
template <typename _Derived>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::log(OptJacobianRef J_t_m) const
{
return derived().log(J_t_m);
}
template <typename _Derived>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::lift(OptJacobianRef J_t_m) const
{
return derived().log(J_t_m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb) const
{
return derived().compose(m, J_mc_ma, J_mc_mb);
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::between(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb) const
{
const LieGroup mc = inverse().compose(m);
if (J_mc_ma)
{
*J_mc_ma = -(mc.inverse().adj());
}
if (J_mc_mb)
{
J_mc_mb->setIdentity();
}
return mc;
}
template <typename _Derived>
template <typename _DerivedOther>
bool LieGroupBase<_Derived>::isApprox(const LieGroupBase<_DerivedOther>& m,
const Scalar eps) const
{
return rminus(m).isApprox(Tangent::Zero(), eps);
}
template <typename _Derived>
typename LieGroupBase<_Derived>::Jacobian
LieGroupBase<_Derived>::adj() const
{
return derived().adj();
}
template <typename _Derived>
template <typename _EigenDerived>
typename LieGroupBase<_Derived>::Vector
LieGroupBase<_Derived>::act(
const Eigen::MatrixBase<_EigenDerived>& v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v
) const
{
return derived().act(v, J_vout_m, J_vout_v);
}
// Operators
template <typename _Derived>
template <typename _DerivedOther>
bool LieGroupBase<_Derived>::operator ==(
const LieGroupBase<_DerivedOther>& m) const
{
return isApprox(m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::operator +(
const TangentBase<_DerivedOther>& t) const
{
return derived().rplus(t);
}
template <typename _Derived>
template <typename _DerivedOther>
_Derived&
LieGroupBase<_Derived>::operator +=(
const TangentBase<_DerivedOther>& t)
{
derived() = derived().rplus(t);
return derived();
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::Tangent
LieGroupBase<_Derived>::operator -(
const LieGroupBase<_DerivedOther>& m) const
{
return derived().rminus(m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::operator *(
const LieGroupBase<_DerivedOther>& m) const
{
return derived().compose(m);
}
template <typename _Derived>
template <typename _DerivedOther>
_Derived&
LieGroupBase<_Derived>::operator *=(
const LieGroupBase<_DerivedOther>& m)
{
derived() = derived().compose(m);
return derived();
}
// Static helpers
template <typename _Derived>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::Identity()
{
const static LieGroup I(LieGroup().setIdentity());
return I;
}
template <typename _Derived>
typename LieGroupBase<_Derived>::LieGroup
LieGroupBase<_Derived>::Random()
{
return LieGroup().setRandom();
}
// Utils
template <typename _Stream, typename _Derived>
_Stream& operator << (
_Stream& s,
const manif::LieGroupBase<_Derived>& m)
{
s << m.coeffs().transpose();
return s;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_LIE_GROUP_BASE_H_ */

329
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/macro.h vendored Normal file
View File

@ -0,0 +1,329 @@
#ifndef _MANIF_MANIF_FWD_H_
#define _MANIF_MANIF_FWD_H_
#include <stdexcept> // for std::runtime_error
#include <utility> // for std::forward
#ifdef NDEBUG
# ifndef MANIF_NO_DEBUG
# define MANIF_NO_DEBUG
# endif
#endif
namespace manif {
struct runtime_error : std::runtime_error
{
using std::runtime_error::runtime_error;
using std::runtime_error::what;
};
struct invalid_argument : std::invalid_argument
{
using std::invalid_argument::invalid_argument;
using std::invalid_argument::what;
};
namespace detail {
template <typename E, typename... Args>
void
#if defined(__GNUC__) || defined(__clang__)
__attribute__(( noinline, cold, noreturn ))
#elif defined(_MSC_VER)
__declspec( noinline, noreturn )
#else
// nothing
#endif
raise(Args&&... args)
{
throw E(std::forward<Args>(args)...);
}
} /* namespace detail */
} /* namespace manif */
#define MANIF_UNUSED_VARIABLE(x) EIGEN_UNUSED_VARIABLE(x)
// gcc expands __VA_ARGS___ before passing it into the macro.
// Visual Studio expands __VA_ARGS__ after passing it.
// This macro is a workaround to support both
#define __MANIF_EXPAND(x) x
#if defined(__cplusplus) && defined(__has_cpp_attribute)
#define __MANIF_HAVE_CPP_ATTRIBUTE(x) __has_cpp_attribute(x)
#else
#define __MANIF_HAVE_CPP_ATTRIBUTE(x) 0
#endif
#define __MANIF_THROW_EXCEPT(msg, except) manif::detail::raise<except>(msg);
#define __MANIF_THROW(msg) __MANIF_THROW_EXCEPT(msg, manif::runtime_error)
#define __MANIF_GET_MACRO_2(_1,_2,NAME,...) NAME
#define MANIF_THROW(...) \
__MANIF_EXPAND( \
__MANIF_GET_MACRO_2(__VA_ARGS__, \
__MANIF_THROW_EXCEPT, \
__MANIF_THROW)(__VA_ARGS__) )
#define __MANIF_CHECK_MSG_EXCEPT(cond, msg, except) \
if (!(cond)) {MANIF_THROW(msg, except);}
#define __MANIF_CHECK_MSG(cond, msg) \
__MANIF_CHECK_MSG_EXCEPT(cond, msg, manif::runtime_error)
#define __MANIF_CHECK(cond) \
__MANIF_CHECK_MSG_EXCEPT(cond, "Condition: '"#cond"' failed!", manif::runtime_error)
#define __MANIF_GET_MACRO_3(_1,_2,_3,NAME,...) NAME
#define MANIF_CHECK(...) \
__MANIF_EXPAND( \
__MANIF_GET_MACRO_3(__VA_ARGS__, \
__MANIF_CHECK_MSG_EXCEPT, \
__MANIF_CHECK_MSG, \
__MANIF_CHECK)(__VA_ARGS__) )
// Assertions cost run time and can be turned off.
// You can suppress MANIF_ASSERT by defining
// MANIF_NO_DEBUG before including manif headers.
// MANIF_NO_DEBUG is undefined by default unless NDEBUG is defined.
#ifndef MANIF_NO_DEBUG
#define MANIF_ASSERT(...) \
__MANIF_EXPAND( \
__MANIF_GET_MACRO_3(__VA_ARGS__, \
__MANIF_CHECK_MSG_EXCEPT, \
__MANIF_CHECK_MSG, \
__MANIF_CHECK)(__VA_ARGS__) )
#else
#define MANIF_ASSERT(...) ((void)0)
#endif
#define MANIF_NOT_IMPLEMENTED_YET \
MANIF_THROW("Not implemented yet !");
#if defined(__cplusplus) && (__cplusplus >= 201402L) && __MANIF_HAVE_CPP_ATTRIBUTE(deprecated)
#define MANIF_DEPRECATED [[deprecated]]
#elif defined(__GNUC__) || defined(__clang__)
#define MANIF_DEPRECATED __attribute__((deprecated))
#elif defined(_MSC_VER)
#define MANIF_DEPRECATED __declspec(deprecated)
#else
#pragma message("WARNING: Deprecation is disabled "\
"-- the compiler is not supported.")
#define MANIF_DEPRECATED
#endif
// Common macros
#define MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF((Eigen::internal::traits<typename Base::DataType>::Alignment>0))
#define MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND_TYPE(X) \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF((Eigen::internal::traits<typename X::DataType>::Alignment>0))
#define MANIF_MOVE_NOEXCEPT \
noexcept(std::is_nothrow_move_constructible<Scalar>::value)
#define MANIF_DEFAULT_CONSTRUCTOR(X) \
X() = default; \
~X() = default; \
X(const X&) = default; \
X(X&&) = default;
#define MANIF_GROUP_ML_ASSIGN_OP(X) \
_Derived& operator =(const X& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _DerivedOther>\
_Derived& operator =(const LieGroupBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _EigenDerived>\
_Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return derived(); } \
_Derived& operator =(X&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _DerivedOther>\
_Derived& operator =(LieGroupBase<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _EigenDerived>\
_Derived& operator =(Eigen::MatrixBase<_EigenDerived>&& o) { coeffs() = std::move(o); return derived(); }
#define MANIF_GROUP_ASSIGN_OP(X) \
X& operator=(const X& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _DerivedOther>\
X& operator =(const X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _DerivedOther>\
X& operator =(const LieGroupBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _EigenDerived>\
X& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return derived(); }\
X& operator=(X&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _DerivedOther>\
X& operator =(X##Base<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _DerivedOther>\
X& operator =(LieGroupBase<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _EigenDerived>\
X& operator =(Eigen::MatrixBase<_EigenDerived>&& o) { coeffs() = std::move(o); return derived(); }
#define MANIF_GROUP_MAP_ASSIGN_OP(X) \
Map(const Map & o) : Base(), data_(o.coeffs()) { }\
Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\
template <typename _DerivedOther>\
Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\
template <typename _DerivedOther>\
Map& operator =(const manif::LieGroupBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\
template <typename _EigenDerived>\
Map& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return *this; }\
Map& operator=(Map&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return *this; }\
template <typename _DerivedOther>\
Map& operator =(manif::X##Base<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return *this; }\
template <typename _DerivedOther>\
Map& operator =(manif::LieGroupBase<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return *this; }\
template <typename _EigenDerived>\
Map& operator =(Eigen::MatrixBase<_EigenDerived>&& o) { coeffs() = std::move(o); return *this; }
#define MANIF_TANGENT_ML_ASSIGN_OP(X) \
_Derived& operator=(const X& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _DerivedOther>\
_Derived& operator =(const TangentBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _EigenDerived>\
_Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return derived(); }\
_Derived& operator=(X&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _DerivedOther>\
_Derived& operator =(TangentBase<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _EigenDerived>\
_Derived& operator =(Eigen::MatrixBase<_EigenDerived>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o); return derived(); }
#define MANIF_TANGENT_ASSIGN_OP(X) \
X& operator=(const X& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _DerivedOther>\
X& operator =(const X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _DerivedOther>\
X& operator =(const TangentBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return derived(); }\
template <typename _EigenDerived>\
X& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return derived(); }\
X& operator=(X&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _DerivedOther>\
X& operator =(X##Base<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _DerivedOther>\
X& operator =(TangentBase<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return derived(); }\
template <typename _EigenDerived>\
X& operator =(Eigen::MatrixBase<_EigenDerived>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o); return derived(); }
#define MANIF_TANGENT_MAP_ASSIGN_OP(X) \
Map(const Map & o) : Base(), data_(o.coeffs()) { }\
Map& operator=(const Map& o) { coeffs() = o.coeffs(); return *this; }\
template <typename _DerivedOther>\
Map& operator =(const manif::X##Base<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\
template <typename _DerivedOther>\
Map& operator =(const manif::TangentBase<_DerivedOther>& o) { coeffs() = o.coeffs(); return *this; }\
template <typename _EigenDerived>\
Map& operator =(const Eigen::MatrixBase<_EigenDerived>& o) { coeffs() = o; return *this; }\
Map& operator=(Map&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return *this; }\
template <typename _DerivedOther>\
Map& operator =(manif::X##Base<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return *this; }\
template <typename _DerivedOther>\
Map& operator =(manif::TangentBase<_DerivedOther>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o.coeffs()); return *this; }\
template <typename _EigenDerived>\
Map& operator =(Eigen::MatrixBase<_EigenDerived>&& o) MANIF_MOVE_NOEXCEPT { coeffs() = std::move(o); return *this; }
/**
* @brief Automatically define:
* - copy constructor
* - copy constructor given Base object
* - copy constructor given Eigen object
*/
#define MANIF_COPY_CONSTRUCTOR(X) \
X(const X& o) : Base(), data_(o.coeffs()) { } \
X(const Base& o) : Base(), data_(o.coeffs()) { } \
template <typename D> X(const Eigen::MatrixBase<D>& o) : Base(), data_(o) \
{ manif::internal::AssignmentEvaluator<Base>().run(data_); }
#define MANIF_MOVE_CONSTRUCTOR(X) \
X(X&& o) MANIF_MOVE_NOEXCEPT : Base(), data_(std::move(o.coeffs())) { } \
X(Base&& o) MANIF_MOVE_NOEXCEPT : Base(), data_(std::move(o.coeffs())) { } \
template <typename D> X(Eigen::MatrixBase<D>&& o) : Base(), data_(std::move(o)) \
{ manif::internal::AssignmentEvaluator<Base>().run(data_); }
#define MANIF_COEFFS_FUNCTIONS() \
DataType& coeffs() & { return data_; } \
const DataType& coeffs() const & { return data_; }
// LieGroup - related macros
#define MANIF_INHERIT_GROUP_AUTO_API \
using Base::setRandom; \
using Base::rplus; \
using Base::lplus; \
using Base::rminus; \
using Base::lminus; \
using Base::between;
#define MANIF_INHERIT_GROUP_API \
MANIF_INHERIT_GROUP_AUTO_API \
using Base::setIdentity; \
using Base::inverse; \
using Base::lift; \
using Base::log; \
using Base::adj;
#define MANIF_INHERIT_GROUP_OPERATOR \
using Base::operator +; \
using Base::operator +=; \
using Base::operator -; \
using Base::operator *; \
using Base::operator *=; \
using Base::operator =;
#define MANIF_GROUP_PROPERTIES \
using Base::Dim; \
using Base::DoF;
#define MANIF_GROUP_TYPEDEF \
MANIF_GROUP_PROPERTIES \
using Scalar = typename Base::Scalar; \
using LieGroup = typename Base::LieGroup; \
using Tangent = typename Base::Tangent; \
using Jacobian = typename Base::Jacobian; \
using DataType = typename Base::DataType; \
using Vector = typename Base::Vector; \
using OptJacobianRef = typename Base::OptJacobianRef;
#define MANIF_COMPLETE_GROUP_TYPEDEF \
MANIF_GROUP_TYPEDEF \
MANIF_INHERIT_GROUP_OPERATOR
#define MANIF_EXTRA_GROUP_TYPEDEF(group) \
using group##f = group<float>; \
using group##d = group<double>;
// Tangent - related macros
#define MANIF_INHERIT_TANGENT_API \
using Base::setZero; \
using Base::setRandom; \
using Base::retract; \
using Base::exp; \
using Base::hat; \
using Base::rjac; \
using Base::ljac; \
using Base::smallAdj;
#define MANIF_INHERIT_TANGENT_OPERATOR \
using Base::operator +=; \
using Base::operator -=; \
using Base::operator *=; \
using Base::operator /=; \
using Base::operator =; \
using Base::operator <<;
#define MANIF_TANGENT_PROPERTIES \
using Base::Dim; \
using Base::DoF;
#define MANIF_TANGENT_TYPEDEF \
MANIF_TANGENT_PROPERTIES \
using Scalar = typename Base::Scalar; \
using LieGroup = typename Base::LieGroup; \
using Tangent = typename Base::Tangent; \
using Jacobian = typename Base::Jacobian; \
using DataType = typename Base::DataType; \
using LieAlg = typename Base::LieAlg; \
using OptJacobianRef = typename Base::OptJacobianRef;
#define MANIF_EXTRA_TANGENT_TYPEDEF(tangent) \
using tangent##f = tangent<float>; \
using tangent##d = tangent<double>;
#endif /* _MANIF_MANIF_FWD_H_ */

39
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/random.h vendored Normal file
View File

@ -0,0 +1,39 @@
#ifndef _MANIF_MANIF_IMPL_RANDOM_H_
#define _MANIF_MANIF_IMPL_RANDOM_H_
namespace manif {
namespace internal {
template <typename Derived>
struct RandomEvaluatorImpl
{
template <typename T>
static void run(T&)
{
/// @todo print actual Derived type
static_assert(constexpr_false<Derived>(),
"RandomEvaluator not overloaded for Derived type!");
}
};
template <typename Derived>
struct RandomEvaluator : RandomEvaluatorImpl<Derived>
{
using Base = RandomEvaluatorImpl<Derived>;
RandomEvaluator(Derived& xptr) : xptr_(xptr) {}
void run()
{
Base::run(xptr_);
}
protected:
Derived& xptr_;
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_IMPL_RANDOM_H_ */

141
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/rn/Rn.h vendored Normal file
View File

@ -0,0 +1,141 @@
#ifndef _MANIF_MANIF_RN_H_
#define _MANIF_MANIF_RN_H_
#include "manif/impl/rn/Rn_base.h"
namespace manif {
// Forward declare for type traits specialization
template <typename _Scalar, unsigned int N> struct Rn;
template <typename _Scalar, unsigned int N> struct RnTangent;
namespace internal {
//! Traits specialization
template <typename _Scalar, unsigned int _N>
struct traits<Rn<_Scalar, _N>>
{
using Scalar = _Scalar;
using LieGroup = Rn<_Scalar, _N>;
using Tangent = RnTangent<_Scalar, _N>;
using Base = RnBase<Rn<_Scalar, _N>>;
static constexpr int Dim = _N;
static constexpr int DoF = _N;
static constexpr int RepSize = _N;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using Transformation = Eigen::Matrix<Scalar, DoF, DoF>;
using Vector = Eigen::Matrix<Scalar, DoF, 1>;
};
} // namespace internal
} // namespace manif
namespace manif {
//
// LieGroup
//
/**
* @brief Represents an element of Rn.
*/
template <typename _Scalar, unsigned int _N>
struct Rn : RnBase<Rn<_Scalar, _N>>
{
private:
static_assert(_N > 0, "N must be greater than 0 !");
using Base = RnBase<Rn<_Scalar, _N>>;
using Type = Rn<_Scalar, _N>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
Rn() = default;
~Rn() = default;
MANIF_COPY_CONSTRUCTOR(Rn)
MANIF_MOVE_CONSTRUCTOR(Rn)
// Copy constructor given base
template <typename _DerivedOther>
Rn(const LieGroupBase<_DerivedOther>& o);
MANIF_GROUP_ASSIGN_OP(Rn)
// LieGroup common API
//! Get a reference to the underlying DataType.
DataType& coeffs();
//! Get a const reference to the underlying DataType.
const DataType& coeffs() const;
// Rn specific API
protected:
DataType data_;
};
template <typename _Scalar> using R1 = Rn<_Scalar, 1>;
template <typename _Scalar> using R2 = Rn<_Scalar, 2>;
template <typename _Scalar> using R3 = Rn<_Scalar, 3>;
template <typename _Scalar> using R4 = Rn<_Scalar, 4>;
template <typename _Scalar> using R5 = Rn<_Scalar, 5>;
template <typename _Scalar> using R6 = Rn<_Scalar, 6>;
template <typename _Scalar> using R7 = Rn<_Scalar, 7>;
template <typename _Scalar> using R8 = Rn<_Scalar, 8>;
template <typename _Scalar> using R9 = Rn<_Scalar, 9>;
MANIF_EXTRA_GROUP_TYPEDEF(R1)
MANIF_EXTRA_GROUP_TYPEDEF(R2)
MANIF_EXTRA_GROUP_TYPEDEF(R3)
MANIF_EXTRA_GROUP_TYPEDEF(R4)
MANIF_EXTRA_GROUP_TYPEDEF(R5)
MANIF_EXTRA_GROUP_TYPEDEF(R6)
MANIF_EXTRA_GROUP_TYPEDEF(R7)
MANIF_EXTRA_GROUP_TYPEDEF(R8)
MANIF_EXTRA_GROUP_TYPEDEF(R9)
template <typename _Scalar, unsigned int _N>
template <typename _DerivedOther>
Rn<_Scalar, _N>::Rn(const LieGroupBase<_DerivedOther>& o)
: Rn(o.coeffs())
{
//
}
template <typename _Scalar, unsigned int _N>
typename Rn<_Scalar, _N>::DataType&
Rn<_Scalar, _N>::coeffs()
{
return data_;
}
template <typename _Scalar, unsigned int _N>
const typename Rn<_Scalar, _N>::DataType&
Rn<_Scalar, _N>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_RN_H_ */

130
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/rn/RnTangent.h vendored Normal file
View File

@ -0,0 +1,130 @@
#ifndef _MANIF_MANIF_RNTANGENT_H_
#define _MANIF_MANIF_RNTANGENT_H_
#include "manif/impl/rn/RnTangent_base.h"
namespace manif {
namespace internal {
//! Traits specialization
template <typename _Scalar, unsigned int _N>
struct traits<RnTangent<_Scalar, _N>>
{
using Scalar = _Scalar;
using LieGroup = Rn<_Scalar, _N>;
using Tangent = RnTangent<_Scalar, _N>;
using Base = RnTangentBase<Tangent>;
static constexpr int Dim = _N;
static constexpr int DoF = _N;
static constexpr int RepSize = _N;
using DataType = Eigen::Matrix<Scalar, DoF, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using LieAlg = Eigen::Matrix<Scalar, DoF+1, DoF+1>;
};
} // namespace internal
} // namespace manif
namespace manif {
//
// Tangent
//
/**
* @brief Represents an element of tangent space of Rn.
*/
template <typename _Scalar, unsigned int _N>
struct RnTangent : RnTangentBase<RnTangent<_Scalar, _N>>
{
private:
static_assert(_N > 0, "N must be greater than 0 !");
using Base = RnTangentBase<RnTangent<_Scalar, _N>>;
using Type = RnTangent<_Scalar, _N>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
RnTangent() = default;
~RnTangent() = default;
MANIF_COPY_CONSTRUCTOR(RnTangent)
MANIF_MOVE_CONSTRUCTOR(RnTangent)
// Copy constructor given base
template <typename _DerivedOther>
RnTangent(const TangentBase<_DerivedOther>& o);
MANIF_TANGENT_ASSIGN_OP(RnTangent)
// Tangent common API
DataType& coeffs();
const DataType& coeffs() const;
protected:
DataType data_;
};
template <typename _Scalar> using R1Tangent = RnTangent<_Scalar, 1>;
template <typename _Scalar> using R2Tangent = RnTangent<_Scalar, 2>;
template <typename _Scalar> using R3Tangent = RnTangent<_Scalar, 3>;
template <typename _Scalar> using R4Tangent = RnTangent<_Scalar, 4>;
template <typename _Scalar> using R5Tangent = RnTangent<_Scalar, 5>;
template <typename _Scalar> using R6Tangent = RnTangent<_Scalar, 6>;
template <typename _Scalar> using R7Tangent = RnTangent<_Scalar, 7>;
template <typename _Scalar> using R8Tangent = RnTangent<_Scalar, 8>;
template <typename _Scalar> using R9Tangent = RnTangent<_Scalar, 9>;
MANIF_EXTRA_TANGENT_TYPEDEF(R1Tangent)
MANIF_EXTRA_TANGENT_TYPEDEF(R2Tangent)
MANIF_EXTRA_TANGENT_TYPEDEF(R3Tangent)
MANIF_EXTRA_TANGENT_TYPEDEF(R4Tangent)
MANIF_EXTRA_TANGENT_TYPEDEF(R5Tangent)
MANIF_EXTRA_TANGENT_TYPEDEF(R6Tangent)
MANIF_EXTRA_TANGENT_TYPEDEF(R7Tangent)
MANIF_EXTRA_TANGENT_TYPEDEF(R8Tangent)
MANIF_EXTRA_TANGENT_TYPEDEF(R9Tangent)
template <typename _Scalar, unsigned int _N>
template <typename _DerivedOther>
RnTangent<_Scalar, _N>::RnTangent(const TangentBase<_DerivedOther>& o)
: data_(o.coeffs())
{
//
}
template <typename _Scalar, unsigned int _N>
typename RnTangent<_Scalar, _N>::DataType&
RnTangent<_Scalar, _N>::coeffs()
{
return data_;
}
template <typename _Scalar, unsigned int _N>
const typename RnTangent<_Scalar, _N>::DataType&
RnTangent<_Scalar, _N>::coeffs() const
{
return data_;
}
} // namespace manif
#endif // _MANIF_MANIF_RNTANGENT_H_

226
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/rn/RnTangent_base.h vendored Normal file
View File

@ -0,0 +1,226 @@
#ifndef _MANIF_MANIF_RNTANGENT_BASE_H_
#define _MANIF_MANIF_RNTANGENT_BASE_H_
#include "manif/impl/rn/Rn_properties.h"
#include "manif/impl/tangent_base.h"
namespace manif {
//
// Tangent
//
/**
* @brief The base class of the R^n tangent.
* @note See Appendix E.
*/
template <typename _Derived>
struct RnTangentBase : TangentBase<_Derived>
{
private:
using Base = TangentBase<_Derived>;
using Type = RnTangentBase<_Derived>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_OPERATOR
using Base::coeffs;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(RnTangentBase)
public:
MANIF_TANGENT_ML_ASSIGN_OP(RnTangentBase)
// Tangent common API
/**
* @brief Hat operator of Rn.
* @return An element of the Lie algebra rn.
* @note See Appendix E.
*/
LieAlg hat() const;
/**
* @brief Get the Rn element.
* @param[out] -optional- J_m_t Jacobian of the Rn element wrt this.
* @return The Rn element.
* @note This is the exp() map with the argument in vector form.
* @note See Eqs. (184) and Eq. (191).
*/
LieGroup exp(OptJacobianRef J_m_t = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t = {}) const;
/**
* @brief Get the right Jacobian of Rn.
* @note See Eq. (191).
*/
Jacobian rjac() const;
/**
* @brief Get the left Jacobian of Rn.
* @note See Eq. (191).
*/
Jacobian ljac() const;
/**
* @brief Get the inverse of the right Jacobian of Rn.
* @note See Eq. (191).
* @see rjac.
*/
Jacobian rjacinv() const;
/**
* @brief Get the inverse of the left Jacobian of Rn.
* @note See Eq. (191).
* @see ljac.
*/
Jacobian ljacinv() const;
/**
* @brief
* @return
*/
Jacobian smallAdj() const;
// RnTangent specific API
};
template <typename _Derived>
typename RnTangentBase<_Derived>::LieGroup
RnTangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
if (J_m_t)
{
J_m_t->setIdentity();
}
return LieGroup(coeffs());
}
template <typename _Derived>
typename RnTangentBase<_Derived>::LieGroup
RnTangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
return exp(J_m_t);
}
template <typename _Derived>
typename RnTangentBase<_Derived>::LieAlg
RnTangentBase<_Derived>::hat() const
{
LieAlg t_hat = LieAlg::Zero();
t_hat.template topRightCorner<Dim, 1>() = coeffs();
return t_hat;
}
template <typename _Derived>
typename RnTangentBase<_Derived>::Jacobian
RnTangentBase<_Derived>::rjac() const
{
static const Jacobian Jr = Jacobian::Identity();
return Jr;
}
template <typename _Derived>
typename RnTangentBase<_Derived>::Jacobian
RnTangentBase<_Derived>::ljac() const
{
static const Jacobian Jl = Jacobian::Identity();
return Jl;
}
template <typename _Derived>
typename RnTangentBase<_Derived>::Jacobian
RnTangentBase<_Derived>::rjacinv() const
{
return rjac();
}
template <typename _Derived>
typename RnTangentBase<_Derived>::Jacobian
RnTangentBase<_Derived>::ljacinv() const
{
return ljac();
}
template <typename _Derived>
typename RnTangentBase<_Derived>::Jacobian
RnTangentBase<_Derived>::smallAdj() const
{
static const Jacobian smallAdj = Jacobian::Zero();
return smallAdj;
}
// RnTangent specific API
namespace internal {
/**
* @brief Generator specialization for RnTangentBase objects.
*/
template <typename Derived>
struct GeneratorEvaluator<RnTangentBase<Derived>>
{
static typename RnTangentBase<Derived>::LieAlg
run(const unsigned int i)
{
MANIF_CHECK(i<RnTangentBase<Derived>::DoF,
"Index i must less than DoF!",
invalid_argument);
using LieAlg = typename RnTangentBase<Derived>::LieAlg;
LieAlg Ei = LieAlg::Zero();
Ei(i, RnTangentBase<Derived>::DoF) = 1;
return Ei;
}
};
//! @brief Random specialization for RnTangentBase objects.
template <typename Derived>
struct RandomEvaluatorImpl<RnTangentBase<Derived>>
{
static void run(RnTangentBase<Derived>& m)
{
m.coeffs().setRandom();
}
};
//! @brief Bracket specialization for RnTangentBase objects.
template <typename Derived>
struct BracketEvaluatorImpl<RnTangentBase<Derived>> {
template <typename TL, typename TR>
static typename Derived::Tangent run(const TL&, const TR&) {
return Derived::Tangent::Zero();
}
};
//! @brief Vee specialization for RnTangentBase objects.
template <typename Derived>
struct VeeEvaluatorImpl<RnTangentBase<Derived>> {
template <typename TL, typename TR>
static void run(TL& t, const TR& v) {
t.coeffs() = v.template topRightCorner<Derived::Dim, 1>();
}
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_RNTANGENT_BASE_H_

85
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/rn/RnTangent_map.h vendored Normal file
View File

@ -0,0 +1,85 @@
#ifndef _MANIF_MANIF_RNTANGENT_MAP_H_
#define _MANIF_MANIF_RNTANGENT_MAP_H_
#include "manif/impl/rn/RnTangent.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar, unsigned int _N>
struct traits< Eigen::Map<RnTangent<_Scalar, _N>,0> >
: public traits<RnTangent<_Scalar, _N>>
{
using typename traits<RnTangent<_Scalar, _N>>::Scalar;
using traits<RnTangent<_Scalar, _N>>::DoF;
using DataType = ::Eigen::Map<Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = RnTangentBase<Eigen::Map<RnTangent<Scalar, _N>, 0>>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar, unsigned int _N>
struct traits< Eigen::Map<const RnTangent<_Scalar, _N>,0> >
: public traits<const RnTangent<_Scalar, _N>>
{
using typename traits<const RnTangent<_Scalar, _N>>::Scalar;
using traits<const RnTangent<_Scalar, _N>>::DoF;
using DataType = ::Eigen::Map<const Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = RnTangentBase<const Eigen::Map<RnTangent<Scalar, _N>, 0>>;
};
} // namespace internal
} // namespace manif
namespace Eigen {
//! @brief Specialization of Map for manif::RnTangent
template <class _Scalar, unsigned int _N>
class Map<manif::RnTangent<_Scalar, _N>, 0>
: public manif::RnTangentBase<Map<manif::RnTangent<_Scalar, _N>, 0> >
{
using Base = manif::RnTangentBase<Map<manif::RnTangent<_Scalar, _N>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_TANGENT_MAP_ASSIGN_OP(RnTangent)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
//! @brief Specialization of Map for const manif::RnTangent
template <class _Scalar, unsigned int _N>
class Map<const manif::RnTangent<_Scalar, _N>, 0>
: public manif::RnTangentBase<Map<const manif::RnTangent<_Scalar, _N>, 0> >
{
using Base = manif::RnTangentBase<Map<const manif::RnTangent<_Scalar, _N>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} // namespace Eigen
#endif // _MANIF_MANIF_RNTANGENT_MAP_H_

224
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/rn/Rn_base.h vendored Normal file
View File

@ -0,0 +1,224 @@
#ifndef _MANIF_MANIF_RN_BASE_H_
#define _MANIF_MANIF_RN_BASE_H_
#include "manif/impl/rn/Rn_properties.h"
#include "manif/impl/lie_group_base.h"
namespace manif {
//
// LieGroup
//
/**
* @brief The base class of the Rn group.
* @note See Appendix E.
*/
template <typename _Derived>
struct RnBase : LieGroupBase<_Derived>
{
private:
using Base = LieGroupBase<_Derived>;
using Type = RnBase<_Derived>;
public:
MANIF_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_AUTO_API
MANIF_INHERIT_GROUP_OPERATOR
using Base::coeffs;
using Transformation = typename internal::traits<_Derived>::Transformation;
// LieGroup common API
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(RnBase)
public:
MANIF_GROUP_ML_ASSIGN_OP(RnBase)
/**
* @brief Get the inverse of this.
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this.
* @note r^-1 = -r
* @note See Appendix E and Eq. (189).
*/
LieGroup inverse(OptJacobianRef J_minv_m = {}) const;
/**
* @brief Get the Rn corresponding Lie algebra element in vector form.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt to this.
* @return The Rn tangent of this.
* @note This is the log() map in vector form.
* @note See Appendix E.
* @see RnTangent.
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another Rn element.
* @param[in] m Another Rn element.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
* @note See Eq. (190).
*/
template <typename _DerivedOther>
LieGroup compose(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}) const;
/**
* @brief Translation action on a 2-vector.
* @param v A 2-vector.
* @param[out] -optional- J_vout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object.
* @return The translated 2-vector.
*/
template <typename _EigenDerived>
auto
act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v = {}) const
-> Eigen::Matrix<Scalar, Dim, 1>;
/**
* @brief Get the ajoint matrix of Rn at this.
* @note See Eqs. (188).
*/
Jacobian adj() const;
// Rn specific functions
/**
* @brief Get the transformation matrix (2D isometry).
* @note T = | 0 t |
* | 0 1 |
*/
Transformation transform() const;
};
template <typename _Derived>
typename RnBase<_Derived>::Transformation
RnBase<_Derived>::transform() const
{
Transformation T(Transformation::Identity());
T.template topRightCorner<Dim,1>() = coeffs();
return T;
}
template <typename _Derived>
typename RnBase<_Derived>::LieGroup
RnBase<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
if (J_minv_m)
J_minv_m->setIdentity() *= Scalar(-1);
return LieGroup(-coeffs());
}
template <typename _Derived>
typename RnBase<_Derived>::Tangent
RnBase<_Derived>::log(OptJacobianRef J_t_m) const
{
if (J_t_m)
J_t_m->setIdentity();
return Tangent(coeffs());
}
template <typename _Derived>
typename RnBase<_Derived>::Tangent
RnBase<_Derived>::lift(OptJacobianRef J_t_m) const
{
return log(J_t_m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename RnBase<_Derived>::LieGroup
RnBase<_Derived>::compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb) const
{
static_assert(
std::is_base_of<RnBase<_DerivedOther>, _DerivedOther>::value,
"Argument does not inherit from RnBase !");
static_assert(
RnBase<_DerivedOther>::Dim==_DerivedOther::Dim, "Dimension mismatch !");
if (J_mc_ma)
J_mc_ma->setIdentity();
if (J_mc_mb)
J_mc_mb->setIdentity();
return LieGroup(coeffs() + m.coeffs());
}
template <typename _Derived>
template <typename _EigenDerived>
// Eigen::Matrix<typename RnBase<_Derived>::Scalar, RnBase<_Derived>::Dim, 1>
auto
RnBase<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, DoF>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, Dim, Dim>>> J_vout_v) const
-> Eigen::Matrix<Scalar, Dim, 1>
{
assert_vector_dim(v, Dim);
if (J_vout_m)
{
J_vout_m->setIdentity();
}
if (J_vout_v)
{
J_vout_v->setIdentity();
}
return coeffs() + v;
}
template <typename _Derived>
typename RnBase<_Derived>::Jacobian
RnBase<_Derived>::adj() const
{
static const Jacobian adj = Jacobian::Identity();
return adj;
}
namespace internal {
//! @brief Random specialization for RnBase objects.
template <typename Derived>
struct RandomEvaluatorImpl<RnBase<Derived>>
{
template <typename T>
static void run(T& m)
{
using Tangent = typename Derived::Tangent;
m = Tangent::Random().exp();
}
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_RN_BASE_H_

87
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/rn/Rn_map.h vendored Normal file
View File

@ -0,0 +1,87 @@
#ifndef _MANIF_MANIF_RN_MAP_H_
#define _MANIF_MANIF_RN_MAP_H_
#include "manif/impl/rn/Rn.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar, unsigned int _N>
struct traits< Eigen::Map<Rn<_Scalar, _N>,0> >
: public traits<Rn<_Scalar, _N>>
{
using typename traits<Rn<_Scalar, _N>>::Scalar;
using traits<Rn<Scalar, _N>>::RepSize;
using Base = RnBase<Eigen::Map<Rn<Scalar, _N>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar, unsigned int _N>
struct traits< Eigen::Map<const Rn<_Scalar, _N>,0> >
: public traits<const Rn<_Scalar, _N>>
{
using typename traits<const Rn<_Scalar, _N>>::Scalar;
using traits<const Rn<Scalar, _N>>::RepSize;
using Base = RnBase<Eigen::Map<const Rn<Scalar, _N>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
} // namespace internal
} // namespace manif
namespace Eigen {
/**
* @brief Specialization of Map for manif::Rn
*/
template <class _Scalar, unsigned int _N>
class Map<manif::Rn<_Scalar, _N>, 0>
: public manif::RnBase<Map<manif::Rn<_Scalar, _N>, 0> >
{
using Base = manif::RnBase<Map<manif::Rn<_Scalar, _N>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_GROUP_MAP_ASSIGN_OP(Rn)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::Rn
*/
template <class _Scalar, unsigned int _N>
class Map<const manif::Rn<_Scalar, _N>, 0>
: public manif::RnBase<Map<const manif::Rn<_Scalar, _N>, 0> >
{
using Base = manif::RnBase<Map<const manif::Rn<_Scalar, _N>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} // namespace Eigen
#endif // _MANIF_MANIF_RN_MAP_H_

33
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/rn/Rn_properties.h vendored Normal file
View File

@ -0,0 +1,33 @@
#ifndef _MANIF_MANIF_RN_PROPERTIES_H_
#define _MANIF_MANIF_RN_PROPERTIES_H_
#include "manif/impl/traits.h"
namespace manif {
// Forward declaration
template <typename _Derived> struct RnBase;
template <typename _Derived> struct RnTangentBase;
namespace internal {
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<RnBase<_Derived>>
{
static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension
static constexpr int DoF = traits<_Derived>::Dim; /// @brief Degrees of freedom
};
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<RnTangentBase<_Derived>>
{
static constexpr int Dim = traits<_Derived>::Dim; /// @brief Space dimension
static constexpr int DoF = traits<_Derived>::Dim; /// @brief Degrees of freedom
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_RN_PROPERTIES_H_

226
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se2/SE2.h vendored Normal file
View File

@ -0,0 +1,226 @@
#ifndef _MANIF_MANIF_SE2_H_
#define _MANIF_MANIF_SE2_H_
#include "manif/impl/se2/SE2_base.h"
namespace manif {
// Forward declare for type traits specialization
template <typename _Scalar> struct SE2;
template <typename _Scalar> struct SE2Tangent;
namespace internal {
//! traits specialization
template <typename _Scalar>
struct traits<SE2<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SE2<_Scalar>;
using Tangent = SE2Tangent<_Scalar>;
using Base = SE2Base<SE2<_Scalar>>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = 4;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using Transformation = Eigen::Matrix<Scalar, 3, 3>;
using Rotation = Eigen::Matrix<Scalar, Dim, Dim>;
using Translation = Eigen::Matrix<Scalar, Dim, 1>;
using Vector = Eigen::Matrix<Scalar, Dim, 1>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// LieGroup
//
/**
* @brief Represents an element of SE2.
*/
template <typename _Scalar>
struct SE2 : SE2Base<SE2<_Scalar>>
{
private:
using Base = SE2Base<SE2<_Scalar>>;
using Type = SE2<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_COMPLETE_GROUP_TYPEDEF
using Translation = typename Base::Translation;
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
using Base::normalize;
SE2() = default;
~SE2() = default;
MANIF_COPY_CONSTRUCTOR(SE2)
MANIF_MOVE_CONSTRUCTOR(SE2)
// Copy constructor
template <typename _DerivedOther>
SE2(const LieGroupBase<_DerivedOther>& o);
MANIF_GROUP_ASSIGN_OP(SE2)
/**
* @brief Constructor given a translation and a unit complex number.
* @param[in] t A translation vector.
* @param[in] c A complex number.
* @throws manif::invalid_argument on un-normalized complex number.
*/
SE2(const Translation& t, const std::complex<Scalar>& c);
/**
* @brief Constructor given the x and y components of the translational part
* and an angle.
* @param[in] x The x-components of the translational part.
* @param[in] y The y-components of the translational part.
* @param[in] c An angle.
*/
SE2(const Scalar x, const Scalar y, const Scalar theta);
/**
* @brief Constructor given the x and y components of the translational part
* and the real and imaginary part of a unit complex number.
* @param[in] x The x-components of the translational part.
* @param[in] y The y-components of the translational part.
* @param[in] real The real of a unitary complex number.
* @param[in] imag The imaginary of a unitary complex number.
* @throws manif::invalid_argument on un-normalized complex number.
*/
SE2(const Scalar x, const Scalar y, const Scalar real, const Scalar imag);
/**
* @brief Constructor given the x and y components of the translational part
* and the real and imaginary part of a unit complex number.
* @param[in] x The x-components of the translational part.
* @param[in] y The y-components of the translational part.
* @param[in] c The unitary complex number.
* @throws manif::invalid_argument on un-normalized complex number.
*/
SE2(const Scalar x, const Scalar y, const std::complex<Scalar>& c);
/**
* @brief Constructor from a 2D Eigen::Isometry<Scalar>
* @param[in] h an isometry object from Eigen
*
* Isometry is a typedef from Eigen::Transform,
* in which the linear part is assumed a rotation matrix.
* This is used to speed up certain methods of Transform, especially inverse().
*/
SE2(const Eigen::Transform<_Scalar,2,Eigen::Isometry>& h);
// LieGroup common API
/**
* @brief Access the underlying data
* @param[out] a reference to the underlying Eigen vector
*/
DataType& coeffs();
/**
* @brief Access the underlying data
* @param[out] a const reference to the underlying Eigen vector
*/
const DataType& coeffs() const;
// SE2 specific API
using Base::angle;
using Base::real;
using Base::imag;
using Base::x;
using Base::y;
protected:
//! Underlying data (Eigen) vector
DataType data_;
};
MANIF_EXTRA_GROUP_TYPEDEF(SE2)
template <typename _Scalar>
template <typename _DerivedOther>
SE2<_Scalar>::SE2(const LieGroupBase<_DerivedOther>& o)
: SE2(o.coeffs())
{
//
}
template <typename _Scalar>
SE2<_Scalar>::SE2(const Translation& t,
const std::complex<Scalar>& c)
: SE2((DataType() << t, c.real(), c.imag()).finished())
{
//
}
template <typename _Scalar>
SE2<_Scalar>::SE2(const Scalar x, const Scalar y, const Scalar theta)
: SE2(DataType(x, y, cos(theta), sin(theta)))
{
using std::cos;
using std::sin;
}
template <typename _Scalar>
SE2<_Scalar>::SE2(const Scalar x, const Scalar y,
const Scalar real, const Scalar imag)
: SE2(DataType(x, y, real, imag))
{
//
}
template <typename _Scalar>
SE2<_Scalar>::SE2(const Scalar x, const Scalar y, const std::complex<Scalar>& c)
: SE2(x, y, c.real(), c.imag())
{
//
}
template <typename _Scalar>
SE2<_Scalar>::SE2(const Eigen::Transform<_Scalar, 2, Eigen::Isometry>& h)
: SE2(h.translation().x(), h.translation().y(), Eigen::Rotation2D<Scalar>(h.rotation()).angle())
{
//
}
template <typename _Scalar>
typename SE2<_Scalar>::DataType&
SE2<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SE2<_Scalar>::DataType&
SE2<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SE2_H_ */

125
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se2/SE2Tangent.h vendored Normal file
View File

@ -0,0 +1,125 @@
#ifndef _MANIF_MANIF_SE2TANGENT_H_
#define _MANIF_MANIF_SE2TANGENT_H_
#include "manif/impl/se2/SE2Tangent_base.h"
namespace manif {
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SE2Tangent<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SE2<_Scalar>;
using Tangent = SE2Tangent<_Scalar>;
using Base = SE2TangentBase<Tangent>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = DoF;
using DataType = Eigen::Matrix<Scalar, DoF, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using LieAlg = Eigen::Matrix<Scalar, 3, 3>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// Tangent
//
/**
* @brief Represent an element of the tangent space of SE2.
*/
template <typename _Scalar>
struct SE2Tangent : SE2TangentBase<SE2Tangent<_Scalar>>
{
private:
using Base = SE2TangentBase<SE2Tangent<_Scalar>>;
using Type = SE2Tangent<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
SE2Tangent() = default;
~SE2Tangent() = default;
MANIF_COPY_CONSTRUCTOR(SE2Tangent)
MANIF_MOVE_CONSTRUCTOR(SE2Tangent)
// Copy constructor given base
template <typename _DerivedOther>
SE2Tangent(const TangentBase<_DerivedOther>& o);
MANIF_TANGENT_ASSIGN_OP(SE2Tangent)
SE2Tangent(const Scalar x, const Scalar y, const Scalar theta);
// Tangent common API
DataType& coeffs();
const DataType& coeffs() const;
// SE2Tangent specific API
using Base::angle;
protected:
DataType data_;
};
MANIF_EXTRA_TANGENT_TYPEDEF(SE2Tangent);
template <typename _Scalar>
template <typename _DerivedOther>
SE2Tangent<_Scalar>::SE2Tangent(const TangentBase<_DerivedOther>& o)
: data_(o.coeffs())
{
//
}
template <typename _Scalar>
SE2Tangent<_Scalar>::SE2Tangent(const Scalar x,
const Scalar y,
const Scalar theta)
: SE2Tangent(DataType(x, y, theta))
{
//
}
template <typename _Scalar>
typename SE2Tangent<_Scalar>::DataType&
SE2Tangent<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SE2Tangent<_Scalar>::DataType&
SE2Tangent<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SE2TANGENT_H_ */

495
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se2/SE2Tangent_base.h vendored Normal file
View File

@ -0,0 +1,495 @@
#ifndef _MANIF_MANIF_SE2TANGENT_BASE_H_
#define _MANIF_MANIF_SE2TANGENT_BASE_H_
#include "manif/impl/se2/SE2_properties.h"
#include "manif/impl/tangent_base.h"
namespace manif {
//
// Tangent
//
/**
* @brief The base class of the SE2 tangent.
* @note See Appendix C.
*/
template <typename _Derived>
struct SE2TangentBase : TangentBase<_Derived>
{
private:
using Base = TangentBase<_Derived>;
using Type = SE2TangentBase<_Derived>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
using Base::data;
using Base::coeffs;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SE2TangentBase)
public:
MANIF_TANGENT_ML_ASSIGN_OP(SE2TangentBase)
// Tangent common API
/**
* @brief Hat operator of SE2.
* @return An element of the Lie algebra se2 (skew-symmetric matrix).
* @note See Eq. (153).
*/
LieAlg hat() const;
/**
* @brief Get the SE2 element.
* @param[out] -optional- J_m_t Jacobian of the SE2 element wrt this.
* @return The SE2 element.
* @note This is the exp() map with the argument in vector form.
* @note See Eqs. (156,158) & Eq. (163).
*/
LieGroup exp(OptJacobianRef J_m_t = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t = {}) const;
/**
* @brief Get the right Jacobian of SE2.
* @note See Eq. (163).
*/
Jacobian rjac() const;
/**
* @brief Get the inverse right Jacobian of SE2.
*/
Jacobian rjacinv() const;
/**
* @brief Get the left Jacobian of SE2.
* @note See Eq. (164).
*/
Jacobian ljac() const;
/**
* @brief Get the inverse left Jacobian of SE2.
*/
Jacobian ljacinv() const;
/**
* @brief
* @return
*/
Jacobian smallAdj() const;
// SE2Tangent specific API
//! @brief Get the x component of the translational part.
Scalar x() const;
//! @brief Get the y component of the translational part.
Scalar y() const;
//! @brief Get the rotational part.
Scalar angle() const;
};
template <typename _Derived>
typename SE2TangentBase<_Derived>::LieAlg
SE2TangentBase<_Derived>::hat() const
{
return ( LieAlg() <<
Scalar(0), -angle(), x(),
angle(), Scalar(0), y(),
Scalar(0), Scalar(0), Scalar(0) ).finished();
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::LieGroup
SE2TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
using std::abs;
using std::cos;
using std::sin;
const Scalar theta = angle();
const Scalar cos_theta = cos(theta);
const Scalar sin_theta = sin(theta);
const Scalar theta_sq = theta * theta;
Scalar A, // sin_theta_by_theta
B; // one_minus_cos_theta_by_theta
if (theta_sq < Constants<Scalar>::eps)
{
// Taylor approximation
A = Scalar(1) - Scalar(1. / 6.) * theta_sq;
B = Scalar(.5) * theta - Scalar(1. / 24.) * theta * theta_sq;
}
else
{
// Euler
A = sin_theta / theta;
B = (Scalar(1) - cos_theta) / theta;
}
if (J_m_t)
{
// Jr
J_m_t->setIdentity();
(*J_m_t)(0,0) = A;
(*J_m_t)(0,1) = B;
(*J_m_t)(1,0) = -B;
(*J_m_t)(1,1) = A;
if (theta_sq < Constants<Scalar>::eps)
{
(*J_m_t)(0,2) = -y() / Scalar(2) + theta * x() / Scalar(6);
(*J_m_t)(1,2) = x() / Scalar(2) + theta * y() / Scalar(6);
}
else
{
(*J_m_t)(0,2) = (-y() + theta*x() + y()*cos_theta - x()*sin_theta)/theta_sq;
(*J_m_t)(1,2) = ( x() + theta*y() - x()*cos_theta - y()*sin_theta)/theta_sq;
}
}
return LieGroup( A * x() - B * y(),
B * x() + A * y(),
cos_theta, sin_theta );
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::LieGroup
SE2TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
return exp(J_m_t);
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::Jacobian
SE2TangentBase<_Derived>::rjac() const
{
// const Scalar theta = angle();
// const Scalar cos_theta = cos(theta);
// const Scalar sin_theta = sin(theta);
// const Scalar theta_sq = theta * theta;
// Scalar A, // sin_theta_by_theta
// B; // one_minus_cos_theta_by_theta
// if (abs(theta) < Constants<Scalar>::eps)
// {
// // Taylor approximation
// A = Scalar(1) - Scalar(1. / 6.) * theta_sq;
// B = Scalar(.5) * theta - Scalar(1. / 24.) * theta * theta_sq;
// }
// else
// {
// // Euler
// A = sin_theta / theta;
// B = (Scalar(1) - cos_theta) / theta;
// }
// Jacobian Jr = Jacobian::Identity();
// Jr(0,0) = A;
// Jr(0,1) = B;
// Jr(1,0) = -B;
// Jr(1,1) = A;
// Jr(0,2) = (-y() + theta*x() + y()*cos_theta - x()*sin_theta)/theta_sq;
// Jr(1,2) = ( x() + theta*y() - x()*cos_theta - y()*sin_theta)/theta_sq;
// return Jr;
Jacobian Jr;
exp(Jr);
return Jr;
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::Jacobian
SE2TangentBase<_Derived>::rjacinv() const
{
using std::abs;
using std::cos;
using std::sin;
const Scalar theta = angle();
const Scalar cos_theta = cos(theta);
const Scalar sin_theta = sin(theta);
const Scalar theta_sq = theta * theta;
Scalar A, // theta_sin_theta
B; // theta_cos_theta
A = theta*sin_theta;
B = theta*cos_theta;
Jacobian Jrinv;
Jrinv(0,1) = -theta*Scalar(0.5);
Jrinv(1,0) = -Jrinv(0,1);
if (theta_sq > Constants<Scalar>::eps)
{
Jrinv(0,0) = -A/(Scalar(2)*cos_theta-Scalar(2));
Jrinv(1,1) = Jrinv(0,0);
Scalar den = Scalar(2)*theta*(cos_theta-Scalar(1));
Jrinv(0,2) = (A*x() + B*y() - theta*y() + Scalar(2)*x()*cos_theta - Scalar(2)*x()) / den;
Jrinv(1,2) = (-B*x() + A*y() + theta*x() + Scalar(2)*y()*cos_theta - Scalar(2)*y()) / den;
}
else
{
Jrinv(0,0) = Scalar(1)-theta_sq/Scalar(12);
Jrinv(1,1) = Jrinv(0,0);
Jrinv(0,2) = y()/Scalar(2) + theta*x()/Scalar(12);
Jrinv(1,2) = -x()/Scalar(2) + theta*y()/Scalar(12);
}
Jrinv(2,0) = Scalar(0);
Jrinv(2,1) = Scalar(0);
Jrinv(2,2) = Scalar(1);
return Jrinv;
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::Jacobian
SE2TangentBase<_Derived>::ljac() const
{
using std::cos;
using std::sin;
const Scalar theta = angle();
const Scalar cos_theta = cos(theta);
const Scalar sin_theta = sin(theta);
const Scalar theta_sq = theta * theta;
Scalar A, // sin_theta_by_theta
B; // one_minus_cos_theta_by_theta
if (theta_sq < Constants<Scalar>::eps)
{
// Taylor approximation
A = Scalar(1) - Scalar(1. / 6.) * theta_sq;
B = Scalar(.5) * theta - Scalar(1. / 24.) * theta * theta_sq;
}
else
{
// Euler
A = sin_theta / theta;
B = (Scalar(1) - cos_theta) / theta;
}
Jacobian Jl = Jacobian::Identity();
Jl(0,0) = A;
Jl(0,1) = -B;
Jl(1,0) = B;
Jl(1,1) = A;
if (theta_sq < Constants<Scalar>::eps)
{
Jl(0,2) = y() / Scalar(2) + theta * x() / Scalar(6);
Jl(1,2) = -x() / Scalar(2) + theta * y() / Scalar(6);
}
else
{
Jl(0,2) = ( y() + theta*x() - y()*cos_theta - x()*sin_theta)/theta_sq;
Jl(1,2) = (-x() + theta*y() + x()*cos_theta - y()*sin_theta)/theta_sq;
}
return Jl;
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::Jacobian
SE2TangentBase<_Derived>::ljacinv() const
{
using std::abs;
using std::cos;
using std::sin;
const Scalar theta = angle();
const Scalar cos_theta = cos(theta);
const Scalar sin_theta = sin(theta);
const Scalar theta_sq = theta * theta;
Scalar A, // theta_sin_theta
B; // theta_cos_theta
A = theta*sin_theta;
B = theta*cos_theta;
Jacobian Jlinv;
Jlinv(0,1) = theta*Scalar(0.5);
Jlinv(1,0) = -Jlinv(0,1);
if (theta_sq > Constants<Scalar>::eps)
{
Jlinv(0,0) = -A/(Scalar(2)*cos_theta-Scalar(2));
Jlinv(1,1) = Jlinv(0,0);
Scalar den = Scalar(2)*theta*(cos_theta-Scalar(1));
Jlinv(0,2) = (A*x() - B*y() + theta*y() + Scalar(2)*x()*cos_theta - Scalar(2)*x()) / den;
Jlinv(1,2) = (B*x() + A*y() - theta*x() + Scalar(2)*y()*cos_theta - Scalar(2)*y()) / den;
}
else
{
Jlinv(0,0) = Scalar(1)-theta_sq/Scalar(12);
Jlinv(1,1) = Jlinv(0,0);
Jlinv(0,2) = -y()/Scalar(2) + theta*x()/Scalar(12);
Jlinv(1,2) = x()/Scalar(2) + theta*y()/Scalar(12);
}
Jlinv(2,0) = Scalar(0);
Jlinv(2,1) = Scalar(0);
Jlinv(2,2) = Scalar(1);
return Jlinv;
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::Jacobian
SE2TangentBase<_Derived>::smallAdj() const
{
Jacobian smallAdj = Jacobian::Zero();
smallAdj(0,1) = -angle();
smallAdj(1,0) = angle();
smallAdj(0,2) = y();
smallAdj(1,2) = -x();
return smallAdj;
}
// SE2Tangent specific API
template <typename _Derived>
typename SE2TangentBase<_Derived>::Scalar
SE2TangentBase<_Derived>::x() const
{
return coeffs().x();
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::Scalar
SE2TangentBase<_Derived>::y() const
{
return coeffs().y();
}
template <typename _Derived>
typename SE2TangentBase<_Derived>::Scalar
SE2TangentBase<_Derived>::angle() const
{
return coeffs().z();
}
namespace internal {
//! @brief Generator specialization for SE2TangentBase objects.
template <typename Derived>
struct GeneratorEvaluator<SE2TangentBase<Derived>>
{
static typename SE2TangentBase<Derived>::LieAlg
run(const unsigned int i)
{
using LieAlg = typename SE2TangentBase<Derived>::LieAlg;
using Scalar = typename SE2TangentBase<Derived>::Scalar;
switch (i)
{
case 0:
{
const static LieAlg E0(
(LieAlg() << Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0) ).finished());
return E0;
}
case 1:
{
const static LieAlg E1(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0) ).finished());
return E1;
}
case 2:
{
const static LieAlg E2(
(LieAlg() << Scalar(0), Scalar(-1), Scalar(0),
Scalar(1), Scalar( 0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0) ).finished());
return E2;
}
default:
MANIF_THROW("Index i must be in [0,2]!", invalid_argument);
break;
}
return LieAlg{};
}
};
//! @brief Inner weight matrix specialization for SE2TangentBase objects.
template <typename Derived>
struct InnerWeightsEvaluator<SE2TangentBase<Derived>>
{
static typename Derived::InnerWeightsMatrix
run()
{
using InnerWeightsMatrix = typename SE2TangentBase<Derived>::InnerWeightsMatrix;
using Scalar = typename SE2TangentBase<Derived>::Scalar;
const static InnerWeightsMatrix W(
(InnerWeightsMatrix() << Scalar(1), Scalar(0), Scalar(0),
Scalar(0), Scalar(1), Scalar(0),
Scalar(0), Scalar(0), Scalar(2) ).finished()
);
return W;
}
};
//! @brief Random specialization for SE2TangentBase objects.
template <typename Derived>
struct RandomEvaluatorImpl<SE2TangentBase<Derived>>
{
static void run(SE2TangentBase<Derived>& m)
{
m.coeffs().setRandom(); // in [-1,1]
m.coeffs().coeffRef(2) *= MANIF_PI; // in [-PI,PI]
}
};
//! @brief Vee specialization for SE2TangentBase objects.
template <typename Derived>
struct VeeEvaluatorImpl<SE2TangentBase<Derived>> {
template <typename TL, typename TR>
static void run(TL& t, const TR& v) {
t.coeffs() << v(0, 2), v(1, 2), v(1, 0);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE2_BASE_H_ */

89
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se2/SE2Tangent_map.h vendored Normal file
View File

@ -0,0 +1,89 @@
#ifndef _MANIF_MANIF_SE2TANGENT_MAP_H_
#define _MANIF_MANIF_SE2TANGENT_MAP_H_
#include "manif/impl/se2/SE2Tangent.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SE2Tangent<_Scalar>,0> >
: public traits<SE2Tangent<_Scalar>>
{
using typename traits<SE2Tangent<_Scalar>>::Scalar;
using traits<SE2Tangent<_Scalar>>::DoF;
using DataType = ::Eigen::Map<Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SE2TangentBase<Eigen::Map<SE2Tangent<Scalar>, 0>>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits< Eigen::Map<const SE2Tangent<_Scalar>,0> >
: public traits<const SE2Tangent<_Scalar>>
{
using typename traits<const SE2Tangent<_Scalar>>::Scalar;
using traits<SE2Tangent<_Scalar>>::DoF;
using DataType = ::Eigen::Map<const Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SE2TangentBase<Eigen::Map<const SE2Tangent<Scalar>, 0>>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SE2
*/
template <class _Scalar>
class Map<manif::SE2Tangent<_Scalar>, 0>
: public manif::SE2TangentBase<Map<manif::SE2Tangent<_Scalar>, 0> >
{
using Base = manif::SE2TangentBase<Map<manif::SE2Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_TANGENT_MAP_ASSIGN_OP(SE2Tangent)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SE2
*/
template <class _Scalar>
class Map<const manif::SE2Tangent<_Scalar>, 0>
: public manif::SE2TangentBase<Map<const manif::SE2Tangent<_Scalar>, 0> >
{
using Base = manif::SE2TangentBase<Map<const manif::SE2Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SE2TANGENT_MAP_H_ */

441
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se2/SE2_base.h vendored Normal file
View File

@ -0,0 +1,441 @@
#ifndef _MANIF_MANIF_SE2_BASE_H_
#define _MANIF_MANIF_SE2_BASE_H_
#include "manif/impl/se2/SE2_properties.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/utils.h"
namespace manif {
//
// LieGroup
//
/**
* @brief The base class of the SE2 group.
* @note See Appendix C of the paper.
*/
template <typename _Derived>
struct SE2Base : LieGroupBase<_Derived>
{
private:
using Base = LieGroupBase<_Derived>;
using Type = SE2Base<_Derived>;
public:
MANIF_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_AUTO_API
MANIF_INHERIT_GROUP_OPERATOR
using Base::coeffs;
using Rotation = typename internal::traits<_Derived>::Rotation;
using Translation = typename internal::traits<_Derived>::Translation;
using Transformation = typename internal::traits<_Derived>::Transformation;
using Isometry = Eigen::Transform<Scalar, 2, Eigen::Isometry>;
// LieGroup common API
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SE2Base)
public:
MANIF_GROUP_ML_ASSIGN_OP(SE2Base)
/**
* @brief Get the inverse of this.
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this.
* @note See Eqs. (154, 160).
*/
LieGroup inverse(OptJacobianRef J_minv_m = {}) const;
/**
* @brief Get the SE2 corresponding Lie algebra element in vector form.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt to this.
* @return The SE2 tangent of this.
* @note This is the log() map in vector form.
* @note See Eqs. (157, 158).
* @see SE2Tangent.
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another SE2 element.
* @param[in] m Another SE2 element.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
* @note See Eq. (155) & Eqs. (161,162).
*/
template <typename _DerivedOther>
LieGroup compose(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}) const;
/**
* @brief Rigid motion action on a 2D point.
* @param v A 2D point.
* @param[out] -optional- J_vout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object.
* @return The transformed 2D point.
* @note See Eq. (165) & Eqs. (166,167).
*/
template <typename _EigenDerived>
Eigen::Matrix<Scalar, 2, 1>
act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 3>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 2>>> J_vout_v = {}) const;
/**
* @brief Get the adjoint matrix of SE2 at this.
* @note See Eq. (159).
*/
Jacobian adj() const;
// SE2 specific functions
/**
* @brief Get the transformation matrix (2D isometry).
* @note T = | R t |
* | 0 1 |
*/
Transformation transform() const;
/**
* Get the isometry object (Eigen 2D isometry).
* @note T = | R t |
* | 0 1 |
*/
Isometry isometry() const;
//! @brief Get the rotational part of this as a rotation matrix.
Rotation rotation() const;
//! @brief Get the translational part of this as a vector.
Translation translation() const;
/**
* @brief Get the real part of the underlying complex number representing
* the rotational part.
*/
Scalar real() const;
/**
* @brief Get the imaginary part of the underlying complex number representing
* the rotational part.
*/
Scalar imag() const;
/**
* @brief Get the angle (rad.) of the rotational part.
*/
Scalar angle() const;
/**
* @brief Get the x component of the translational part.
*/
Scalar x() const;
/**
* @brief Get the y component of the translational part.
*/
Scalar y() const;
/**
* @brief Normalize the underlying complex number.
*/
void normalize();
};
template <typename _Derived>
typename SE2Base<_Derived>::Transformation
SE2Base<_Derived>::transform() const
{
Transformation T(Transformation::Identity());
T.template topLeftCorner<2,2>() = rotation();
T(0,2) = x();
T(1,2) = y();
return T;
}
template <typename _Derived>
typename SE2Base<_Derived>::Isometry
SE2Base<_Derived>::isometry() const
{
return Isometry(transform());
}
template <typename _Derived>
typename SE2Base<_Derived>::Rotation
SE2Base<_Derived>::rotation() const
{
return (Rotation() << real(), -imag(),
imag(), real() ).finished();
}
template <typename _Derived>
typename SE2Base<_Derived>::Translation
SE2Base<_Derived>::translation() const
{
return Translation(x(), y());
}
template <typename _Derived>
typename SE2Base<_Derived>::LieGroup
SE2Base<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
using std::cos;
using std::sin;
if (J_minv_m)
{
(*J_minv_m) = -adj();
}
return LieGroup(-x()*real() - y()*imag(),
x()*imag() - y()*real(),
-angle() );
}
template <typename _Derived>
typename SE2Base<_Derived>::Tangent
SE2Base<_Derived>::log(OptJacobianRef J_t_m) const
{
using std::abs;
using std::cos;
using std::sin;
const Scalar theta = angle();
const Scalar cos_theta = coeffs()[2];
const Scalar sin_theta = coeffs()[3];
const Scalar theta_sq = theta * theta;
Scalar A, // sin_theta_by_theta
B; // one_minus_cos_theta_by_theta
if (theta_sq < Constants<Scalar>::eps)
{
// Taylor approximation
A = Scalar(1) - Scalar(1. / 6.) * theta_sq;
B = Scalar(.5) * theta - Scalar(1. / 24.) * theta * theta_sq;
}
else
{
// Euler
A = sin_theta / theta;
B = (Scalar(1) - cos_theta) / theta;
}
const Scalar den = Scalar(1) / (A*A + B*B);
A *= den;
B *= den;
Tangent tan( A * x() + B * y(),
-B * x() + A * y(),
theta );
if (J_t_m)
{
// Jr^-1
(*J_t_m) = tan.rjacinv();
}
return tan;
}
template <typename _Derived>
typename SE2Base<_Derived>::Tangent
SE2Base<_Derived>::lift(OptJacobianRef J_t_m) const
{
return log(J_t_m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename SE2Base<_Derived>::LieGroup
SE2Base<_Derived>::compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb) const
{
using std::abs;
static_assert(
std::is_base_of<SE2Base<_DerivedOther>, _DerivedOther>::value,
"Argument does not inherit from SE2Base !");
if (J_mc_ma)
{
(*J_mc_ma) = m.inverse().adj();
}
if (J_mc_mb)
{
J_mc_mb->setIdentity();
}
const auto& m_se2 = static_cast<const SE2Base<_DerivedOther>&>(m);
const Scalar lhs_real = real(); // cos(t)
const Scalar lhs_imag = imag(); // sin(t)
const Scalar rhs_real = m_se2.real();
const Scalar rhs_imag = m_se2.imag();
Scalar ret_real = lhs_real * rhs_real - lhs_imag * rhs_imag;
Scalar ret_imag = lhs_real * rhs_imag + lhs_imag * rhs_real;
const Scalar ret_sqnorm = ret_real*ret_real+ret_imag*ret_imag;
if (abs(ret_sqnorm-Scalar(1)) > Constants<Scalar>::eps)
{
const Scalar scale = approxSqrtInv(ret_sqnorm);
ret_real *= scale;
ret_imag *= scale;
}
return LieGroup(lhs_real * m_se2.x() - lhs_imag * m_se2.y() + x(),
lhs_imag * m_se2.x() + lhs_real * m_se2.y() + y(),
ret_real, ret_imag );
}
template <typename _Derived>
template <typename _EigenDerived>
Eigen::Matrix<typename SE2Base<_Derived>::Scalar, 2, 1>
SE2Base<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 3>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 2>>> J_vout_v) const
{
assert_vector_dim(v, 2);
const Rotation R(rotation());
if (J_vout_m)
{
J_vout_m->template topLeftCorner<2,2>() = R;
J_vout_m->template topRightCorner<2,1>() = R * (skew(Scalar(1)) * v);
}
if (J_vout_v)
{
(*J_vout_v) = R;
}
return translation() + R * v;
}
template <typename _Derived>
typename SE2Base<_Derived>::Jacobian
SE2Base<_Derived>::adj() const
{
Jacobian Adj = Jacobian::Identity();
Adj.template topLeftCorner<2,2>() = rotation();
Adj(0,2) = y();
Adj(1,2) = -x();
return Adj;
}
// SE2 specific function
template <typename _Derived>
typename SE2Base<_Derived>::Scalar
SE2Base<_Derived>::real() const
{
return coeffs()(2);
}
template <typename _Derived>
typename SE2Base<_Derived>::Scalar
SE2Base<_Derived>::imag() const
{
return coeffs()(3);
}
template <typename _Derived>
typename SE2Base<_Derived>::Scalar
SE2Base<_Derived>::angle() const
{
using std::atan2;
return atan2(imag(), real());
}
template <typename _Derived>
typename SE2Base<_Derived>::Scalar
SE2Base<_Derived>::x() const
{
return coeffs().x();
}
template <typename _Derived>
typename SE2Base<_Derived>::Scalar
SE2Base<_Derived>::y() const
{
return coeffs().y();
}
template <typename _Derived>
void SE2Base<_Derived>::normalize()
{
coeffs().template tail<2>().normalize();
}
namespace internal {
//! @brief Random specialization for SE2Base objects
template <typename Derived>
struct RandomEvaluatorImpl<SE2Base<Derived>>
{
template <typename T>
static void run(T& m)
{
using Tangent = typename LieGroupBase<Derived>::Tangent;
m = Tangent::Random().exp();
}
};
//! @brief Assignment assert specialization for SE2Base objects
template <typename Derived>
struct AssignmentEvaluatorImpl<SE2Base<Derived>>
{
template <typename T>
static void run_impl(const T& data)
{
using std::abs;
MANIF_ASSERT(
abs(data.template tail<2>().norm()-typename SE2Base<Derived>::Scalar(1)) <
Constants<typename SE2Base<Derived>::Scalar>::eps,
"SE2 assigned data not normalized !",
invalid_argument
);
MANIF_UNUSED_VARIABLE(data);
}
};
//! @brief Cast specialization for SE2Base objects.
template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SE2Base<Derived>, NewScalar> {
template <typename T>
static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
return typename Derived::template LieGroupTemplate<NewScalar>(
NewScalar(o.x()), NewScalar(o.y()), NewScalar(o.angle())
);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE2_BASE_H_ */

103
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se2/SE2_map.h vendored Normal file
View File

@ -0,0 +1,103 @@
#ifndef _MANIF_MANIF_SE2_MAP_H_
#define _MANIF_MANIF_SE2_MAP_H_
#include "manif/impl/se2/SE2.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SE2<_Scalar>,0> >
: public traits<SE2<_Scalar>>
{
using typename traits<SE2<_Scalar>>::Scalar;
using traits<SE2<Scalar>>::RepSize;
using Base = SE2Base<Eigen::Map<SE2<Scalar>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits< Eigen::Map<const SE2<_Scalar>,0> >
: public traits<const SE2<_Scalar>>
{
using typename traits<const SE2<_Scalar>>::Scalar;
using traits<const SE2<Scalar>>::RepSize;
using Base = SE2Base<Eigen::Map<const SE2<Scalar>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SE2
*/
template <class _Scalar>
class Map<manif::SE2<_Scalar>, 0>
: public manif::SE2Base<Map<manif::SE2<_Scalar>, 0> >
{
using Base = manif::SE2Base<Map<manif::SE2<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_GROUP_MAP_ASSIGN_OP(SE2)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
using Base::angle;
using Base::real;
using Base::imag;
using Base::x;
using Base::y;
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SE2
*/
template <class _Scalar>
class Map<const manif::SE2<_Scalar>, 0>
: public manif::SE2Base<Map<const manif::SE2<_Scalar>, 0> >
{
using Base = manif::SE2Base<Map<const manif::SE2<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
using Base::angle;
using Base::real;
using Base::imag;
using Base::x;
using Base::y;
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SE2_MAP_H_ */

33
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se2/SE2_properties.h vendored Normal file
View File

@ -0,0 +1,33 @@
#ifndef _MANIF_MANIF_SE2_PROPERTIES_H_
#define _MANIF_MANIF_SE2_PROPERTIES_H_
#include "manif/impl/traits.h"
namespace manif {
// Forward declaration
template <typename _Derived> struct SE2Base;
template <typename _Derived> struct SE2TangentBase;
namespace internal {
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SE2Base<_Derived>>
{
static constexpr int Dim = 2; /// @brief Space dimension
static constexpr int DoF = 3; /// @brief Degrees of freedom
};
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SE2TangentBase<_Derived>>
{
static constexpr int Dim = 2; /// @brief Space dimension
static constexpr int DoF = 3; /// @brief Degrees of freedom
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE2_PROPERTIES_H_ */

217
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se3/SE3.h vendored Normal file
View File

@ -0,0 +1,217 @@
#ifndef _MANIF_MANIF_SE3_H_
#define _MANIF_MANIF_SE3_H_
#include "manif/impl/se3/SE3_base.h"
namespace manif {
// Forward declare for type traits specialization
template <typename _Scalar> struct SE3;
template <typename _Scalar> struct SE3Tangent;
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SE3<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SE3<_Scalar>;
using Tangent = SE3Tangent<_Scalar>;
using Base = SE3Base<SE3<_Scalar>>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = 7;
/// @todo would be nice to concat vec3 + quaternion
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using Transformation = Eigen::Matrix<Scalar, 4, 4>;
using Rotation = Eigen::Matrix<Scalar, Dim, Dim>;
using Translation = Eigen::Matrix<Scalar, Dim, 1>;
using Vector = Eigen::Matrix<Scalar, Dim, 1>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// LieGroup
//
/**
* @brief Represent an element of SE3.
*/
template <typename _Scalar>
struct SE3 : SE3Base<SE3<_Scalar>>
{
private:
using Base = SE3Base<SE3<_Scalar>>;
using Type = SE3<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_COMPLETE_GROUP_TYPEDEF
using Translation = typename Base::Translation;
using Quaternion = Eigen::Quaternion<Scalar>;
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
using Base::normalize;
SE3() = default;
~SE3() = default;
MANIF_COPY_CONSTRUCTOR(SE3)
MANIF_MOVE_CONSTRUCTOR(SE3)
template <typename _DerivedOther>
SE3(const LieGroupBase<_DerivedOther>& o);
MANIF_GROUP_ASSIGN_OP(SE3)
/**
* @brief Constructor given a translation and a unit quaternion.
* @param[in] t A translation vector.
* @param[in] q A unit quaternion.
* @throws manif::invalid_argument on un-normalized complex number.
*/
SE3(const Translation& t,
const Eigen::Quaternion<Scalar>& q);
/**
* @brief Constructor given a translation and an angle axis.
* @param[in] t A translation vector.
* @param[in] angle_axis An angle-axis.
*/
SE3(const Translation& t,
const Eigen::AngleAxis<Scalar>& angle_axis);
/**
* @brief Constructor given a translation and SO3 element.
* @param[in] t A translation vector.
* @param[in] SO3 An element of SO3.
*/
SE3(const Translation& t,
const SO3<Scalar>& SO3);
/**
* @brief Constructor given translation components and
* roll-pitch-yaw angles.
* @param[in] x The x component of the translation.
* @param[in] y The y component of the translation.
* @param[in] z The z component of the translation.
* @param[in] roll The roll angle.
* @param[in] pitch The pitch angle.
* @param[in] yaw The yaw angle.
*/
SE3(const Scalar x, const Scalar y, const Scalar z,
const Scalar roll, const Scalar pitch, const Scalar yaw);
/**
* @brief Constructor from a 3D Eigen::Isometry<Scalar>
* @param[in] h an isometry object from Eigen
*
* Isometry is a typedef from Eigen::Transform, in which the linear part is assumed a rotation matrix.
* This is used to speed up certain methods of Transform, especially inverse().
*/
SE3(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h);
// LieGroup common API
DataType& coeffs();
const DataType& coeffs() const;
// SE3 specific API
protected:
DataType data_;
};
MANIF_EXTRA_GROUP_TYPEDEF(SE3)
template <typename _Scalar>
template <typename _DerivedOther>
SE3<_Scalar>::SE3(const LieGroupBase<_DerivedOther>& o)
: SE3(o.coeffs())
{
//
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
// Temporarily disable this warning which causes false positive with GCC 12
// See e.g. https://gcc.gnu.org/bugzilla/show_bug.cgi?id=106247
template <typename _Scalar>
SE3<_Scalar>::SE3(const Translation& t, const Eigen::Quaternion<Scalar>& q)
: SE3((DataType() << t, q.coeffs() ).finished())
{
//
}
#pragma GCC diagnostic pop
template <typename _Scalar>
SE3<_Scalar>::SE3(const Translation& t, const Eigen::AngleAxis<Scalar>& a)
: SE3(t, Quaternion(a))
{
//
}
template <typename _Scalar>
SE3<_Scalar>::SE3(const Scalar x, const Scalar y, const Scalar z,
const Scalar roll, const Scalar pitch, const Scalar yaw)
: SE3(Translation(x,y,z), Eigen::Quaternion<Scalar>(
Eigen::AngleAxis<Scalar>(yaw, Eigen::Matrix<Scalar, 3, 1>::UnitZ()) *
Eigen::AngleAxis<Scalar>(pitch, Eigen::Matrix<Scalar, 3, 1>::UnitY()) *
Eigen::AngleAxis<Scalar>(roll, Eigen::Matrix<Scalar, 3, 1>::UnitX()) ))
{
//
}
template <typename _Scalar>
SE3<_Scalar>::SE3(const Translation& t, const SO3<Scalar>& so3)
: SE3(t, so3.quat())
{
//
}
template <typename _Scalar>
SE3<_Scalar>::SE3(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h)
: SE3(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation()))
{
//
}
template <typename _Scalar>
typename SE3<_Scalar>::DataType&
SE3<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SE3<_Scalar>::DataType&
SE3<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SE3_H_ */

113
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se3/SE3Tangent.h vendored Normal file
View File

@ -0,0 +1,113 @@
#ifndef _MANIF_MANIF_SE3TANGENT_H_
#define _MANIF_MANIF_SE3TANGENT_H_
#include "manif/impl/se3/SE3Tangent_base.h"
namespace manif {
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SE3Tangent<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SE3<_Scalar>;
using Tangent = SE3Tangent<_Scalar>;
using Base = SE3TangentBase<Tangent>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = DoF;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using LieAlg = Eigen::Matrix<Scalar, 4, 4>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// Tangent
//
/**
* @brief Represents an element of tangent space of SE3.
*/
template <typename _Scalar>
struct SE3Tangent : SE3TangentBase<SE3Tangent<_Scalar>>
{
private:
using Base = SE3TangentBase<SE3Tangent<_Scalar>>;
using Type = SE3Tangent<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
SE3Tangent() = default;
~SE3Tangent() = default;
MANIF_COPY_CONSTRUCTOR(SE3Tangent)
MANIF_MOVE_CONSTRUCTOR(SE3Tangent)
// Copy constructor given base
template <typename _DerivedOther>
SE3Tangent(const TangentBase<_DerivedOther>& o);
MANIF_TANGENT_ASSIGN_OP(SE3Tangent)
// Tangent common API
DataType& coeffs();
const DataType& coeffs() const;
// SE3Tangent specific API
protected:
DataType data_;
};
MANIF_EXTRA_TANGENT_TYPEDEF(SE3Tangent);
template <typename _Scalar>
template <typename _DerivedOther>
SE3Tangent<_Scalar>::SE3Tangent(
const TangentBase<_DerivedOther>& o)
: data_(o.coeffs())
{
//
}
template <typename _Scalar>
typename SE3Tangent<_Scalar>::DataType&
SE3Tangent<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SE3Tangent<_Scalar>::DataType&
SE3Tangent<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SE3TANGENT_H_ */

473
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se3/SE3Tangent_base.h vendored Normal file
View File

@ -0,0 +1,473 @@
#ifndef _MANIF_MANIF_SE3TANGENT_BASE_H_
#define _MANIF_MANIF_SE3TANGENT_BASE_H_
#include "manif/impl/se3/SE3_properties.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/so3/SO3Tangent_map.h"
namespace manif {
//
// Tangent
//
/**
* @brief The base class of the SE3 tangent.
* @note See Appendix D of the paper.
*/
template <typename _Derived>
struct SE3TangentBase : TangentBase<_Derived>
{
private:
using Base = TangentBase<_Derived>;
using Type = SE3TangentBase<_Derived>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_OPERATOR
using LinBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
using AngBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
using ConstLinBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;
using ConstAngBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;
using Base::data;
using Base::coeffs;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SE3TangentBase)
public:
MANIF_TANGENT_ML_ASSIGN_OP(SE3TangentBase)
// Tangent common API
/**
* @brief Hat operator of SE3.
* @return An element of the Lie algebra se3.
* @note See Eq. (169).
*/
LieAlg hat() const;
/**
* @brief Get the SE3 element.
* @param[out] -optional- J_m_t Jacobian of the SE3 element wrt this.
* @return The SE3 element.
* @note This is the exp() map with the argument in vector form.
* @note See Eq. (172) & Eqs. (179,180).
*/
LieGroup exp(OptJacobianRef J_m_t = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t = {}) const;
/**
* @brief Get the right Jacobian of SE3.
* @note See note after Eqs. (179,180).
*/
Jacobian rjac() const;
/**
* @brief Get the left Jacobian of SE3.
* @note See Eqs. (179,180).
*/
Jacobian ljac() const;
/**
* @brief Get the inverse right Jacobian of SE3.
* @note See note after Eqs. (179,180).
*/
Jacobian rjacinv() const;
/**
* @brief Get the inverse left Jacobian of SE3.
* @note See Eqs. (179,180).
*/
Jacobian ljacinv() const;
/**
* @brief
* @return
*/
Jacobian smallAdj() const;
// SE3Tangent specific API
//! @brief Get the linear part.
LinBlock lin();
const ConstLinBlock lin() const;
//! @brief Get the angular part.
AngBlock ang();
const ConstAngBlock ang() const;
// Scalar x() const;
// Scalar y() const;
// Scalar z() const;
//Scalar roll() const;
//Scalar pitch() const;
//Scalar yaw() const;
public: /// @todo make protected
const Eigen::Map<const SO3Tangent<Scalar>> asSO3() const
{
return Eigen::Map<const SO3Tangent<Scalar>>(coeffs().data()+3);
}
Eigen::Map<SO3Tangent<Scalar>> asSO3()
{
return Eigen::Map<SO3Tangent<Scalar>>(coeffs().data()+3);
}
// private:
template <typename _EigenDerived>
static void fillQ(Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>> Q,
const Eigen::MatrixBase<_EigenDerived>& c);
};
template <typename _Derived>
typename SE3TangentBase<_Derived>::LieGroup
SE3TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
using std::sqrt;
using std::cos;
using std::sin;
if (J_m_t)
{
*J_m_t = rjac();
}
/// @note Eq. 10.93
return LieGroup(asSO3().ljac()*lin(), asSO3().exp().quat());
}
template <typename _Derived>
typename SE3TangentBase<_Derived>::LieGroup
SE3TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
return exp(J_m_t);
}
template <typename _Derived>
typename SE3TangentBase<_Derived>::LieAlg
SE3TangentBase<_Derived>::hat() const
{
return (LieAlg() <<
Scalar(0) , Scalar(-coeffs()(5)), Scalar( coeffs()(4)), Scalar(coeffs()(0)),
Scalar( coeffs()(5)), Scalar(0) , Scalar(-coeffs()(3)), Scalar(coeffs()(1)),
Scalar(-coeffs()(4)), Scalar( coeffs()(3)), Scalar(0) , Scalar(coeffs()(2)),
Scalar(0) , Scalar(0) , Scalar(0) , Scalar(0)
).finished();
}
/// @note Eq. 10.95
/// @note barfoot14tro Eq. 102
template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::rjac() const
{
/// @note Eq. 10.95
Jacobian Jr;
Jr.template bottomLeftCorner<3,3>().setZero();
Jr.template topLeftCorner<3,3>() = asSO3().rjac();
Jr.template bottomRightCorner<3,3>() = Jr.template topLeftCorner<3,3>();
fillQ( Jr.template topRightCorner<3,3>(), -coeffs() );
return Jr;
}
template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::ljac() const
{
/// @note Eq. 10.95
Jacobian Jl;
Jl.template bottomLeftCorner<3,3>().setZero();
Jl.template topLeftCorner<3,3>() = asSO3().ljac();
Jl.template bottomRightCorner<3,3>() = Jl.template topLeftCorner<3,3>();
fillQ( Jl.template topRightCorner<3,3>(), coeffs() );
return Jl;
}
/// @note barfoot14tro Eq. 102
template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::rjacinv() const
{
/// @note Eq. 10.95
Jacobian Jr_inv;
fillQ( Jr_inv.template bottomLeftCorner<3,3>(), -coeffs() ); // serves as temporary Q
Jr_inv.template topLeftCorner<3,3>() = asSO3().rjacinv();
Jr_inv.template bottomRightCorner<3,3>() = Jr_inv.template topLeftCorner<3,3>();
Jr_inv.template topRightCorner<3,3>().noalias() =
-Jr_inv.template topLeftCorner<3,3>() *
Jr_inv.template bottomLeftCorner<3,3>() *
Jr_inv.template topLeftCorner<3,3>();
Jr_inv.template bottomLeftCorner<3,3>().setZero();
return Jr_inv;
}
template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::ljacinv() const
{
Jacobian Jl_inv;
fillQ( Jl_inv.template bottomLeftCorner<3,3>(), coeffs() ); // serves as temporary Q
Jl_inv.template topLeftCorner<3,3>() = asSO3().ljacinv();
Jl_inv.template bottomRightCorner<3,3>() = Jl_inv.template topLeftCorner<3,3>();
Jl_inv.template topRightCorner<3,3>().noalias() =
-Jl_inv.template topLeftCorner<3,3>() *
Jl_inv.template bottomLeftCorner<3,3>() *
Jl_inv.template topLeftCorner<3,3>();
Jl_inv.template bottomLeftCorner<3,3>().setZero();
return Jl_inv;
}
template <typename _Derived>
template <typename _EigenDerived>
void SE3TangentBase<_Derived>::fillQ(
Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>> Q,
const Eigen::MatrixBase<_EigenDerived>& c)
{
using std::cos;
using std::sin;
using std::sqrt;
const Scalar theta_sq = c.template tail<3>().squaredNorm();
Scalar A(0.5), B, C, D;
// Small angle approximation
if (theta_sq <= Constants<Scalar>::eps)
{
B = Scalar(1./6.) + Scalar(1./120.) * theta_sq;
C = -Scalar(1./24.) + Scalar(1./720.) * theta_sq;
D = -Scalar(1./60.);
}
else
{
const Scalar theta = sqrt(theta_sq);
const Scalar sin_theta = sin(theta);
const Scalar cos_theta = cos(theta);
B = (theta - sin_theta) / (theta_sq*theta);
C = (Scalar(1) - theta_sq/Scalar(2) - cos_theta) / (theta_sq*theta_sq);
D = (C - Scalar(3)*(theta-sin_theta-theta_sq*theta/Scalar(6)) / (theta_sq*theta_sq*theta));
// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
// C = (theta_sq+Scalar(2)*cos_theta-Scalar(2)) / (Scalar(2)*theta_sq*theta_sq);
// D = (Scalar(2)*theta - Scalar(3)*sin_theta + theta*cos_theta) / (Scalar(2)*theta_sq*theta_sq*theta);
}
/// @note Barfoot14tro Eq. 102
const Eigen::Matrix<Scalar, 3, 3> V = skew(c.template head<3>());
const Eigen::Matrix<Scalar, 3, 3> W = skew(c.template tail<3>());
const Eigen::Matrix<Scalar, 3, 3> VW = V * W;
const Eigen::Matrix<Scalar, 3, 3> WV = VW.transpose(); // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
const Eigen::Matrix<Scalar, 3, 3> WVW = WV * W;
const Eigen::Matrix<Scalar, 3, 3> VWW = VW * W;
Q.noalias() =
+ A * V
+ B * (WV + VW + WVW)
- C * (VWW - VWW.transpose() - Scalar(3) * WVW) // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
- D * WVW * W; // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
}
template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::smallAdj() const
{
/// @note Chirikjian (close to Eq.10.94)
/// says
/// ad(g) = | Omega 0 |
/// | V Omega |
///
/// considering vee(log(g)) = (w;v)
///
/// but this is
/// ad(g) = | Omega V |
/// | 0 Omega |
///
/// considering vee(log(g)) = (v;w)
Jacobian smallAdj;
smallAdj.template topRightCorner<3,3>() = skew(lin());
smallAdj.template topLeftCorner<3,3>() = skew(ang());
smallAdj.template bottomRightCorner<3,3>() = smallAdj.template topLeftCorner<3,3>();
smallAdj.template bottomLeftCorner<3,3>().setZero();
return smallAdj;
}
// SE3Tangent specific API
template <typename _Derived>
typename SE3TangentBase<_Derived>::LinBlock
SE3TangentBase<_Derived>::lin()
{
return coeffs().template head<3>();
}
template <typename _Derived>
const typename SE3TangentBase<_Derived>::ConstLinBlock
SE3TangentBase<_Derived>::lin() const
{
return coeffs().template head<3>();
}
template <typename _Derived>
typename SE3TangentBase<_Derived>::AngBlock
SE3TangentBase<_Derived>::ang()
{
return coeffs().template tail<3>();
}
template <typename _Derived>
const typename SE3TangentBase<_Derived>::ConstAngBlock
SE3TangentBase<_Derived>::ang() const
{
return coeffs().template tail<3>();
}
//template <typename _Derived>
//typename SE3TangentBase<_Derived>::Scalar
//SE3TangentBase<_Derived>::x() const
//{
// return data()->x();
//}
//template <typename _Derived>
//typename SE3TangentBase<_Derived>::Scalar
//SE3TangentBase<_Derived>::y() const
//{
// return data()->y();
//}
//template <typename _Derived>
//typename SE3TangentBase<_Derived>::Scalar
//SE3TangentBase<_Derived>::z() const
//{
// return data()->z();
//}
namespace internal {
//! @brief Generator specialization for SE3TangentBase objects.
template <typename Derived>
struct GeneratorEvaluator<SE3TangentBase<Derived>>
{
static typename SE3TangentBase<Derived>::LieAlg
run(const unsigned int i)
{
using LieAlg = typename SE3TangentBase<Derived>::LieAlg;
using Scalar = typename SE3TangentBase<Derived>::Scalar;
switch (i)
{
case 0:
{
static const LieAlg E0(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E0;
}
case 1:
{
static const LieAlg E1(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E1;
}
case 2:
{
static const LieAlg E2(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E2;
}
case 3:
{
static const LieAlg E3(
(LieAlg() << Scalar(0), Scalar(0), Scalar( 0), Scalar(0),
Scalar(0), Scalar(0), Scalar(-1), Scalar(0),
Scalar(0), Scalar(1), Scalar( 0), Scalar(0),
Scalar(0), Scalar(0), Scalar( 0), Scalar(0) ).finished());
return E3;
}
case 4:
{
static const LieAlg E4(
(LieAlg() << Scalar( 0), Scalar(0), Scalar(1), Scalar(0),
Scalar( 0), Scalar(0), Scalar(0), Scalar(0),
Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
Scalar( 0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E4;
}
case 5:
{
static const LieAlg E5(
(LieAlg() << Scalar(0), Scalar(-1), Scalar(0), Scalar(0),
Scalar(1), Scalar( 0), Scalar(0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0), Scalar(0) ).finished());
return E5;
}
default:
MANIF_THROW("Index i must be in [0,5]!", invalid_argument);
break;
}
return LieAlg{};
}
};
//! @brief Random specialization for SE3TangentBase objects.
template <typename Derived>
struct RandomEvaluatorImpl<SE3TangentBase<Derived>>
{
static void run(SE3TangentBase<Derived>& m)
{
m.coeffs().template head<3>().setRandom();
// In ball of radius PI
m.coeffs().template tail<3>() = randPointInBall(MANIF_PI).template cast<typename Derived::Scalar>();
}
};
//! @brief Vee specialization for SE3TangentBase objects.
template <typename Derived>
struct VeeEvaluatorImpl<SE3TangentBase<Derived>> {
template <typename TL, typename TR>
static void run(TL& t, const TR& v) {
t.coeffs() << v(0, 3), v(1, 3), v(2, 3), v(2, 1), v(0, 2), v(1, 0);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE3_BASE_H_ */

89
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se3/SE3Tangent_map.h vendored Normal file
View File

@ -0,0 +1,89 @@
#ifndef _MANIF_MANIF_SE3TANGENT_MAP_H_
#define _MANIF_MANIF_SE3TANGENT_MAP_H_
#include "manif/impl/se3/SE3Tangent.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SE3Tangent<_Scalar>,0> >
: public traits<SE3Tangent<_Scalar>>
{
using typename traits<SE3Tangent<_Scalar>>::Scalar;
using traits<SE3Tangent<_Scalar>>::DoF;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SE3TangentBase<Eigen::Map<SE3Tangent<Scalar>, 0>>;
};
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<const SE3Tangent<_Scalar>,0> >
: public traits<const SE3Tangent<_Scalar>>
{
using typename traits<const SE3Tangent<_Scalar>>::Scalar;
using traits<const SE3Tangent<_Scalar>>::DoF;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SE3TangentBase<Eigen::Map<const SE3Tangent<Scalar>, 0>>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SE3
*/
template <class _Scalar>
class Map<manif::SE3Tangent<_Scalar>, 0>
: public manif::SE3TangentBase<Map<manif::SE3Tangent<_Scalar>, 0> >
{
using Base = manif::SE3TangentBase<Map<manif::SE3Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_TANGENT_MAP_ASSIGN_OP(SE3Tangent)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SE3
*/
template <class _Scalar>
class Map<const manif::SE3Tangent<_Scalar>, 0>
: public manif::SE3TangentBase<Map<const manif::SE3Tangent<_Scalar>, 0> >
{
using Base = manif::SE3TangentBase<Map<const manif::SE3Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SE3TANGENT_MAP_H_ */

481
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se3/SE3_base.h vendored Normal file
View File

@ -0,0 +1,481 @@
#ifndef _MANIF_MANIF_SE3_BASE_H_
#define _MANIF_MANIF_SE3_BASE_H_
#include "manif/impl/se3/SE3_properties.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/so3/SO3_map.h"
namespace manif {
//
// LieGroup
//
/**
* @brief The base class of the SE3 group.
* @note See Appendix D of the paper.
*/
template <typename _Derived>
struct SE3Base : LieGroupBase<_Derived>
{
private:
using Base = LieGroupBase<_Derived>;
using Type = SE3Base<_Derived>;
public:
MANIF_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_AUTO_API
MANIF_INHERIT_GROUP_OPERATOR
using Base::coeffs;
using Rotation = typename internal::traits<_Derived>::Rotation;
using Translation = typename internal::traits<_Derived>::Translation;
using Transformation = typename internal::traits<_Derived>::Transformation;
using Isometry = Eigen::Transform<Scalar, 3, Eigen::Isometry>;
using QuaternionDataType = Eigen::Quaternion<Scalar>;
// LieGroup common API
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SE3Base)
public:
MANIF_GROUP_ML_ASSIGN_OP(SE3Base)
/**
* @brief Get the inverse.
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this.
* @note See Eqs. (170,176).
*/
LieGroup inverse(OptJacobianRef J_minv_m = {}) const;
/**
* @brief Get the SE3 corresponding Lie algebra element in vector form.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt to this.
* @return The SE3 tangent of this.
* @note This is the log() map in vector form.
* @note See Eq. (173) & Eq. (79,179,180) and following notes.
* @see SE3Tangent.
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another SE3 element.
* @param[in] m Another SE3 element.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
* @note See Eq. (171) and Eqs. (177,178).
*/
template <typename _DerivedOther>
LieGroup compose(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}) const;
/**
* @brief Rigid motion action on a 3D point.
* @param v A 3D point.
* @param[out] -optional- J_vout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object.
* @return The transformed 3D point.
* @note See Eq. (181) & Eqs. (182,183).
*/
template <typename _EigenDerived>
Eigen::Matrix<Scalar, 3, 1>
act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 6>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v = {}) const;
/**
* @brief Get the adjoint matrix of SE3 at this.
* @note See Eq. (175).
*/
Jacobian adj() const;
// SE3 specific functions
/**
* Get the transformation matrix (3D isometry).
* @note T = | R t |
* | 0 1 |
*/
Transformation transform() const;
/**
* Get the isometry object (Eigen 3D isometry).
* @note T = | R t |
* | 0 1 |
*/
Isometry isometry() const;
/**
* @brief Get the rotational part of this as a rotation matrix.
*/
Rotation rotation() const;
/**
* @brief Get the rotational part of this as a quaternion.
*/
QuaternionDataType quat() const;
/**
* @brief Get the translational part in vector form.
*/
Translation translation() const;
/**
* @brief Get the x component of the translational part.
*/
Scalar x() const;
/**
* @brief Get the y component of translational part.
*/
Scalar y() const;
/**
* @brief Get the z component of translational part.
*/
Scalar z() const;
//Scalar roll() const;
//Scalar pitch() const;
//Scalar yaw() const;
/**
* @brief Normalize the underlying quaternion.
*/
void normalize();
/**
* @brief Set the rotational as a quaternion.
* @param quaternion a unitary quaternion
*/
void quat(const QuaternionDataType& quaternion);
/**
* @brief Set the rotational as a quaternion.
* @param quaternion an Eigen::Vector representing a unitary quaternion
*/
template <typename _EigenDerived>
void quat(const Eigen::MatrixBase<_EigenDerived>& quaternion);
/**
* @brief Set the rotational as a so3 object.
* @param so3 a manif::SO3 object
*/
void quat(const SO3<Scalar>& so3);
/**
* @brief Set the translation of the SE3 object
* @param translation, 3d-vector representing the translation
*/
void translation(const Translation& translation);
public: /// @todo make protected
Eigen::Map<const SO3<Scalar>> asSO3() const
{
return Eigen::Map<const SO3<Scalar>>(coeffs().data()+3);
}
Eigen::Map<SO3<Scalar>> asSO3()
{
return Eigen::Map<SO3<Scalar>>(coeffs().data()+3);
}
};
template <typename _Derived>
typename SE3Base<_Derived>::Transformation
SE3Base<_Derived>::transform() const
{
Transformation T = Transformation::Identity();
T.template topLeftCorner<3,3>() = rotation();
T.template topRightCorner<3,1>() = translation();
return T;
}
template <typename _Derived>
typename SE3Base<_Derived>::Isometry
SE3Base<_Derived>::isometry() const
{
return Isometry(transform());
}
template <typename _Derived>
typename SE3Base<_Derived>::Rotation
SE3Base<_Derived>::rotation() const
{
return asSO3().rotation();
}
template <typename _Derived>
typename SE3Base<_Derived>::QuaternionDataType
SE3Base<_Derived>::quat() const
{
return asSO3().quat();
}
template <typename _Derived>
typename SE3Base<_Derived>::Translation
SE3Base<_Derived>::translation() const
{
return coeffs().template head<3>();
}
template <typename _Derived>
void SE3Base<_Derived>::quat(const QuaternionDataType& quaternion)
{
quat(quaternion.coeffs());
}
template <typename _Derived>
template <typename _EigenDerived>
void SE3Base<_Derived>::quat(const Eigen::MatrixBase<_EigenDerived>& quaternion)
{
using std::abs;
assert_vector_dim(quaternion, 4);
MANIF_ASSERT(abs(quaternion.norm()-Scalar(1)) <
Constants<Scalar>::eps,
"The quaternion is not normalized !",
invalid_argument);
asSO3().coeffs() = quaternion;
}
template <typename _Derived>
void SE3Base<_Derived>::quat(const SO3<Scalar>& so3)
{
quat(so3.coeffs());
}
template <typename _Derived>
void SE3Base<_Derived>::translation(const Translation& translation)
{
coeffs().template head<3>() = translation;
}
template <typename _Derived>
typename SE3Base<_Derived>::LieGroup
SE3Base<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
if (J_minv_m)
{
(*J_minv_m) = -adj();
}
const SO3<Scalar> so3inv = asSO3().inverse();
return LieGroup(-so3inv.act(translation()),
so3inv);
}
template <typename _Derived>
typename SE3Base<_Derived>::Tangent
SE3Base<_Derived>::log(OptJacobianRef J_t_m) const
{
using std::abs;
using std::sqrt;
const SO3Tangent<Scalar> so3tan = asSO3().log();
Tangent tan((typename Tangent::DataType() <<
so3tan.ljacinv()*translation(),
so3tan.coeffs()).finished());
if (J_t_m)
{
// Jr^-1
(*J_t_m) = tan.rjacinv();
}
return tan;
}
template <typename _Derived>
typename SE3Base<_Derived>::Tangent
SE3Base<_Derived>::lift(OptJacobianRef J_t_m) const
{
return log(J_t_m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename SE3Base<_Derived>::LieGroup
SE3Base<_Derived>::compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb) const
{
static_assert(
std::is_base_of<SE3Base<_DerivedOther>, _DerivedOther>::value,
"Argument does not inherit from SE3Base !");
const auto& m_se3 = static_cast<const SE3Base<_DerivedOther>&>(m);
if (J_mc_ma)
{
(*J_mc_ma) = m.inverse().adj();
}
if (J_mc_mb)
{
J_mc_mb->setIdentity();
}
return LieGroup(rotation()*m_se3.translation() + translation(),
asSO3().compose(m_se3.asSO3()).quat());
}
template <typename _Derived>
template <typename _EigenDerived>
Eigen::Matrix<typename SE3Base<_Derived>::Scalar, 3, 1>
SE3Base<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 6>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v) const
{
assert_vector_dim(v, 3);
const Rotation R(rotation());
if (J_vout_m)
{
J_vout_m->template topLeftCorner<3,3>() = R;
J_vout_m->template topRightCorner<3,3>() = -R * skew(v);
}
if (J_vout_v)
{
(*J_vout_v) = R;
}
return translation() + R * v;
}
template <typename _Derived>
typename SE3Base<_Derived>::Jacobian
SE3Base<_Derived>::adj() const
{
/// @note Chirikjian (close to Eq.10.94)
/// says
/// Ad(g) = | R 0 |
/// | T.R R |
///
/// considering vee(log(g)) = (w;v)
/// with T = [t]_x
///
/// but this is
/// Ad(g) = | R T.R |
/// | 0 R |
///
/// considering vee(log(g)) = (v;w)
Jacobian Adj;
Adj.template topLeftCorner<3,3>() = rotation();
Adj.template bottomRightCorner<3,3>() =
Adj.template topLeftCorner<3,3>();
Adj.template topRightCorner<3,3>().noalias() =
skew(translation()) * Adj.template topLeftCorner<3,3>();
Adj.template bottomLeftCorner<3,3>().setZero();
return Adj;
}
// SE3 specific function
template <typename _Derived>
typename SE3Base<_Derived>::Scalar
SE3Base<_Derived>::x() const
{
return coeffs().x();
}
template <typename _Derived>
typename SE3Base<_Derived>::Scalar
SE3Base<_Derived>::y() const
{
return coeffs().y();
}
template <typename _Derived>
typename SE3Base<_Derived>::Scalar
SE3Base<_Derived>::z() const
{
return coeffs().z();
}
template <typename _Derived>
void SE3Base<_Derived>::normalize()
{
coeffs().template tail<4>().normalize();
}
namespace internal {
//! @brief Random specialization for SE3Base objects.
template <typename Derived>
struct RandomEvaluatorImpl<SE3Base<Derived>>
{
template <typename T>
static void run(T& m)
{
using Scalar = typename SE3Base<Derived>::Scalar;
using Translation = typename SE3Base<Derived>::Translation;
using LieGroup = typename SE3Base<Derived>::LieGroup;
m = LieGroup(Translation::Random(), randQuat<Scalar>());
}
};
//! @brief Assignment assert specialization for SE3Base objects
template <typename Derived>
struct AssignmentEvaluatorImpl<SE3Base<Derived>>
{
template <typename T>
static void run_impl(const T& data)
{
using std::abs;
MANIF_ASSERT(
abs(data.template tail<4>().norm()-typename SE3Base<Derived>::Scalar(1)) <
Constants<typename SE3Base<Derived>::Scalar>::eps,
"SE3 assigned data not normalized !",
manif::invalid_argument
);
MANIF_UNUSED_VARIABLE(data);
}
};
//! @brief Cast specialization for SE3Base objects.
template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SE3Base<Derived>, NewScalar> {
template <typename T>
static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
const typename SE3Base<Derived>::QuaternionDataType q = o.quat();
const typename SE3Base<Derived>::Translation t = o.translation();
return typename Derived::template LieGroupTemplate<NewScalar>(
t.template cast<NewScalar>(), q.template cast<NewScalar>().normalized()
);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE3_BASE_H_ */

91
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se3/SE3_map.h vendored Normal file
View File

@ -0,0 +1,91 @@
#ifndef _MANIF_MANIF_SE3_MAP_H_
#define _MANIF_MANIF_SE3_MAP_H_
#include "manif/impl/se3/SE3.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SE3<_Scalar>,0> >
: public traits<SE3<_Scalar>>
{
using typename traits<SE3<_Scalar>>::Scalar;
using traits<SE3<Scalar>>::RepSize;
using Base = SE3Base<Eigen::Map<SE3<Scalar>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits< Eigen::Map<const SE3<_Scalar>,0> >
: public traits<const SE3<_Scalar>>
{
using typename traits<const SE3<_Scalar>>::Scalar;
using traits<const SE3<Scalar>>::RepSize;
using Base = SE3Base<Eigen::Map<const SE3<Scalar>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SE3
*/
template <class _Scalar>
class Map<manif::SE3<_Scalar>, 0>
: public manif::SE3Base<Map<manif::SE3<_Scalar>, 0> >
{
using Base = manif::SE3Base<Map<manif::SE3<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_GROUP_MAP_ASSIGN_OP(SE3)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SE3
*/
template <class _Scalar>
class Map<const manif::SE3<_Scalar>, 0>
: public manif::SE3Base<Map<const manif::SE3<_Scalar>, 0> >
{
using Base = manif::SE3Base<Map<const manif::SE3<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SE3_MAP_H_ */

33
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se3/SE3_properties.h vendored Normal file
View File

@ -0,0 +1,33 @@
#ifndef _MANIF_MANIF_SE3_PROPERTIES_H_
#define _MANIF_MANIF_SE3_PROPERTIES_H_
#include "manif/impl/traits.h"
namespace manif {
// Forward declaration
template <typename _Derived> struct SE3Base;
template <typename _Derived> struct SE3TangentBase;
namespace internal {
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SE3Base<_Derived>>
{
static constexpr int Dim = 3; /// @brief Space dimension
static constexpr int DoF = 6; /// @brief Degrees of freedom
};
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SE3TangentBase<_Derived>>
{
static constexpr int Dim = 3; /// @brief Space dimension
static constexpr int DoF = 6; /// @brief Degrees of freedom
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE3_PROPERTIES_H_ */

230
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se_2_3/SE_2_3.h vendored Normal file
View File

@ -0,0 +1,230 @@
#ifndef _MANIF_MANIF_SE_2_3_H_
#define _MANIF_MANIF_SE_2_3_H_
#include "manif/impl/se_2_3/SE_2_3_base.h"
namespace manif {
// Forward declare for type traits specialization
template <typename _Scalar> struct SE_2_3;
template <typename _Scalar> struct SE_2_3Tangent;
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SE_2_3<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SE_2_3<_Scalar>;
using Tangent = SE_2_3Tangent<_Scalar>;
using Base = SE_2_3Base<SE_2_3<_Scalar>>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = 10;
/// @todo would be nice to concat vec3 + quaternion + vec3
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using Rotation = Eigen::Matrix<Scalar, Dim, Dim>;
using Translation = Eigen::Matrix<Scalar, Dim, 1>;
using LinearVelocity = Eigen::Matrix<Scalar, Dim, 1>;
using Vector = Eigen::Matrix<Scalar, Dim, 1>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// LieGroup
//
/**
* @brief Represent an element of SE_2_3.
*/
template <typename _Scalar>
struct SE_2_3 : SE_2_3Base<SE_2_3<_Scalar>>
{
private:
using Base = SE_2_3Base<SE_2_3<_Scalar>>;
using Type = SE_2_3<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_COMPLETE_GROUP_TYPEDEF
using Translation = typename Base::Translation;
using Quaternion = Eigen::Quaternion<Scalar>;
using LinearVelocity = typename Base::LinearVelocity;
MANIF_INHERIT_GROUP_API
using Base::rotation;
using Base::normalize;
SE_2_3() = default;
~SE_2_3() = default;
MANIF_COPY_CONSTRUCTOR(SE_2_3)
MANIF_MOVE_CONSTRUCTOR(SE_2_3)
template <typename _DerivedOther>
SE_2_3(const LieGroupBase<_DerivedOther>& o);
MANIF_GROUP_ASSIGN_OP(SE_2_3)
/**
* @brief Constructor given a translation, a unit quaternion and a linear velocity.
* @param[in] t A translation vector.
* @param[in] q A unit quaternion.
* @param[in] v A linear velocity vector.
* @throws manif::invalid_argument on un-normalized complex number.
*/
SE_2_3(const Translation& t,
const Eigen::Quaternion<Scalar>& q,
const LinearVelocity& v);
/**
* @brief Constructor given a translation, an angle axis and a linear velocity.
* @param[in] t A translation vector.
* @param[in] angle_axis An angle-axis.
* @param[in] v A linear velocity vector.
*/
SE_2_3(const Translation& t,
const Eigen::AngleAxis<Scalar>& angle_axis,
const LinearVelocity& v);
/**
* @brief Constructor given a translation, SO3 element and a linear velocity.
* @param[in] t A translation vector.
* @param[in] SO3 An element of SO3.
* @param[in] v A linear velocity vector.
*/
SE_2_3(const Translation& t,
const SO3<Scalar>& SO3,
const LinearVelocity& v);
/**
* @brief Constructor given translation components,
* roll-pitch-yaw angles and linear velocity components
* @param[in] x The x component of the translation.
* @param[in] y The y component of the translation.
* @param[in] z The z component of the translation.
* @param[in] roll The roll angle.
* @param[in] pitch The pitch angle.
* @param[in] yaw The yaw angle.
* @param[in] vx The x component of the linear velocity.
* @param[in] vy The y component of the linear velocity.
* @param[in] vz The z component of the linear velocity.
*/
SE_2_3(const Scalar x, const Scalar y, const Scalar z,
const Scalar roll, const Scalar pitch, const Scalar yaw,
const Scalar vx, const Scalar vy, const Scalar vz);
/**
* @brief Constructor from a 3D Eigen::Isometry<Scalar> relevant to SE(3) and a linear velocity
* @param[in] h a isometry object from Eigen defined for SE(3)
* @param[in] v a linear velocity vector.
* @note overall, this should be a double direct spatial isometry,
*/
SE_2_3(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h, const LinearVelocity& v);
// LieGroup common API
DataType& coeffs();
const DataType& coeffs() const;
// SE_2_3 specific API
protected:
DataType data_;
};
MANIF_EXTRA_GROUP_TYPEDEF(SE_2_3)
template <typename _Scalar>
template <typename _DerivedOther>
SE_2_3<_Scalar>::SE_2_3(
const LieGroupBase<_DerivedOther>& o)
: SE_2_3(o.coeffs())
{
//
}
template <typename _Scalar>
SE_2_3<_Scalar>::SE_2_3(const Translation& t,
const Eigen::Quaternion<Scalar>& q,
const LinearVelocity& v)
: SE_2_3((DataType() << t, q.coeffs(), v ).finished())
{
//
}
template <typename _Scalar>
SE_2_3<_Scalar>::SE_2_3(const Translation& t,
const Eigen::AngleAxis<Scalar>& a,
const LinearVelocity& v)
: SE_2_3(t, Quaternion(a), v)
{
//
}
template <typename _Scalar>
SE_2_3<_Scalar>::SE_2_3(const Scalar x, const Scalar y, const Scalar z,
const Scalar roll, const Scalar pitch, const Scalar yaw,
const Scalar vx, const Scalar vy, const Scalar vz)
: SE_2_3(Translation(x,y,z), Eigen::Quaternion<Scalar>(
Eigen::AngleAxis<Scalar>(yaw, Eigen::Matrix<Scalar, 3, 1>::UnitZ()) *
Eigen::AngleAxis<Scalar>(pitch, Eigen::Matrix<Scalar, 3, 1>::UnitY()) *
Eigen::AngleAxis<Scalar>(roll, Eigen::Matrix<Scalar, 3, 1>::UnitX()) ), LinearVelocity(vx, vy, vz))
{
//
}
template <typename _Scalar>
SE_2_3<_Scalar>::SE_2_3(const Translation& t,
const SO3<Scalar>& so3,
const LinearVelocity& v)
: SE_2_3(t, so3.quat(), v)
{
//
}
template <typename _Scalar>
SE_2_3<_Scalar>::SE_2_3(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h, const LinearVelocity& v)
: SE_2_3(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation()), v)
{
//
}
template <typename _Scalar>
typename SE_2_3<_Scalar>::DataType&
SE_2_3<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SE_2_3<_Scalar>::DataType&
SE_2_3<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SE_2_3_H_ */

112
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent.h vendored Normal file
View File

@ -0,0 +1,112 @@
#ifndef _MANIF_MANIF_SE_2_3TANGENT_H_
#define _MANIF_MANIF_SE_2_3TANGENT_H_
#include "manif/impl/se_2_3/SE_2_3Tangent_base.h"
namespace manif {
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SE_2_3Tangent<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SE_2_3<_Scalar>;
using Tangent = SE_2_3Tangent<_Scalar>;
using Base = SE_2_3TangentBase<Tangent>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = DoF;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using LieAlg = Eigen::Matrix<Scalar, 5, 5>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// Tangent
//
/**
* @brief Represents an element of tangent space of SE_2_3.
*/
template <typename _Scalar>
struct SE_2_3Tangent : SE_2_3TangentBase<SE_2_3Tangent<_Scalar>>
{
private:
using Base = SE_2_3TangentBase<SE_2_3Tangent<_Scalar>>;
using Type = SE_2_3Tangent<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
SE_2_3Tangent() = default;
~SE_2_3Tangent() = default;
MANIF_COPY_CONSTRUCTOR(SE_2_3Tangent)
MANIF_MOVE_CONSTRUCTOR(SE_2_3Tangent)
template <typename _DerivedOther>
SE_2_3Tangent(const TangentBase<_DerivedOther>& o);
MANIF_TANGENT_ASSIGN_OP(SE_2_3Tangent)
// Tangent common API
DataType& coeffs();
const DataType& coeffs() const;
// SE_2_3Tangent specific API
protected:
DataType data_;
};
MANIF_EXTRA_TANGENT_TYPEDEF(SE_2_3Tangent);
template <typename _Scalar>
template <typename _DerivedOther>
SE_2_3Tangent<_Scalar>::SE_2_3Tangent(
const TangentBase<_DerivedOther>& o)
: data_(o.coeffs())
{
//
}
template <typename _Scalar>
typename SE_2_3Tangent<_Scalar>::DataType&
SE_2_3Tangent<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SE_2_3Tangent<_Scalar>::DataType&
SE_2_3Tangent<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SE_2_3TANGENT_H_ */

495
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent_base.h vendored Normal file
View File

@ -0,0 +1,495 @@
#ifndef _MANIF_MANIF_SE_2_3TANGENT_BASE_H_
#define _MANIF_MANIF_SE_2_3TANGENT_BASE_H_
#include "manif/impl/se_2_3/SE_2_3_properties.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/so3/SO3Tangent_map.h"
#include "manif/impl/se3/SE3Tangent_map.h"
namespace manif {
//
// Tangent
//
/**
* @brief The base class of the SE_2_3 tangent.
*/
template <typename _Derived>
struct SE_2_3TangentBase : TangentBase<_Derived>
{
private:
using Base = TangentBase<_Derived>;
using Type = SE_2_3TangentBase<_Derived>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
using LinBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
using AngBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
using ConstLinBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;
using ConstAngBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;
using Base::data;
using Base::coeffs;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SE_2_3TangentBase)
public:
MANIF_TANGENT_ML_ASSIGN_OP(SE_2_3TangentBase)
// Tangent common API
/**
* @brief Hat operator of SE_2_3.
* @return An element of the Lie algebra se_2_3.
* @note See Eq. (169).
*/
LieAlg hat() const;
/**
* @brief Get the SE_2_3 element.
* @param[out] -optional- J_m_t Jacobian of the SE_2_3 element wrt this.
* @return The SE_2_3 element.
* @note This is the exp() map with the argument in vector form.
*/
LieGroup exp(OptJacobianRef J_m_t = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t = {}) const;
/**
* @brief Get the right Jacobian of SE_2_3.
*/
Jacobian rjac() const;
/**
* @brief Get the inverse right Jacobian of SE_2_3.
*/
Jacobian rjacinv() const;
/**
* @brief Get the left Jacobian of SE_2_3.
*/
Jacobian ljac() const;
/**
* @brief Get the inverse left Jacobian of SE_2_3.
*/
Jacobian ljacinv() const;
/**
* @brief Get the small adjoint matrix ad() of SE_2_3
* that maps isomorphic tangent vectors of SE_2_3
* @return
*/
Jacobian smallAdj() const;
// SE_2_3Tangent specific API
//! @brief Get the linear velocity part.
LinBlock lin();
const ConstLinBlock lin() const;
//! @brief Get the angular part.
AngBlock ang();
const ConstAngBlock ang() const;
//! @brief Get the linear acceleration part
LinBlock lin2();
const ConstLinBlock lin2() const;
public: /// @todo make protected
const Eigen::Map<const SO3Tangent<Scalar>> asSO3() const
{
return Eigen::Map<const SO3Tangent<Scalar>>(coeffs().data()+3);
}
Eigen::Map<SO3Tangent<Scalar>> asSO3()
{
return Eigen::Map<SO3Tangent<Scalar>>(coeffs().data()+3);
}
};
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::LieGroup
SE_2_3TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
if (J_m_t)
{
*J_m_t = rjac();
}
const Eigen::Map<const SO3Tangent<Scalar>> so3 = asSO3();
const typename SO3<Scalar>::Jacobian so3_ljac = so3.ljac();
return LieGroup(so3_ljac*lin(),
so3.exp().quat(),
so3_ljac*lin2());
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::LieGroup
SE_2_3TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
return exp(J_m_t);
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::LieAlg
SE_2_3TangentBase<_Derived>::hat() const
{
return (LieAlg() <<
Scalar(0) , Scalar(-coeffs()(5)), Scalar( coeffs()(4)), Scalar(coeffs()(0)), Scalar(coeffs()(6)),
Scalar( coeffs()(5)), Scalar(0) , Scalar(-coeffs()(3)), Scalar(coeffs()(1)), Scalar(coeffs()(7)),
Scalar(-coeffs()(4)), Scalar( coeffs()(3)), Scalar(0) , Scalar(coeffs()(2)), Scalar(coeffs()(8)),
Scalar(0) , Scalar(0) , Scalar(0) , Scalar(0) , Scalar(0),
Scalar(0) , Scalar(0) , Scalar(0) , Scalar(0) , Scalar(0) ).finished();
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::Jacobian
SE_2_3TangentBase<_Derived>::rjac() const
{
Jacobian Jr;
Jr.template block<6, 3>(3, 0).setZero();
Jr.template block<6, 3>(0, 6).setZero();
Jr.template topLeftCorner<3,3>() = asSO3().rjac();
Jr.template block<3,3>(3,3) = Jr.template topLeftCorner<3,3>();
Jr.template bottomRightCorner<3, 3>() = Jr.template topLeftCorner<3,3>();
// fill Qv
SE3Tangent<Scalar>::fillQ(
Jr.template block<3,3>(0, 3), -coeffs().template head<6>()
);
// fill Qa
Eigen::Matrix<Scalar, 6, 1> aw;
aw << -coeffs()(6), -coeffs()(7), -coeffs()(8),
-coeffs()(3), -coeffs()(4), -coeffs()(5);
SE3Tangent<Scalar>::fillQ(Jr.template block<3,3>(6, 3), aw);
return Jr;
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::Jacobian
SE_2_3TangentBase<_Derived>::rjacinv() const
{
Jacobian Jr_inv;
Jr_inv.template block<3, 3>(3, 0).setZero();
// Jr_inv.template block<3, 3>(6, 0).setZero(); // Serves as temp Q
Jr_inv.template block<6, 3>(0, 6).setZero();
Jr_inv.template topLeftCorner<3, 3>() = asSO3().rjacinv();
Jr_inv.template block<3, 3>(3,3) = Jr_inv.template topLeftCorner<3,3>();
Jr_inv.template bottomRightCorner<3, 3>() = Jr_inv.template topLeftCorner<3,3>();
// fill Qv
SE3Tangent<Scalar>::fillQ(
Jr_inv.template block<3, 3>(6, 0), -coeffs().template head<6>()
);
Jr_inv.template block<3, 3>(0, 3).noalias() =
-Jr_inv.template topLeftCorner<3,3>() *
Jr_inv.template block<3, 3>(6, 0) *
Jr_inv.template topLeftCorner<3,3>();
// fill Qa
Eigen::Matrix<Scalar, 6, 1> aw;
aw << -coeffs()(6), -coeffs()(7), -coeffs()(8),
-coeffs()(3), -coeffs()(4), -coeffs()(5);
SE3Tangent<Scalar>::fillQ(Jr_inv.template block<3, 3>(6, 0), aw);
Jr_inv.template block<3, 3>(6, 3).noalias() =
-Jr_inv.template topLeftCorner<3,3>() *
Jr_inv.template block<3, 3>(6, 0) *
Jr_inv.template topLeftCorner<3,3>();
Jr_inv.template block<3, 3>(6, 0).setZero();
return Jr_inv;
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::Jacobian
SE_2_3TangentBase<_Derived>::ljac() const
{
Jacobian Jl;
Jl.template block<6, 3>(3, 0).setZero();
Jl.template block<6, 3>(0, 6).setZero();
Jl.template topLeftCorner<3,3>() = asSO3().ljac();
Jl.template block<3,3>(3,3) = Jl.template topLeftCorner<3,3>();
Jl.template bottomRightCorner<3, 3>() = Jl.template topLeftCorner<3,3>();
// fill Qv
SE3Tangent<Scalar>::fillQ(
Jl.template block<3,3>(0, 3), coeffs().template head<6>()
);
// fill Qa
Eigen::Matrix<Scalar, 6, 1> aw;
aw << coeffs()(6), coeffs()(7), coeffs()(8),
coeffs()(3), coeffs()(4), coeffs()(5);
SE3Tangent<Scalar>::fillQ(Jl.template block<3,3>(6, 3), aw);
return Jl;
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::Jacobian
SE_2_3TangentBase<_Derived>::ljacinv() const
{
Jacobian Jlinv;
Jlinv.template block<3, 3>(3, 0).setZero();
// Jlinv.template block<3, 3>(6, 0).setZero(); // Serves as temp Q
Jlinv.template block<6, 3>(0, 6).setZero();
Jlinv.template topLeftCorner<3, 3>() = asSO3().ljacinv();
Jlinv.template block<3, 3>(3, 3) = Jlinv.template topLeftCorner<3,3>();
Jlinv.template bottomRightCorner<3, 3>() = Jlinv.template topLeftCorner<3,3>();
// fill Qv
SE3Tangent<Scalar>::fillQ(
Jlinv.template block<3, 3>(6, 0), coeffs().template head<6>()
);
Jlinv.template block<3, 3>(0, 3).noalias() =
-Jlinv.template topLeftCorner<3, 3>() *
Jlinv.template block<3, 3>(6, 0) *
Jlinv.template topLeftCorner<3, 3>();
// fill Qa
Eigen::Matrix<Scalar, 6, 1> aw;
aw << coeffs()(6), coeffs()(7), coeffs()(8),
coeffs()(3), coeffs()(4), coeffs()(5);
SE3Tangent<Scalar>::fillQ(Jlinv.template block<3, 3>(6, 0), aw);
Jlinv.template block<3, 3>(6, 3).noalias() =
-Jlinv.template topLeftCorner<3, 3>() *
Jlinv.template block<3, 3>(6, 0) *
Jlinv.template topLeftCorner<3, 3>();
Jlinv.template block<3, 3>(6, 0).setZero();
return Jlinv;
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::Jacobian
SE_2_3TangentBase<_Derived>::smallAdj() const
{
/// this is
/// ad(g) = | Omega V 0|
/// | 0 Omega 0|
/// | 0 A Omega|
///
/// considering vee(log(g)) = (v;w; a)
Jacobian smallAdj;
smallAdj.template block<6, 3>(3, 0).setZero();
smallAdj.template block<6, 3>(0, 6).setZero();
smallAdj.template block<3,3>(0, 3) = skew(lin());
smallAdj.template topLeftCorner<3,3>() = skew(ang());
smallAdj.template block<3,3>(3,3) = smallAdj.template topLeftCorner<3,3>();
smallAdj.template bottomRightCorner<3,3>() = smallAdj.template topLeftCorner<3,3>();
smallAdj.template block<3,3>(6, 3) = skew(lin2());
return smallAdj;
}
// SE_2_3Tangent specific API
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::LinBlock
SE_2_3TangentBase<_Derived>::lin()
{
return coeffs().template head<3>();
}
template <typename _Derived>
const typename SE_2_3TangentBase<_Derived>::ConstLinBlock
SE_2_3TangentBase<_Derived>::lin() const
{
return coeffs().template head<3>();
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::AngBlock
SE_2_3TangentBase<_Derived>::ang()
{
return coeffs().template segment<3>(3);
}
template <typename _Derived>
const typename SE_2_3TangentBase<_Derived>::ConstAngBlock
SE_2_3TangentBase<_Derived>::ang() const
{
return coeffs().template segment<3>(3);
}
template <typename _Derived>
typename SE_2_3TangentBase<_Derived>::LinBlock
SE_2_3TangentBase<_Derived>::lin2()
{
return coeffs().template tail<3>();
}
template <typename _Derived>
const typename SE_2_3TangentBase<_Derived>::ConstLinBlock
SE_2_3TangentBase<_Derived>::lin2() const
{
return coeffs().template tail<3>();
}
namespace internal {
//! @brief Generator specialization for SE_2_3TangentBase objects.
template <typename Derived>
struct GeneratorEvaluator<SE_2_3TangentBase<Derived>>
{
static typename SE_2_3TangentBase<Derived>::LieAlg
run(const unsigned int i)
{
using LieAlg = typename SE_2_3TangentBase<Derived>::LieAlg;
using Scalar = typename SE_2_3TangentBase<Derived>::Scalar;
switch (i)
{
case 0:
{
static const LieAlg E0(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)).finished());
return E0;
}
case 1:
{
static const LieAlg E1(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E1;
}
case 2:
{
static const LieAlg E2(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E2;
}
case 3:
{
static const LieAlg E3(
(LieAlg() << Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(-1), Scalar(0), Scalar(0),
Scalar(0), Scalar(1), Scalar( 0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0) ).finished());
return E3;
}
case 4:
{
static const LieAlg E4(
(LieAlg() << Scalar( 0), Scalar(0), Scalar(1), Scalar(0), Scalar(0),
Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E4;
}
case 5:
{
static const LieAlg E5(
(LieAlg() << Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
Scalar(1), Scalar( 0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E5;
}
case 6:
{
static const LieAlg E6(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E6;
}
case 7:
{
static const LieAlg E7(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E7;
}
case 8:
{
static const LieAlg E8(
(LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
return E8;
}
default:
MANIF_THROW("Index i must be in [0,8]!", invalid_argument);
break;
}
return LieAlg{};
}
};
//! @brief Random specialization for SE_2_3TangentBase objects.
template <typename Derived>
struct RandomEvaluatorImpl<SE_2_3TangentBase<Derived>>
{
static void run(SE_2_3TangentBase<Derived>& m)
{
// in [-1,1]
m.coeffs().setRandom();
// In ball of radius PI
m.coeffs().template segment<3>(3) = randPointInBall(MANIF_PI).template cast<typename Derived::Scalar>();
}
};
//! @brief Vee specialization for SE_2_3TangentBase objects.
template <typename Derived>
struct VeeEvaluatorImpl<SE_2_3TangentBase<Derived>> {
template <typename TL, typename TR>
static void run(TL& t, const TR& v) {
t.coeffs() << v(0, 3), v(1, 3), v(2, 3),
v(2, 1), v(0, 2), v(1, 0),
v(0, 4), v(1, 4), v(2, 4);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE_2_3_BASE_H_ */

89
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent_map.h vendored Normal file
View File

@ -0,0 +1,89 @@
#ifndef _MANIF_MANIF_SE_2_3TANGENT_MAP_H_
#define _MANIF_MANIF_SE_2_3TANGENT_MAP_H_
#include "manif/impl/se_2_3/SE_2_3Tangent.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SE_2_3Tangent<_Scalar>,0> >
: public traits<SE_2_3Tangent<_Scalar>>
{
using typename traits<SE_2_3Tangent<_Scalar>>::Scalar;
using traits<SE_2_3Tangent<_Scalar>>::DoF;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SE_2_3TangentBase<Eigen::Map<SE_2_3Tangent<Scalar>, 0>>;
};
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<const SE_2_3Tangent<_Scalar>,0> >
: public traits<const SE_2_3Tangent<_Scalar>>
{
using typename traits<const SE_2_3Tangent<_Scalar>>::Scalar;
using traits<const SE_2_3Tangent<_Scalar>>::DoF;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SE_2_3TangentBase<Eigen::Map<const SE_2_3Tangent<Scalar>, 0>>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SE_2_3
*/
template <class _Scalar>
class Map<manif::SE_2_3Tangent<_Scalar>, 0>
: public manif::SE_2_3TangentBase<Map<manif::SE_2_3Tangent<_Scalar>, 0> >
{
using Base = manif::SE_2_3TangentBase<Map<manif::SE_2_3Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_TANGENT_MAP_ASSIGN_OP(SE_2_3Tangent)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SE_2_3
*/
template <class _Scalar>
class Map<const manif::SE_2_3Tangent<_Scalar>, 0>
: public manif::SE_2_3TangentBase<Map<const manif::SE_2_3Tangent<_Scalar>, 0> >
{
using Base = manif::SE_2_3TangentBase<Map<const manif::SE_2_3Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SE_2_3TANGENT_MAP_H_ */

491
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se_2_3/SE_2_3_base.h vendored Normal file
View File

@ -0,0 +1,491 @@
#ifndef _MANIF_MANIF_SE_2_3_BASE_H_
#define _MANIF_MANIF_SE_2_3_BASE_H_
#include "manif/impl/se_2_3/SE_2_3_properties.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/so3/SO3_map.h"
#include "manif/impl/se3/SE3_map.h"
namespace manif {
//
// LieGroup
//
/**
* @brief The base class of the SE_2_3 group.
* @note See Appendix A2 in the paper "The Invariant Extended Kalman filter as a stable
observer".
* However, note that the serialization used in that paper is different from that defined below
* The paper uses a SE_2_3 definition as,
* X = |R v p|
* | 1 |
* | 1|
* with a vector space serialization as (w, a, v)
* Instead, here we define the SE_2_3 to be,
* X = |R p v|
* | 1 |
* | 1|
* with a vector space serialization as (v, w, a)
*/
template <typename _Derived>
struct SE_2_3Base : LieGroupBase<_Derived>
{
private:
using Base = LieGroupBase<_Derived>;
using Type = SE_2_3Base<_Derived>;
public:
MANIF_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_AUTO_API
MANIF_INHERIT_GROUP_OPERATOR
using Base::coeffs;
using Rotation = typename internal::traits<_Derived>::Rotation;
using Translation = typename internal::traits<_Derived>::Translation;
using LinearVelocity = typename internal::traits<_Derived>::LinearVelocity;
using Transformation = Eigen::Matrix<Scalar, 5, 5>;
using Isometry = Eigen::Matrix<Scalar, 5, 5>; /**< Double direct spatial isometry*/
using QuaternionDataType = Eigen::Quaternion<Scalar>;
// LieGroup common API
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SE_2_3Base)
public:
MANIF_GROUP_ML_ASSIGN_OP(SE_2_3Base)
/**
* @brief Get the inverse.
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this.
*/
LieGroup inverse(OptJacobianRef J_minv_m = {}) const;
/**
* @brief Get the SE_2_3 corresponding Lie algebra element in vector form.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt to this.
* @return The SE_2_3 tangent of this.
* @note This is the log() map in vector form.
* @see SE_2_3Tangent.
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another SE_2_3 element.
* @param[in] m Another SE_2_3 element.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
*/
template <typename _DerivedOther>
LieGroup compose(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}) const;
/**
* @brief Get the action of the underlying SE(3) element on a 3d point
* @note this method by default returns a rigid motion action on 3d points and
* does not take into account the embedded linear velocity of total SE_2(3) element
* @param[in] v A 3D point.
* @param[out] -optional- J_vout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object.
*/
template <typename _EigenDerived>
Eigen::Matrix<Scalar, 3, 1>
act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 9>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v = {}) const;
/**
* @brief Get the adjoint matrix of SE_2_3 at this.
*/
Jacobian adj() const;
// SE_2_3 specific functions
/**
* Get the isometry object (double direct isometry).
* @note T = | R t v|
* | 1 |
* | 1|
*/
Transformation transform() const;
/**
* Get the isometry object (double direct isometry).
* @note T = | R t v|
* | 1 |
* | 1|
*/
Isometry isometry() const;
/**
* @brief Get the rotational part of this as a rotation matrix.
*/
Rotation rotation() const;
/**
* @brief Get the rotational part of this as a quaternion.
*/
QuaternionDataType quat() const;
/**
* @brief Get the translational part in vector form.
*/
Translation translation() const;
/**
* @brief Get the x component of the translational part.
*/
Scalar x() const;
/**
* @brief Get the y component of translational part.
*/
Scalar y() const;
/**
* @brief Get the z component of translational part.
*/
Scalar z() const;
/**
* @brief Get the linear velocity part in vector form.
*/
LinearVelocity linearVelocity() const;
/**
* @brief Get the x component of the linear velocity part.
*/
Scalar vx() const;
/**
* @brief Get the y component of linear velocity part.
*/
Scalar vy() const;
/**
* @brief Get the z component of linear velocity part.
*/
Scalar vz() const;
/**
* @brief Normalize the underlying quaternion.
*/
void normalize();
public: /// @todo make protected
Eigen::Map<const SO3<Scalar>> asSO3() const
{
return Eigen::Map<const SO3<Scalar>>(coeffs().data()+3);
}
Eigen::Map<SO3<Scalar>> asSO3()
{
return Eigen::Map<SO3<Scalar>>(coeffs().data()+3);
}
};
template <typename _Derived>
typename SE_2_3Base<_Derived>::Transformation
SE_2_3Base<_Derived>::transform() const
{
Eigen::Matrix<Scalar, 5, 5> T;
T.template topLeftCorner<3,3>() = rotation();
T.template block<3, 1>(0, 3) = translation();
T.template topRightCorner<3,1>() = linearVelocity();
T.template bottomLeftCorner<2,3>().setZero();
T.template bottomRightCorner<2,2>().setIdentity();
return T;
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Isometry
SE_2_3Base<_Derived>::isometry() const
{
return Isometry(transform());
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Rotation
SE_2_3Base<_Derived>::rotation() const
{
return asSO3().rotation();
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::QuaternionDataType
SE_2_3Base<_Derived>::quat() const
{
return asSO3().quat();
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Translation
SE_2_3Base<_Derived>::translation() const
{
return coeffs().template head<3>();
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::LinearVelocity
SE_2_3Base<_Derived>::linearVelocity() const
{
return coeffs().template tail<3>();
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::LieGroup
SE_2_3Base<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
if (J_minv_m)
{
(*J_minv_m) = -adj();
}
const SO3<Scalar> so3inv = asSO3().inverse();
return LieGroup(-so3inv.act(translation()),
so3inv,
-so3inv.act(linearVelocity()));
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Tangent
SE_2_3Base<_Derived>::log(OptJacobianRef J_t_m) const
{
const SO3Tangent<Scalar> so3tan = asSO3().log();
Tangent tan((typename Tangent::DataType() <<
so3tan.ljacinv()*translation(),
so3tan.coeffs(),
so3tan.ljacinv()*linearVelocity()).finished());
if (J_t_m)
{
// Jr^-1
(*J_t_m) = tan.rjacinv();
}
return tan;
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Tangent
SE_2_3Base<_Derived>::lift(OptJacobianRef J_t_m) const
{
return log(J_t_m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename SE_2_3Base<_Derived>::LieGroup
SE_2_3Base<_Derived>::compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb) const
{
static_assert(
std::is_base_of<SE_2_3Base<_DerivedOther>, _DerivedOther>::value,
"Argument does not inherit from SE_2_3Base !");
const auto& m_se_2_3 = static_cast<const SE_2_3Base<_DerivedOther>&>(m);
if (J_mc_ma)
{
(*J_mc_ma) = m.inverse().adj();
}
if (J_mc_mb)
{
J_mc_mb->setIdentity();
}
return LieGroup(rotation()*m_se_2_3.translation() + translation(),
asSO3().compose(m_se_2_3.asSO3()).quat(),
rotation()*m_se_2_3.linearVelocity() + linearVelocity());
}
template <typename _Derived>
template <typename _EigenDerived>
Eigen::Matrix<typename SE_2_3Base<_Derived>::Scalar, 3, 1>
SE_2_3Base<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 9>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v) const
{
assert_vector_dim(v, 3);
const Rotation R(rotation());
if (J_vout_m)
{
J_vout_m->template topLeftCorner<3,3>() = R;
J_vout_m->template block<3,3>(0, 3).noalias() = -R * skew(v);
J_vout_m->template topRightCorner<3,3>().setZero();
}
if (J_vout_v)
{
(*J_vout_v) = R;
}
return translation() + R * v;
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Jacobian
SE_2_3Base<_Derived>::adj() const
{
///
/// this is
/// Ad(g) = | R T.R 0|
/// | 0 R 0|
/// | 0 V.R R|
///
/// considering vee(log(g)) = (v;w;a)
/// with T = [t]_x
/// with V = [v]_x
Jacobian Adj;
Adj.template topLeftCorner<3,3>() = rotation();
Adj.template bottomRightCorner<3,3>() =
Adj.template topLeftCorner<3,3>();
Adj.template block<3,3>(3,3) =
Adj.template topLeftCorner<3,3>();
Adj.template block<3,3>(0, 3).noalias() =
skew(translation()) * Adj.template topLeftCorner<3,3>();
Adj.template block<3,3>(6, 3).noalias() =
skew(linearVelocity()) * Adj.template topLeftCorner<3,3>();
Adj.template bottomLeftCorner<6,3>().setZero();
Adj.template topRightCorner<6,3>().setZero();
return Adj;
}
// SE_2_3 specific function
template <typename _Derived>
typename SE_2_3Base<_Derived>::Scalar
SE_2_3Base<_Derived>::x() const
{
return coeffs()(0);
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Scalar
SE_2_3Base<_Derived>::y() const
{
return coeffs()(1);
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Scalar
SE_2_3Base<_Derived>::z() const
{
return coeffs()(2);
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Scalar
SE_2_3Base<_Derived>::vx() const
{
return coeffs()(7);
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Scalar
SE_2_3Base<_Derived>::vy() const
{
return coeffs()(8);
}
template <typename _Derived>
typename SE_2_3Base<_Derived>::Scalar
SE_2_3Base<_Derived>::vz() const
{
return coeffs()(9);
}
template <typename _Derived>
void SE_2_3Base<_Derived>::normalize()
{
coeffs().template segment<4>(3).normalize();
}
namespace internal {
//! @brief Random specialization for SE_2_3Base objects.
template <typename Derived>
struct RandomEvaluatorImpl<SE_2_3Base<Derived>>
{
template <typename T>
static void run(T& m)
{
using Scalar = typename SE_2_3Base<Derived>::Scalar;
using Translation = typename SE_2_3Base<Derived>::Translation;
using LinearVelocity = typename SE_2_3Base<Derived>::LinearVelocity;
using LieGroup = typename SE_2_3Base<Derived>::LieGroup;
m = LieGroup(Translation::Random(), randQuat<Scalar>(), LinearVelocity::Random());
}
};
//! @brief Assignment assert specialization for SE2Base objects
template <typename Derived>
struct AssignmentEvaluatorImpl<SE_2_3Base<Derived>>
{
template <typename T>
static void run_impl(const T& data)
{
using std::abs;
MANIF_ASSERT(
abs(data.template segment<4>(3).norm()-typename SE_2_3Base<Derived>::Scalar(1)) <
Constants<typename SE_2_3Base<Derived>::Scalar>::eps,
"SE_2_3 assigned data not normalized !",
manif::invalid_argument
);
MANIF_UNUSED_VARIABLE(data);
}
};
//! @brief Cast specialization for SE_2_3Base objects.
template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SE_2_3Base<Derived>, NewScalar> {
template <typename T>
static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
const typename SE_2_3Base<Derived>::QuaternionDataType q = o.quat();
const typename SE_2_3Base<Derived>::Translation t = o.translation();
const typename SE_2_3Base<Derived>::LinearVelocity v = o.linearVelocity();
return typename Derived::template LieGroupTemplate<NewScalar>(
t.template cast<NewScalar>(),
q.template cast<NewScalar>().normalized(),
v.template cast<NewScalar>()
);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE_2_3_BASE_H_ */

89
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se_2_3/SE_2_3_map.h vendored Normal file
View File

@ -0,0 +1,89 @@
#ifndef _MANIF_MANIF_SE_2_3_MAP_H_
#define _MANIF_MANIF_SE_2_3_MAP_H_
#include "manif/impl/se_2_3/SE_2_3.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SE_2_3<_Scalar>,0> >
: public traits<SE_2_3<_Scalar>>
{
using typename traits<SE_2_3<_Scalar>>::Scalar;
using traits<SE_2_3<Scalar>>::RepSize;
using Base = SE_2_3Base<Eigen::Map<SE_2_3<Scalar>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits< Eigen::Map<const SE_2_3<_Scalar>,0> >
: public traits<const SE_2_3<_Scalar>>
{
using typename traits<const SE_2_3<_Scalar>>::Scalar;
using traits<const SE_2_3<Scalar>>::RepSize;
using Base = SE_2_3Base<Eigen::Map<const SE_2_3<Scalar>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SE_2_3
*/
template <class _Scalar>
class Map<manif::SE_2_3<_Scalar>, 0>
: public manif::SE_2_3Base<Map<manif::SE_2_3<_Scalar>, 0> >
{
using Base = manif::SE_2_3Base<Map<manif::SE_2_3<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::rotation;
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_GROUP_MAP_ASSIGN_OP(SE_2_3)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SE_2_3
*/
template <class _Scalar>
class Map<const manif::SE_2_3<_Scalar>, 0>
: public manif::SE_2_3Base<Map<const manif::SE_2_3<_Scalar>, 0> >
{
using Base = manif::SE_2_3Base<Map<const manif::SE_2_3<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::rotation;
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SE_2_3_MAP_H_ */

33
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/se_2_3/SE_2_3_properties.h vendored Normal file
View File

@ -0,0 +1,33 @@
#ifndef _MANIF_MANIF_SE_2_3_PROPERTIES_H_
#define _MANIF_MANIF_SE_2_3_PROPERTIES_H_
#include "manif/impl/traits.h"
namespace manif {
// Forward declaration
template <typename _Derived> struct SE_2_3Base;
template <typename _Derived> struct SE_2_3TangentBase;
namespace internal {
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SE_2_3Base<_Derived>>
{
static constexpr int Dim = 3; /// @brief Space dimension
static constexpr int DoF = 9; /// @brief Degrees of freedom
};
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SE_2_3TangentBase<_Derived>>
{
static constexpr int Dim = 3; /// @brief Space dimension
static constexpr int DoF = 9; /// @brief Degrees of freedom
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SE_2_3_PROPERTIES_H_ */

254
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/sgal3/SGal3.h vendored Normal file
View File

@ -0,0 +1,254 @@
#ifndef _MANIF_MANIF_SGAL3_H_
#define _MANIF_MANIF_SGAL3_H_
#include "manif/impl/sgal3/SGal3_base.h"
namespace manif {
// Forward declare for type traits specialization
template <typename _Scalar> struct SGal3;
template <typename _Scalar> struct SGal3Tangent;
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SGal3<_Scalar>> {
using Scalar = _Scalar;
using LieGroup = SGal3<_Scalar>;
using Tangent = SGal3Tangent<_Scalar>;
using Base = SGal3Base<SGal3<_Scalar>>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = 11;
/// @todo would be nice to concat vec3 + quaternion + vec3 + t
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using Rotation = Eigen::Matrix<Scalar, 3, 3>;
using Translation = Eigen::Matrix<Scalar, 3, 1>;
using LinearVelocity = Eigen::Matrix<Scalar, 3, 1>;
using Vector = Eigen::Matrix<Scalar, 3, 1>;
};
} // namespace internal
} // namespace manif
namespace manif {
//
// LieGroup
//
/**
* @brief Represent an element of SGal3.
*/
template <typename _Scalar>
struct SGal3 : SGal3Base<SGal3<_Scalar>> {
private:
using Base = SGal3Base<SGal3<_Scalar>>;
using Type = SGal3<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_COMPLETE_GROUP_TYPEDEF
using Translation = typename Base::Translation;
using Quaternion = Eigen::Quaternion<Scalar>;
using LinearVelocity = typename Base::LinearVelocity;
MANIF_INHERIT_GROUP_API
using Base::rotation;
using Base::normalize;
SGal3() = default;
~SGal3() = default;
MANIF_COPY_CONSTRUCTOR(SGal3)
MANIF_MOVE_CONSTRUCTOR(SGal3)
template <typename _DerivedOther>
SGal3(const LieGroupBase<_DerivedOther>& o);
MANIF_GROUP_ASSIGN_OP(SGal3)
/**
* @brief Constructor given a translation, a unit quaternion and a linear velocity.
* @param[in] t A translation vector.
* @param[in] q A unit quaternion.
* @param[in] v A linear velocity vector.
* @param[in] time A time.
* @throws manif::invalid_argument on un-normalized complex number.
*/
SGal3(
const Translation& t,
const Eigen::Quaternion<Scalar>& q,
const LinearVelocity& v,
const Scalar time
);
/**
* @brief Constructor given a translation, an angle axis and a linear velocity.
* @param[in] t A translation vector.
* @param[in] angle_axis An angle-axis.
* @param[in] v A linear velocity vector.
* @param[in] time A time.
*/
SGal3(
const Translation& t,
const Eigen::AngleAxis<Scalar>& angle_axis,
const LinearVelocity& v,
const Scalar time
);
/**
* @brief Constructor given a translation, SO3 element and a linear velocity.
* @param[in] t A translation vector.
* @param[in] SO3 An element of SO3.
* @param[in] v A linear velocity vector.
* @param[in] time A time.
*/
SGal3(
const Translation& t,
const SO3<Scalar>& SO3,
const LinearVelocity& v,
const Scalar time
);
/**
* @brief Constructor given translation components,
* roll-pitch-yaw angles and linear velocity components
* @param[in] x The x component of the translation.
* @param[in] y The y component of the translation.
* @param[in] z The z component of the translation.
* @param[in] roll The roll angle.
* @param[in] pitch The pitch angle.
* @param[in] yaw The yaw angle.
* @param[in] vx The x component of the linear velocity.
* @param[in] vy The y component of the linear velocity.
* @param[in] vz The z component of the linear velocity.
* @param[in] t time.
*/
SGal3(
const Scalar x, const Scalar y, const Scalar z,
const Scalar roll, const Scalar pitch, const Scalar yaw,
const Scalar vx, const Scalar vy, const Scalar vz,
const Scalar t
);
/**
* @brief Constructor from a 3D Eigen::Isometry<Scalar> relevant to SE(3) and a linear velocity
* @param[in] h a isometry object from Eigen defined for SE(3)
* @param[in] v a linear velocity vector.
* @note overall, this should be a double direct spatial isometry,
*/
SGal3(
const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h,
const LinearVelocity& v,
const Scalar t
);
// LieGroup common API
DataType& coeffs();
const DataType& coeffs() const;
// SGal3 specific API
protected:
DataType data_;
};
MANIF_EXTRA_GROUP_TYPEDEF(SGal3)
template <typename _Scalar>
template <typename _DerivedOther>
SGal3<_Scalar>::SGal3(const LieGroupBase<_DerivedOther>& o) : SGal3(o.coeffs()) {
//
}
template <typename _Scalar>
SGal3<_Scalar>::SGal3(
const Translation& t,
const Eigen::Quaternion<Scalar>& q,
const LinearVelocity& v,
const Scalar time
) : SGal3((DataType() << t, q.coeffs(), v, time).finished()) {
//
}
template <typename _Scalar>
SGal3<_Scalar>::SGal3(
const Translation& t,
const Eigen::AngleAxis<Scalar>& a,
const LinearVelocity& v,
const Scalar time
) : SGal3(t, Quaternion(a), v, time) {
//
}
template <typename _Scalar>
SGal3<_Scalar>::SGal3(
const Scalar x, const Scalar y, const Scalar z,
const Scalar roll, const Scalar pitch, const Scalar yaw,
const Scalar vx, const Scalar vy, const Scalar vz,
const Scalar t
) : SGal3(
Translation(x,y,z),
Eigen::Quaternion<Scalar>(
Eigen::AngleAxis<Scalar>(yaw, Eigen::Matrix<Scalar, 3, 1>::UnitZ()) *
Eigen::AngleAxis<Scalar>(pitch, Eigen::Matrix<Scalar, 3, 1>::UnitY()) *
Eigen::AngleAxis<Scalar>(roll, Eigen::Matrix<Scalar, 3, 1>::UnitX())
),
LinearVelocity(vx, vy, vz),
t
) {
//
}
template <typename _Scalar>
SGal3<_Scalar>::SGal3(
const Translation& t,
const SO3<Scalar>& so3,
const LinearVelocity& v,
const Scalar time
) : SGal3(t, so3.quat(), v, time) {
//
}
template <typename _Scalar>
SGal3<_Scalar>::SGal3(
const Eigen::Transform<_Scalar, 3, Eigen::Isometry>& h,
const LinearVelocity& v,
const Scalar t
) : SGal3(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation()), v, t) {
//
}
template <typename _Scalar>
typename SGal3<_Scalar>::DataType&
SGal3<_Scalar>::coeffs() {
return data_;
}
template <typename _Scalar>
const typename SGal3<_Scalar>::DataType&
SGal3<_Scalar>::coeffs() const {
return data_;
}
} // namespace manif
#endif // _MANIF_MANIF_SGAL3_H_

106
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/sgal3/SGal3Tangent.h vendored Normal file
View File

@ -0,0 +1,106 @@
#ifndef _MANIF_MANIF_SGAL3TANGENT_H_
#define _MANIF_MANIF_SGAL3TANGENT_H_
#include "manif/impl/sgal3/SGal3Tangent_base.h"
namespace manif {
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SGal3Tangent<_Scalar>> {
using Scalar = _Scalar;
using LieGroup = SGal3<_Scalar>;
using Tangent = SGal3Tangent<_Scalar>;
using Base = SGal3TangentBase<Tangent>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = DoF;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using LieAlg = Eigen::Matrix<Scalar, 5, 5>;
};
} // namespace internal
} // namespace manif
namespace manif {
//
// Tangent
//
/**
* @brief Represents an element of tangent space of SGal3.
*/
template <typename _Scalar>
struct SGal3Tangent : SGal3TangentBase<SGal3Tangent<_Scalar>> {
private:
using Base = SGal3TangentBase<SGal3Tangent<_Scalar>>;
using Type = SGal3Tangent<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
SGal3Tangent() = default;
~SGal3Tangent() = default;
MANIF_COPY_CONSTRUCTOR(SGal3Tangent)
MANIF_MOVE_CONSTRUCTOR(SGal3Tangent)
template <typename _DerivedOther>
SGal3Tangent(const TangentBase<_DerivedOther>& o);
MANIF_TANGENT_ASSIGN_OP(SGal3Tangent)
// Tangent common API
DataType& coeffs();
const DataType& coeffs() const;
// SGal3Tangent specific API
protected:
DataType data_;
};
MANIF_EXTRA_TANGENT_TYPEDEF(SGal3Tangent);
template <typename _Scalar>
template <typename _DerivedOther>
SGal3Tangent<_Scalar>::SGal3Tangent(const TangentBase<_DerivedOther>& o)
: data_(o.coeffs()) {
//
}
template <typename _Scalar>
typename SGal3Tangent<_Scalar>::DataType&
SGal3Tangent<_Scalar>::coeffs() {
return data_;
}
template <typename _Scalar>
const typename SGal3Tangent<_Scalar>::DataType&
SGal3Tangent<_Scalar>::coeffs() const {
return data_;
}
} // namespace manif
#endif // _MANIF_MANIF_SGAL3TANGENT_H_

635
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/sgal3/SGal3Tangent_base.h vendored Normal file
View File

@ -0,0 +1,635 @@
#ifndef _MANIF_MANIF_SGAL3TANGENT_BASE_H_
#define _MANIF_MANIF_SGAL3TANGENT_BASE_H_
#include "manif/impl/sgal3/SGal3_properties.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/so3/SO3Tangent_map.h"
#include "manif/impl/se3/SE3Tangent.h"
namespace manif {
//
// Tangent
//
/**
* @brief The base class of the SGal3 tangent.
*/
template <typename _Derived>
struct SGal3TangentBase : TangentBase<_Derived> {
private:
using Base = TangentBase<_Derived>;
using Type = SGal3TangentBase<_Derived>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
using LinBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
using AngBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
using ConstLinBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;
using ConstAngBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;
using Base::data;
using Base::coeffs;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SGal3TangentBase)
public:
MANIF_TANGENT_ML_ASSIGN_OP(SGal3TangentBase)
// Tangent common API
/**
* @brief Hat operator of SGal3.
* @return An element of the Lie algebra se_2_3.
* @note See Eq. (169).
*/
LieAlg hat() const;
/**
* @brief Get the SGal3 element.
* @param[out] -optional- J_m_t Jacobian of the SGal3 element wrt this.
* @return The SGal3 element.
* @note This is the exp() map with the argument in vector form.
*/
LieGroup exp(OptJacobianRef J_m_t = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t = {}) const;
/**
* @brief Get the right Jacobian of SGal3.
*/
Jacobian rjac() const;
/**
* @brief Get the left Jacobian of SGal3.
*/
Jacobian ljac() const;
/**
* @brief Get the small adjoint matrix ad() of SGal3
* that maps isomorphic tangent vectors of SGal3
* @return
*/
Jacobian smallAdj() const;
// SGal3Tangent specific API
//! @brief Get the linear translation part.
LinBlock lin();
const ConstLinBlock lin() const;
//! @brief Get the angular part.
AngBlock ang();
const ConstAngBlock ang() const;
//! @brief Get the linear velocity part
LinBlock lin2();
const ConstLinBlock lin2() const;
Scalar t() const;
public: /// @todo make protected
const Eigen::Map<const SO3Tangent<Scalar>> asSO3() const {
return Eigen::Map<const SO3Tangent<Scalar>>(coeffs().data()+6);
}
Eigen::Map<SO3Tangent<Scalar>> asSO3() {
return Eigen::Map<SO3Tangent<Scalar>>(coeffs().data()+6);
}
static void fillE(
Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>> E,
const Eigen::Map<const SO3Tangent<Scalar>>& so3
);
};
template <typename _Derived>
typename SGal3TangentBase<_Derived>::LieGroup
SGal3TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const {
if (J_m_t) {
*J_m_t = rjac();
}
const Eigen::Map<const SO3Tangent<Scalar>> so3 = asSO3();
const typename SO3<Scalar>::Jacobian so3_ljac = so3.ljac();
Eigen::Matrix<Scalar, 3, 3> E;
fillE(E, so3);
return LieGroup(
so3_ljac * lin() + (E * (t() * lin2())),
so3.exp(),
so3_ljac * lin2(),
t()
);
}
template <typename _Derived>
typename SGal3TangentBase<_Derived>::LieGroup
SGal3TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const {
return exp(J_m_t);
}
template <typename _Derived>
typename SGal3TangentBase<_Derived>::LieAlg
SGal3TangentBase<_Derived>::hat() const {
LieAlg sgal3;
sgal3.template topLeftCorner<3, 3>() = skew(ang());
sgal3.template block<3, 1>(0, 3) = lin2();
sgal3.template topRightCorner<3, 1>() = lin();
sgal3(3, 4) = t();
sgal3.template bottomLeftCorner<2, 4>().setZero();
sgal3(4, 4) = Scalar(0);
return sgal3;
}
template <typename _Derived>
typename SGal3TangentBase<_Derived>::Jacobian
SGal3TangentBase<_Derived>::rjac() const {
return (-*this).ljac();
// Those were verified against auto diff
// Jacobian Jr = Jacobian::Zero();
// Jr.template topLeftCorner<3, 3>() = asSO3().rjac();
// // Jr.template block<3, 3>(0, 3) = ??;
// // Jr.template block<3, 3>(0, 6) = ??;
// // Jr.template block<3, 1>(0, 9) = ??;
// Jr.template block<3, 3>(3, 3) = Jr.template topLeftCorner<3,3>();
// SE3Tangent<Scalar>::fillQ(
// Jr.template block<3, 3>(3, 6), -coeffs().template segment<6>(3)
// );
// Jr.template block<3, 3>(6, 6) = Jr.template topLeftCorner<3,3>();
// Jr(9, 9) = Scalar(1);
// Jr.template bottomLeftCorner<7, 3>().setZero();
// Jr.template block<4, 3>(6, 3).setZero();
// Jr.template block<1, 3>(9, 6).setZero();
// Jr.template block<6, 1>(3, 9).setZero();
// return Jr;
}
template <typename _Derived>
typename SGal3TangentBase<_Derived>::Jacobian
SGal3TangentBase<_Derived>::ljac() const {
using ConstRef33 = const Eigen::Ref<const Eigen::Matrix<Scalar, 3, 3>>;
using ConstRef61 = const Eigen::Ref<const Eigen::Matrix<Scalar, 6, 1>>;
using Diag = typename Eigen::DiagonalMatrix<Scalar, 3>;
auto I33 = [](const Scalar d){ return Diag(d, d, d).toDenseMatrix(); };
using std::sqrt;
using std::cos;
using std::sin;
/** Structure of the left Jacobian according to J. Kelly
*
* Jl = [ D -L*t N E*nu
* 0 D M 0
* 0 0 D 0
* 0 0 0 1 ]
*
* with N = N1 - N2.
*
* Tangent space is tau = [rho ; nu ; theta ; t] in R^10
*
* Matrix blocks D, E, L, M, N, N1, N2 are referred to in the comments and
* correspond to eqs. in Kelly's paper:
*
* D: (18) = Jl_SO3(theta)
* E: (19)
* L: (32)
* M: (33) = Q(nu,theta)
* N: (34) = N1 - N2
* N1: (35) = Q(rho,theta)
* N2: (36)
*
* Note that we use the following temporary blocks for computation
*
* Jl = [ . . E .
* V . . rho
* W W^2 . theta
* . . . . ]
*
*/
Jacobian Jl;
// theta vector
const Eigen::Map<const SO3Tangent<Scalar>> so3 = asSO3();
// Skew matrix W = theta^
Jl.template block<3, 3>(3, 0) = so3.hat();
ConstRef33 W = Jl.template block<3, 3>(3, 0);
// Skew matrix W^2
Jl.template block<3, 3>(6, 3) = W * W;
ConstRef33 WW = Jl.template block<3, 3>(6, 3);
// Skew matrix V = nu^
Jl.template block<3, 3>(6, 0) = skew(lin2());
ConstRef33 V = Jl.template block<3, 3>(6, 0);
// Angles and trigonometry
const Scalar theta_sq = so3.coeffs().squaredNorm();
// rotation angle
const Scalar theta = sqrt(theta_sq);
const Scalar theta_cu = theta * theta_sq;
const Scalar sin_t = sin(theta);
const Scalar cos_t = cos(theta);
// Blocks D
Jl.template topLeftCorner<3, 3>() = so3.ljac();
Jl.template block<3, 3>(3, 3) = Jl.template topLeftCorner<3, 3>();
Jl.template block<3, 3>(6, 6) = Jl.template topLeftCorner<3, 3>();
// Block E
// Note - we use here a temporary block to hold E
Jl.template block<3, 3>(0, 6) = I33(Scalar(0.5));
if (theta_sq > Constants<Scalar>::eps) {
const Scalar A = (theta - sin_t) / theta_sq / theta;
const Scalar B = (
theta_sq + Scalar(2) * cos_t - Scalar(2)
) / (Scalar(2) * theta_sq * theta_sq);
Jl.template block<3, 3>(0, 6).noalias() += A * W + B * WW;
}
// Block E * nu
Jl.template block<3, 1>(0, 9) = Jl.template block<3, 3>(0, 6) * lin2();
// Block L
Scalar cA, cB;
// small angle approx.
if (theta_cu > Constants<Scalar>::eps) {
cA = (sin_t - theta * cos_t) / theta_cu;
cB = (
theta_sq + Scalar(2) * (Scalar(1) - theta * sin_t - cos_t)
) / (Scalar(2) * theta_sq * theta_sq);
} else {
cA = Scalar(1./3.) - Scalar(1./30.) * theta_sq;
cB = Scalar(1./8.);
}
// Block - L * t
Jl.template block<3, 3>(0, 3).noalias() = -t() * (
// Block L
I33(Scalar(0.5)) + cA * W + cB * WW
);
// Block M = Q(nu, theta)
SE3Tangent<Scalar>::fillQ(
Jl.template block<3, 3>(3, 6), coeffs().template segment<6>(3)
);
// Block N1, part of N. N1 = Q(rho, theta)
Jl.template block<6, 1>(3, 9) << lin(), ang();
ConstRef61 rho_theta = Jl.template block<6, 1>(3, 9);
// block N1 = Q(rho,theta)
SE3Tangent<Scalar>::fillQ(Jl.template block<3, 3>(0, 6), rho_theta);
// Block N2, part of N
Scalar cC, cD, cE, cF;
if (theta_cu > Constants<Scalar>::eps) {
cA = (Scalar(2) - theta * sin_t - Scalar(2) * cos_t) / theta_cu / theta;
cB = (
theta_cu + Scalar(6) * theta + Scalar(6) * theta * cos_t - Scalar(12) * sin_t
) / (Scalar(6) * theta_cu * theta_sq);
cC = (
Scalar(12) * sin_t - theta_cu - Scalar(3) * theta_sq * sin_t - Scalar(12) * theta * cos_t
) / (Scalar(6) * theta_cu * theta_sq);
cD = (
Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t + cos_t)
) / (Scalar(2) * theta_cu * theta_cu);
cE = (theta_sq + Scalar(2) * (cos_t - Scalar(1))) / (Scalar(2) * theta_cu * theta);
cF = (theta_cu + Scalar(6) * (sin_t - theta)) / (Scalar(6) * theta_cu * theta_sq);
} else {
cA = Scalar(1. / 12.);
cB = Scalar(1. / 24.);
cC = Scalar(1. / 10.);
cD = Scalar(1. / 240.);
cE = Scalar(1. / 24.);
cF = Scalar(1. / 120.);
}
// Block N = N1 - N2
Jl.template block<3, 3>(0, 6) -= (
// Block N2
t() / Scalar(6) * V
+ (cA * W + cB * WW) * (t() * V)
+ cC * (W * V * (t() * W))
+ cD * (WW * V * (t() * W))
+ t() * V * (cE * W + cF * WW)
);
// Block 1
Jl(9, 9) = Scalar(1);
// Blocks of zeros
Jl.template bottomLeftCorner<7, 3>().setZero();
Jl.template block<4, 3>(6, 3).setZero();
Jl.template block<1, 3>(9, 6).setZero();
Jl.template block<6, 1>(3, 9).setZero();
return Jl;
}
template <typename _Derived>
typename SGal3TangentBase<_Derived>::Jacobian
SGal3TangentBase<_Derived>::smallAdj() const {
Jacobian smallAdj;
smallAdj.template topLeftCorner<3,3>() = skew(ang());
smallAdj.template block<3, 3>(0, 3) = -t() * Eigen::Matrix3d::Identity();
smallAdj.template block<3, 3>(0, 6) = skew(lin());
smallAdj.template block<3, 1>(0, 9) = lin2();
smallAdj.template block<3, 3>(3, 3) = smallAdj.template topLeftCorner<3,3>();
smallAdj.template block<3, 3>(3, 6) = skew(lin2());
smallAdj.template block<3, 3>(6, 6) = smallAdj.template topLeftCorner<3,3>();
smallAdj.template block<7, 3>(3, 0).setZero();
smallAdj.template block<4, 3>(6, 3).setZero();
smallAdj.template block<1, 3>(9, 6).setZero();
smallAdj.template block<7, 1>(3, 9).setZero();
return smallAdj;
}
// SGal3Tangent specific API
template <typename _Derived>
typename SGal3TangentBase<_Derived>::LinBlock
SGal3TangentBase<_Derived>::lin() {
return coeffs().template head<3>();
}
template <typename _Derived>
const typename SGal3TangentBase<_Derived>::ConstLinBlock
SGal3TangentBase<_Derived>::lin() const {
return coeffs().template head<3>();
}
template <typename _Derived>
typename SGal3TangentBase<_Derived>::LinBlock
SGal3TangentBase<_Derived>::lin2() {
return coeffs().template segment<3>(3);
}
template <typename _Derived>
const typename SGal3TangentBase<_Derived>::ConstLinBlock
SGal3TangentBase<_Derived>::lin2() const {
return coeffs().template segment<3>(3);
}
template <typename _Derived>
typename SGal3TangentBase<_Derived>::AngBlock
SGal3TangentBase<_Derived>::ang() {
return coeffs().template segment<3>(6);
}
template <typename _Derived>
const typename SGal3TangentBase<_Derived>::ConstAngBlock
SGal3TangentBase<_Derived>::ang() const {
return coeffs().template segment<3>(6);
}
template <typename _Derived>
typename SGal3TangentBase<_Derived>::Scalar
SGal3TangentBase<_Derived>::t() const {
return coeffs()(9);
}
template <typename _Derived>
void SGal3TangentBase<_Derived>::fillE(
Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>> E,
const Eigen::Map<const SO3Tangent<Scalar>>& so3
) {
using I = typename Eigen::DiagonalMatrix<Scalar, 3>;
const Scalar theta_sq = so3.coeffs().squaredNorm();
E.noalias() = I(Scalar(0.5), Scalar(0.5), Scalar(0.5)).toDenseMatrix();
// small angle approx.
if (theta_sq < Constants<Scalar>::eps) {
return;
}
const Scalar theta = sqrt(theta_sq); // rotation angle
const Scalar A = (theta - sin(theta)) / theta_sq / theta;
const Scalar B = (theta_sq + Scalar(2) * cos(theta) - Scalar(2)) / (Scalar(2) * theta_sq * theta_sq);
const typename SO3Tangent<Scalar>::LieAlg W = so3.hat();
E.noalias() += A * W + B * W * W;
}
namespace internal {
//! @brief Generator specialization for SGal3TangentBase objects.
template <typename Derived>
struct GeneratorEvaluator<SGal3TangentBase<Derived>> {
static typename SGal3TangentBase<Derived>::LieAlg
run(const unsigned int i) {
using LieAlg = typename SGal3TangentBase<Derived>::LieAlg;
using Scalar = typename SGal3TangentBase<Derived>::Scalar;
switch (i) {
case 0: {
static const LieAlg E0(
(
LieAlg() <<
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E0;
}
case 1: {
static const LieAlg E1(
(
LieAlg() <<
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E1;
}
case 2: {
static const LieAlg E2(
(
LieAlg() <<
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E2;
}
case 3: {
static const LieAlg E3(
(
LieAlg() <<
Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E3;
}
case 4: {
static const LieAlg E4(
(
LieAlg() <<
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E4;
}
case 5: {
static const LieAlg E5(
(
LieAlg() <<
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(1), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E5;
}
case 6: {
static const LieAlg E6(
(
LieAlg() <<
Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(-1), Scalar(0), Scalar(0),
Scalar(0), Scalar(1), Scalar( 0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar( 0), Scalar(0), Scalar(0)
).finished()
);
return E6;
}
case 7: {
static const LieAlg E7(
(
LieAlg() <<
Scalar( 0), Scalar(0), Scalar(1), Scalar(0), Scalar(0),
Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar( 0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E7;
}
case 8: {
static const LieAlg E8(
(
LieAlg() <<
Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
Scalar(1), Scalar( 0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E8;
}
case 9: {
static const LieAlg E9(
(
LieAlg() <<
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(1),
Scalar(0), Scalar(0), Scalar(0), Scalar(0), Scalar(0)
).finished()
);
return E9;
}
default:
MANIF_THROW("Index i must be in [0,9]!", invalid_argument);
break;
}
return LieAlg{};
}
};
//! @brief Random specialization for SGal3TangentBase objects.
template <typename Derived>
struct RandomEvaluatorImpl<SGal3TangentBase<Derived>> {
static void run(SGal3TangentBase<Derived>& m) {
// in [-1,1]
m.coeffs().setRandom();
// In ball of radius PI
m.coeffs().template segment<3>(6) = randPointInBall(MANIF_PI).template cast<typename Derived::Scalar>();
}
};
//! @brief Vee specialization for SGal3TangentBase objects.
template <typename Derived>
struct VeeEvaluatorImpl<SGal3TangentBase<Derived>> {
template <typename TL, typename TR>
static void run(TL& t, const TR& v) {
t.coeffs() << v(0, 4), v(1, 4), v(2, 4),
v(0, 3), v(1, 3), v(2, 3),
v(2, 1), v(0, 2), v(1, 0),
v(3, 4);
}
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_SGAL3TANGENT_BASE_H_

85
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/sgal3/SGal3Tangent_map.h vendored Normal file
View File

@ -0,0 +1,85 @@
#ifndef _MANIF_MANIF_SGAL3TANGENT_MAP_H_
#define _MANIF_MANIF_SGAL3TANGENT_MAP_H_
#include "manif/impl/sgal3/SGal3Tangent.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits<Eigen::Map<SGal3Tangent<_Scalar>, 0> >
: public traits<SGal3Tangent<_Scalar>> {
using typename traits<SGal3Tangent<_Scalar>>::Scalar;
using traits<SGal3Tangent<_Scalar>>::DoF;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SGal3TangentBase<Eigen::Map<SGal3Tangent<Scalar>, 0>>;
};
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits<Eigen::Map<const SGal3Tangent<_Scalar>, 0> >
: public traits<const SGal3Tangent<_Scalar>> {
using typename traits<const SGal3Tangent<_Scalar>>::Scalar;
using traits<const SGal3Tangent<_Scalar>>::DoF;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SGal3TangentBase<Eigen::Map<const SGal3Tangent<Scalar>, 0>>;
};
} // namespace internal
} // namespace manif
namespace Eigen {
/**
* @brief Specialization of Map for manif::SGal3
*/
template <class _Scalar>
class Map<manif::SGal3Tangent<_Scalar>, 0>
: public manif::SGal3TangentBase<Map<manif::SGal3Tangent<_Scalar>, 0> > {
using Base = manif::SGal3TangentBase<Map<manif::SGal3Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_TANGENT_MAP_ASSIGN_OP(SGal3Tangent)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SGal3
*/
template <class _Scalar>
class Map<const manif::SGal3Tangent<_Scalar>, 0>
: public manif::SGal3TangentBase<Map<const manif::SGal3Tangent<_Scalar>, 0> > {
using Base = manif::SGal3TangentBase<Map<const manif::SGal3Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} // namespace Eigen
#endif // _MANIF_MANIF_SGAL3TANGENT_MAP_H_

484
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/sgal3/SGal3_base.h vendored Normal file
View File

@ -0,0 +1,484 @@
#ifndef _MANIF_MANIF_SGAL3_BASE_H_
#define _MANIF_MANIF_SGAL3_BASE_H_
#include "manif/impl/sgal3/SGal3_properties.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/so3/SO3_map.h"
#include "manif/impl/se3/SE3_map.h"
namespace manif {
//
// LieGroup
//
/**
* @brief The base class of the SGal3 group.
* @note See "All About the Galilean Group SGal(3)" J. Kelly.
* https://arxiv.org/abs/2312.07555
*
*/
template <typename _Derived>
struct SGal3Base : LieGroupBase<_Derived> {
private:
using Base = LieGroupBase<_Derived>;
using Type = SGal3Base<_Derived>;
public:
MANIF_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_AUTO_API
MANIF_INHERIT_GROUP_OPERATOR
using Base::coeffs;
using Rotation = typename internal::traits<_Derived>::Rotation;
using Translation = typename internal::traits<_Derived>::Translation;
using LinearVelocity = typename internal::traits<_Derived>::LinearVelocity;
using Time = Scalar;
using Transformation = Eigen::Matrix<Scalar, 5, 5>;
using Isometry = Eigen::Matrix<Scalar, 5, 5>; /**< Double direct spatial isometry*/
using QuaternionDataType = Eigen::Quaternion<Scalar>;
// LieGroup common API
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SGal3Base)
public:
MANIF_GROUP_ML_ASSIGN_OP(SGal3Base)
/**
* @brief Get the inverse.
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this.
*/
LieGroup inverse(OptJacobianRef J_minv_m = {}) const;
/**
* @brief Get the SGal3 corresponding Lie algebra element in vector form.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt to this.
* @return The SGal3 tangent of this.
* @note This is the log() map in vector form.
* @see SGal3Tangent.
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another SGal3 element.
* @param[in] m Another SGal3 element.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
*/
template <typename _DerivedOther>
LieGroup compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}
) const;
/**
* @brief Get the action
* @param[in] p A 3D point.
* @param[out] -optional- J_pout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_pout_p The Jacobian of the new object wrt input object.
*/
template <typename _EigenDerived>
Eigen::Matrix<Scalar, 3, 1>
act(
const Eigen::MatrixBase<_EigenDerived> &p,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_pout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_pout_p = {}
) const;
/**
* @brief Get the adjoint matrix of SGal3 at this.
*/
Jacobian adj() const;
// SGal3 specific functions
/**
* Get the isometry object (double direct isometry).
* @note T = | R v t|
* | 1 s|
* | 1|
*/
Transformation transform() const;
/**
* Get the isometry object (double direct isometry).
* @note T = | R v t|
* | 1 s|
* | 1|
*/
Isometry isometry() const;
/**
* @brief Get the rotational part of this as a rotation matrix.
*/
Rotation rotation() const;
/**
* @brief Get the rotational part of this as a quaternion.
*/
QuaternionDataType quat() const;
/**
* @brief Get the translational part in vector form.
*/
Translation translation() const;
/**
* @brief Get the x component of the translational part.
*/
Scalar x() const;
/**
* @brief Get the y component of translational part.
*/
Scalar y() const;
/**
* @brief Get the z component of translational part.
*/
Scalar z() const;
/**
* @brief Get the linear velocity part in vector form.
*/
LinearVelocity linearVelocity() const;
/**
* @brief Get the x component of the linear velocity part.
*/
Scalar vx() const;
/**
* @brief Get the y component of linear velocity part.
*/
Scalar vy() const;
/**
* @brief Get the z component of linear velocity part.
*/
Scalar vz() const;
/**
* @brief Get the time.
*/
Scalar t() const;
/**
* @brief Normalize the underlying quaternion.
*/
void normalize();
public: /// @todo make protected
Eigen::Map<const SO3<Scalar>> asSO3() const {
return Eigen::Map<const SO3<Scalar>>(coeffs().data()+3);
}
Eigen::Map<SO3<Scalar>> asSO3() {
return Eigen::Map<SO3<Scalar>>(coeffs().data()+3);
}
};
template <typename _Derived>
typename SGal3Base<_Derived>::Transformation
SGal3Base<_Derived>::transform() const {
Eigen::Matrix<Scalar, 5, 5> T;
T.template topLeftCorner<3, 3>() = rotation();
T.template block<3, 1>(0, 3) = linearVelocity();
T.template topRightCorner<3, 1>() = translation();
T.template bottomLeftCorner<2, 3>().setZero();
T.template bottomRightCorner<2, 2>().setIdentity();
T(3, 4) = t();
return T;
}
template <typename _Derived>
typename SGal3Base<_Derived>::Isometry
SGal3Base<_Derived>::isometry() const {
return Isometry(transform());
}
template <typename _Derived>
typename SGal3Base<_Derived>::Rotation
SGal3Base<_Derived>::rotation() const {
return asSO3().rotation();
}
template <typename _Derived>
typename SGal3Base<_Derived>::QuaternionDataType
SGal3Base<_Derived>::quat() const {
return asSO3().quat();
}
template <typename _Derived>
typename SGal3Base<_Derived>::Translation
SGal3Base<_Derived>::translation() const {
return coeffs().template head<3>();
}
template <typename _Derived>
typename SGal3Base<_Derived>::LinearVelocity
SGal3Base<_Derived>::linearVelocity() const {
return coeffs().template segment<3>(7);
}
template <typename _Derived>
typename SGal3Base<_Derived>::LieGroup
SGal3Base<_Derived>::inverse(OptJacobianRef J_minv_m) const {
if (J_minv_m) {
(*J_minv_m) = -adj();
}
const SO3<Scalar> so3inv = asSO3().inverse();
return LieGroup(
-so3inv.act((translation()-t()*linearVelocity())),
so3inv,
-so3inv.act(linearVelocity()),
-t()
);
}
template <typename _Derived>
typename SGal3Base<_Derived>::Tangent
SGal3Base<_Derived>::log(OptJacobianRef J_t_m) const {
const SO3Tangent<Scalar> so3tan = asSO3().log();
Eigen::Matrix<Scalar, 3, 3> E;
Tangent::fillE(E, Eigen::Map<const SO3Tangent<Scalar>>(so3tan.data()));
const LinearVelocity nu = so3tan.ljacinv() * linearVelocity();
Tangent tan(
(
typename Tangent::DataType() <<
so3tan.ljacinv() * (translation() - E * (t() * nu)), nu, so3tan.coeffs(), t()
).finished()
);
if (J_t_m) {
// Jr^-1
(*J_t_m) = tan.rjacinv();
}
return tan;
}
template <typename _Derived>
typename SGal3Base<_Derived>::Tangent
SGal3Base<_Derived>::lift(OptJacobianRef J_t_m) const {
return log(J_t_m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename SGal3Base<_Derived>::LieGroup
SGal3Base<_Derived>::compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb
) const {
static_assert(
std::is_base_of<SGal3Base<_DerivedOther>, _DerivedOther>::value,
"Argument does not inherit from SGal3Base !"
);
const auto& m_sgal3 = static_cast<const SGal3Base<_DerivedOther>&>(m);
if (J_mc_ma) {
(*J_mc_ma) = m.inverse().adj();
}
if (J_mc_mb) {
J_mc_mb->setIdentity();
}
return LieGroup(
rotation() * m_sgal3.translation() + m_sgal3.t() * linearVelocity() + translation(),
asSO3() * m_sgal3.asSO3(),
rotation() * m_sgal3.linearVelocity() + linearVelocity(),
t() + m_sgal3.t()
);
}
template <typename _Derived>
template <typename _EigenDerived>
Eigen::Matrix<typename SGal3Base<_Derived>::Scalar, 3, 1>
SGal3Base<_Derived>::act(
const Eigen::MatrixBase<_EigenDerived> &p,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_pout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_pout_p
) const {
assert_vector_dim(p, 3);
const Rotation R(rotation());
if (J_pout_m) {
J_pout_m->template topLeftCorner<3, 3>() = R;
J_pout_m->template block<3, 3>(0, 3).setZero();
J_pout_m->template block<3, 3>(0, 6).noalias() = -R * skew(p);
J_pout_m->template topRightCorner<3, 1>() = linearVelocity();
}
if (J_pout_p) {
(*J_pout_p) = R;
}
return translation() + R * p;
}
template <typename _Derived>
typename SGal3Base<_Derived>::Jacobian
SGal3Base<_Derived>::adj() const {
///
/// this is
/// Ad(g) = | R -R.tau [(t-v.tau)]x.R v|
/// | 0 R [v]x.R 0|
/// | 0 0 R 0|
/// | 0 0 0 1|
///
/// considering vee(log(g)) = (rho;v;w;iota)
Jacobian Adj;
Adj.template topLeftCorner<3, 3>() = rotation();
Adj.template block<3, 3>(0, 3).noalias() = -t() * Adj.template topLeftCorner<3, 3>();
Adj.template block<3, 3>(0, 6).noalias() =
skew(translation() - t() * linearVelocity()) * Adj.template topLeftCorner<3, 3>();
Adj.template topRightCorner<3, 1>() = linearVelocity();
Adj.template block<3, 3>(3, 3) = Adj.template topLeftCorner<3, 3>();
Adj.template block<3, 3>(3, 6).noalias() =
skew(linearVelocity()) * Adj.template topLeftCorner<3, 3>();
Adj.template block<3, 3>(6, 6) = Adj.template topLeftCorner<3, 3>();
Adj.template bottomLeftCorner<7, 3>().setZero();
Adj.template block<4, 3>(6, 3).setZero();
Adj.template block<1, 3>(9, 6).setZero();
Adj.template block<6, 1>(3, 9).setZero();
Adj(9, 9) = Scalar(1);
return Adj;
}
// SGal3 specific function
template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::x() const {
return coeffs()(0);
}
template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::y() const {
return coeffs()(1);
}
template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::z() const {
return coeffs()(2);
}
template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::vx() const {
return coeffs()(7);
}
template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::vy() const {
return coeffs()(8);
}
template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::vz() const {
return coeffs()(9);
}
template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::t() const {
return coeffs()(10);
}
template <typename _Derived>
void SGal3Base<_Derived>::normalize() {
coeffs().template segment<4>(3).normalize();
}
namespace internal {
//! @brief Random specialization for SGal3Base objects.
template <typename Derived>
struct RandomEvaluatorImpl<SGal3Base<Derived>> {
template <typename T>
static void run(T& m) {
using Scalar = typename SGal3Base<Derived>::Scalar;
using LieGroup = typename SGal3Base<Derived>::LieGroup;
typename LieGroup::DataType data = LieGroup::DataType::Random();
data.template segment<4>(3) = randQuat<Scalar>().coeffs();
m = LieGroup(data);
}
};
//! @brief Assignment assert specialization for SGal3Base objects
template <typename Derived>
struct AssignmentEvaluatorImpl<SGal3Base<Derived>> {
template <typename T>
static void run_impl(const T& data) {
using std::abs;
MANIF_ASSERT(
abs(data.template segment<4>(3).norm()-typename SGal3Base<Derived>::Scalar(1)) <
Constants<typename SGal3Base<Derived>::Scalar>::eps,
"SGal3 assigned data not normalized !",
manif::invalid_argument
);
MANIF_UNUSED_VARIABLE(data);
}
};
//! @brief Cast specialization for SGal3Base objects.
template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SGal3Base<Derived>, NewScalar> {
template <typename T>
static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
return typename Derived::template LieGroupTemplate<NewScalar>(
o.translation().template cast<NewScalar>(),
o.quat().template cast<NewScalar>().normalized(),
o.linearVelocity().template cast<NewScalar>(),
NewScalar(o.t())
);
}
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_SGAL3_BASE_H_

84
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/sgal3/SGal3_map.h vendored Normal file
View File

@ -0,0 +1,84 @@
#ifndef _MANIF_MANIF_SGAL3_MAP_H_
#define _MANIF_MANIF_SGAL3_MAP_H_
#include "manif/impl/sgal3/SGal3.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits<Eigen::Map<SGal3<_Scalar>, 0> > : public traits<SGal3<_Scalar>> {
using typename traits<SGal3<_Scalar>>::Scalar;
using traits<SGal3<Scalar>>::RepSize;
using Base = SGal3Base<Eigen::Map<SGal3<Scalar>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits<Eigen::Map<const SGal3<_Scalar>,0> >
: public traits<const SGal3<_Scalar>> {
using typename traits<const SGal3<_Scalar>>::Scalar;
using traits<const SGal3<Scalar>>::RepSize;
using Base = SGal3Base<Eigen::Map<const SGal3<Scalar>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
} // namespace internal
} // namespace manif
namespace Eigen {
/**
* @brief Specialization of Map for manif::SGal3
*/
template <class _Scalar>
class Map<manif::SGal3<_Scalar>, 0>
: public manif::SGal3Base<Map<manif::SGal3<_Scalar>, 0> > {
using Base = manif::SGal3Base<Map<manif::SGal3<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::rotation;
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_GROUP_MAP_ASSIGN_OP(SGal3)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SGal3
*/
template <class _Scalar>
class Map<const manif::SGal3<_Scalar>, 0>
: public manif::SGal3Base<Map<const manif::SGal3<_Scalar>, 0> > {
using Base = manif::SGal3Base<Map<const manif::SGal3<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
// using Base::rotation;
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} // namespace Eigen
#endif // _MANIF_MANIF_SGAL3_MAP_H_

31
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/sgal3/SGal3_properties.h vendored Normal file
View File

@ -0,0 +1,31 @@
#ifndef _MANIF_MANIF_SGAL3_PROPERTIES_H_
#define _MANIF_MANIF_SGAL3_PROPERTIES_H_
#include "manif/impl/traits.h"
namespace manif {
// Forward declaration
template <typename _Derived> struct SGal3Base;
template <typename _Derived> struct SGal3TangentBase;
namespace internal {
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SGal3Base<_Derived>> {
static constexpr int Dim = 3; /// @brief Space dimension
static constexpr int DoF = 10; /// @brief Degrees of freedom
};
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SGal3TangentBase<_Derived>> {
static constexpr int Dim = 3; /// @brief Space dimension
static constexpr int DoF = 10; /// @brief Degrees of freedom
};
} // namespace internal
} // namespace manif
#endif // _MANIF_MANIF_SGAL3_PROPERTIES_H_

152
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so2/SO2.h vendored Normal file
View File

@ -0,0 +1,152 @@
#ifndef _MANIF_MANIF_SO2_H_
#define _MANIF_MANIF_SO2_H_
#include "manif/impl/so2/SO2_base.h"
namespace manif {
// Forward declare for type traits specialization
template <typename _Scalar> struct SO2;
template <typename _Scalar> struct SO2Tangent;
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SO2<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SO2<_Scalar>;
using Tangent = SO2Tangent<_Scalar>;
using Base = SO2Base<SO2<_Scalar>>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = 2;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using Transformation = Eigen::Matrix<Scalar, 3, 3>;
using Rotation = Eigen::Matrix<Scalar, Dim, Dim>;
using Vector = Eigen::Matrix<Scalar, Dim, 1>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// LieGroup
//
/**
* @brief Represents an element of SO2.
*/
template <typename _Scalar>
struct SO2 : SO2Base<SO2<_Scalar>>
{
private:
using Base = SO2Base<SO2<_Scalar>>;
using Type = SO2<_Scalar>;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
using Base::normalize;
protected:
using Base::derived;
public:
SO2() = default;
~SO2() = default;
MANIF_COPY_CONSTRUCTOR(SO2)
MANIF_MOVE_CONSTRUCTOR(SO2)
// Copy constructor given base
template <typename _DerivedOther>
SO2(const LieGroupBase<_DerivedOther>& o);
MANIF_GROUP_ASSIGN_OP(SO2)
/**
* @brief Constructor given the real and imaginary part
* of a unit complex number representing the angle.
* @param[in] real The real of a unitary complex number.
* @param[in] imag The imaginary of a unitary complex number.
* @throws manif::invalid_argument on un-normalized complex number.
*/
SO2(const Scalar real, const Scalar imag);
//! @brief Constructor given an angle (rad.)
SO2(const Scalar theta);
// LieGroup common API
//! Get a const reference to the underlying DataType.
DataType& coeffs();
const DataType& coeffs() const;
// SO2 specific API
using Base::angle;
protected:
DataType data_;
};
MANIF_EXTRA_GROUP_TYPEDEF(SO2)
template <typename _Scalar>
template <typename _DerivedOther>
SO2<_Scalar>::SO2(const LieGroupBase<_DerivedOther>& o)
: SO2(o.coeffs())
{
//
}
template <typename _Scalar>
SO2<_Scalar>::SO2(const Scalar real, const Scalar imag)
: SO2(DataType(real, imag))
{
//
}
template <typename _Scalar>
SO2<_Scalar>::SO2(const Scalar theta)
: SO2(cos(theta), sin(theta))
{
using std::cos;
using std::sin;
}
template <typename _Scalar>
typename SO2<_Scalar>::DataType&
SO2<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SO2<_Scalar>::DataType&
SO2<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SO2_H_ */

122
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so2/SO2Tangent.h vendored Normal file
View File

@ -0,0 +1,122 @@
#ifndef _MANIF_MANIF_SO2TANGENT_H_
#define _MANIF_MANIF_SO2TANGENT_H_
#include "manif/impl/so2/SO2Tangent_base.h"
namespace manif {
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SO2Tangent<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SO2<_Scalar>;
using Tangent = SO2Tangent<_Scalar>;
using Base = SO2TangentBase<Tangent>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = DoF;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using LieAlg = Eigen::Matrix<Scalar, 2, 2>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// Tangent
//
/**
* @brief Represents an element of tangent space of SO2.
*/
template <typename _Scalar>
struct SO2Tangent : SO2TangentBase<SO2Tangent<_Scalar>>
{
private:
using Base = SO2TangentBase<SO2Tangent<_Scalar>>;
using Type = SO2Tangent<_Scalar>;
protected:
using Base::derived;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
SO2Tangent() = default;
~SO2Tangent() = default;
MANIF_COPY_CONSTRUCTOR(SO2Tangent)
MANIF_MOVE_CONSTRUCTOR(SO2Tangent)
// Copy constructor given base
template <typename _DerivedOther>
SO2Tangent(const TangentBase<_DerivedOther>& o);
MANIF_TANGENT_ASSIGN_OP(SO2Tangent)
//! @brief Constructor given an angle (rad.).
SO2Tangent(const Scalar theta);
// Tangent common API
DataType& coeffs();
const DataType& coeffs() const;
// SO2Tangent specific API
using Base::angle;
protected:
DataType data_;
};
MANIF_EXTRA_TANGENT_TYPEDEF(SO2Tangent);
template <typename _Scalar>
template <typename _DerivedOther>
SO2Tangent<_Scalar>::SO2Tangent(const TangentBase<_DerivedOther>& o)
: data_(o.coeffs())
{
//
}
template <typename _Scalar>
SO2Tangent<_Scalar>::SO2Tangent(const Scalar theta)
: data_(theta)
{
//
}
template <typename _Scalar>
typename SO2Tangent<_Scalar>::DataType&
SO2Tangent<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SO2Tangent<_Scalar>::DataType&
SO2Tangent<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SO2TANGENT_H_ */

240
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so2/SO2Tangent_base.h vendored Normal file
View File

@ -0,0 +1,240 @@
#ifndef _MANIF_MANIF_SO2TANGENT_BASE_H_
#define _MANIF_MANIF_SO2TANGENT_BASE_H_
#include "manif/impl/so2/SO2_properties.h"
#include "manif/impl/tangent_base.h"
namespace manif {
//
// Tangent
//
/**
* @brief The base class of the SO2 tangent.
* @note See Appendix A.
*/
template <typename _Derived>
struct SO2TangentBase : TangentBase<_Derived>
{
private:
using Base = TangentBase<_Derived>;
using Type = SO2TangentBase<_Derived>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_OPERATOR
using Base::coeffs;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SO2TangentBase)
public:
MANIF_TANGENT_ML_ASSIGN_OP(SO2TangentBase)
// Tangent common API
/**
* @brief Hat operator of SO2.
* @return An element of the Lie algebra so2 (skew-symmetric matrix).
* @note See Eqs. (112, 113).
*/
LieAlg hat() const;
/**
* @brief Get the SO2 element.
* @param[out] -optional- J_m_t Jacobian of the SO2 element wrt this.
* @return The SO2 element.
* @note This is the exp() map with the argument in vector form.
* @note See Eqs. (114, 116) and Eq. (126).
*/
LieGroup exp(OptJacobianRef J_m_t = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t = {}) const;
/**
* @brief Get the right Jacobian of SO2.
* @note See Eq. (126).
*/
Jacobian rjac() const;
/**
* @brief Get the left Jacobian of SO2.
* @note See Eq. (126).
*/
Jacobian ljac() const;
/**
* @brief Get the inverse of the right Jacobian of SO2.
* @note See Eq. (126).
* @see rjac.
*/
Jacobian rjacinv() const;
/**
* @brief Get the inverse of the right Jacobian of SO2.
* @note See Eq. (126).
* @see ljac.
*/
Jacobian ljacinv() const;
/**
* @brief
* @return
*/
Jacobian smallAdj() const;
// SO2Tangent specific API
//const Scalar& angle() const;
//! @brief Get the angle (rad.).
Scalar angle() const;
};
template <typename _Derived>
typename SO2TangentBase<_Derived>::LieGroup
SO2TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
using std::cos;
using std::sin;
if (J_m_t)
{
(*J_m_t) = rjac();
}
return LieGroup(cos(angle()), sin(angle()));
}
template <typename _Derived>
typename SO2TangentBase<_Derived>::LieGroup
SO2TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
return exp(J_m_t);
}
template <typename _Derived>
typename SO2TangentBase<_Derived>::LieAlg
SO2TangentBase<_Derived>::hat() const
{
return (LieAlg() <<
Scalar(0) , Scalar(-angle()),
Scalar(angle()), Scalar(0) ).finished();
}
template <typename _Derived>
typename SO2TangentBase<_Derived>::Jacobian
SO2TangentBase<_Derived>::rjac() const
{
static const Jacobian Jr = Jacobian::Constant(Scalar(1));
return Jr;
}
template <typename _Derived>
typename SO2TangentBase<_Derived>::Jacobian
SO2TangentBase<_Derived>::ljac() const
{
static const Jacobian Jl = Jacobian::Constant(Scalar(1));
return Jl;
}
template <typename _Derived>
typename SO2TangentBase<_Derived>::Jacobian
SO2TangentBase<_Derived>::rjacinv() const
{
return rjac();
}
template <typename _Derived>
typename SO2TangentBase<_Derived>::Jacobian
SO2TangentBase<_Derived>::ljacinv() const
{
return ljac();
}
template <typename _Derived>
typename SO2TangentBase<_Derived>::Jacobian
SO2TangentBase<_Derived>::smallAdj() const
{
static const Jacobian smallAdj = Jacobian::Zero();
return smallAdj;
}
// SO2Tangent specific API
//template <typename _Derived>
//const typename SO2TangentBase<_Derived>::Scalar&
//SO2TangentBase<_Derived>::angle() const
//{
// return coeffs().x();
//}
template <typename _Derived>
typename SO2TangentBase<_Derived>::Scalar
SO2TangentBase<_Derived>::angle() const
{
return coeffs()(0);
}
namespace internal {
/**
* @brief Generator specialization for SO2TangentBase objects.
* E = | 0 -1 |
* | 1 0 |
*/
template <typename Derived>
struct GeneratorEvaluator<SO2TangentBase<Derived>>
{
static typename SO2TangentBase<Derived>::LieAlg
run(const unsigned int i)
{
MANIF_CHECK(i==0,
"Index i must be 0!",
invalid_argument);
const static typename SO2TangentBase<Derived>::LieAlg E0 =
skew(typename SO2TangentBase<Derived>::Scalar(1));
return E0;
}
};
//! @brief Random specialization for SO2TangentBase objects.
template <typename Derived>
struct RandomEvaluatorImpl<SO2TangentBase<Derived>>
{
static void run(SO2TangentBase<Derived>& m)
{
// in [-1,1] / in [-PI,PI]
m.coeffs().setRandom() *= MANIF_PI;
}
};
//! @brief Vee specialization for SO2TangentBase objects.
template <typename Derived>
struct VeeEvaluatorImpl<SO2TangentBase<Derived>> {
template <typename TL, typename TR>
static void run(TL& t, const TR& v) {
t.coeffs() << v(1, 0);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SO2_BASE_H_ */

85
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so2/SO2Tangent_map.h vendored Normal file
View File

@ -0,0 +1,85 @@
#ifndef _MANIF_MANIF_SO2TANGENT_MAP_H_
#define _MANIF_MANIF_SO2TANGENT_MAP_H_
#include "manif/impl/so2/SO2Tangent.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SO2Tangent<_Scalar>,0> >
: public traits<SO2Tangent<_Scalar>>
{
using typename traits<SO2Tangent<_Scalar>>::Scalar;
using traits<SO2Tangent<_Scalar>>::DoF;
using DataType = ::Eigen::Map<Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SO2TangentBase<Eigen::Map<SO2Tangent<Scalar>, 0>>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits< Eigen::Map<const SO2Tangent<_Scalar>,0> >
: public traits<const SO2Tangent<_Scalar>>
{
using typename traits<const SO2Tangent<_Scalar>>::Scalar;
using traits<const SO2Tangent<_Scalar>>::DoF;
using DataType = ::Eigen::Map<const Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SO2TangentBase<Eigen::Map<const SO2Tangent<Scalar>, 0>>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
//! @brief Specialization of Map for manif::SO2Tangent
template <class _Scalar>
class Map<manif::SO2Tangent<_Scalar>, 0>
: public manif::SO2TangentBase<Map<manif::SO2Tangent<_Scalar>, 0> >
{
using Base = manif::SO2TangentBase<Map<manif::SO2Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_TANGENT_MAP_ASSIGN_OP(SO2Tangent)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
//! @brief Specialization of Map for const manif::SO2Tangent
template <class _Scalar>
class Map<const manif::SO2Tangent<_Scalar>, 0>
: public manif::SO2TangentBase<Map<const manif::SO2Tangent<_Scalar>, 0> >
{
using Base = manif::SO2TangentBase<Map<const manif::SO2Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SO2TANGENT_MAP_H_ */

355
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so2/SO2_base.h vendored Normal file
View File

@ -0,0 +1,355 @@
#ifndef _MANIF_MANIF_SO2_BASE_H_
#define _MANIF_MANIF_SO2_BASE_H_
#include "manif/impl/so2/SO2_properties.h"
#include "manif/impl/lie_group_base.h"
namespace manif {
//
// LieGroup
//
/**
* @brief The base class of the SO2 group.
* @note See Appendix A of the paper.
*/
template <typename _Derived>
struct SO2Base : LieGroupBase<_Derived>
{
private:
using Base = LieGroupBase<_Derived>;
using Type = SO2Base<_Derived>;
public:
MANIF_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_AUTO_API
MANIF_INHERIT_GROUP_OPERATOR
using Base::coeffs;
using Rotation = typename internal::traits<_Derived>::Rotation;
using Transformation = typename internal::traits<_Derived>::Transformation;
// LieGroup common API
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SO2Base)
public:
MANIF_GROUP_ML_ASSIGN_OP(SO2Base)
/**
* @brief Get the inverse of this.
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this.
* @note z^-1 = z*
* @note See Eqs. (118,124).
*/
LieGroup inverse(OptJacobianRef J_minv_m = {}) const;
/**
* @brief Get the SO2 corresponding Lie algebra element in vector form.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt to this.
* @return The SO2 tangent of this.
* @note This is the log() map in vector form.
* @note See Eq. (115) & Eqs. (79,126).
* @see SO2Tangent.
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another SO2 element.
* @param[in] m Another SO2 element.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
* @note z_c = z_a z_b.
* @note See Eq. (125).
*/
template <typename _DerivedOther>
LieGroup compose(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}) const;
/**
* @brief Rotation action on a 2-vector.
* @param v A 2-vector.
* @param[out] -optional- J_vout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object.
* @return The rotated 2-vector.
* @note See Eqs. (129, 130).
*/
template <typename _EigenDerived>
Eigen::Matrix<Scalar, 2, 1>
act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 1>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 2>>> J_vout_v = {}) const;
/**
* @brief Get the ajoint matrix of SO2 at this.
* @note See Eqs. (123).
*/
Jacobian adj() const;
// SO2 specific functions
/**
* @brief Get the transformation matrix (2D isometry).
* @note T = | R 0 |
* | 0 1 |
*/
Transformation transform() const;
/**
* @brief Get the rotation matrix R.
*/
Rotation rotation() const;
/**
* @brief Get the real part of the underlying complex number.
*/
Scalar real() const;
/**
* @brief Get the imaginary part of the underlying complex number.
*/
Scalar imag() const;
/**
* @brief Get the angle (rad.).
*/
Scalar angle() const;
/**
* @brief Normalize the underlying complex number.
*/
void normalize();
// protected:
/// @todo given a Eigen::Map<const SO2>
/// coeffs()->x() return a reference to
/// temporary ...
// Scalar& real();
// Scalar& imag();
};
template <typename _Derived>
typename SO2Base<_Derived>::Transformation
SO2Base<_Derived>::transform() const
{
Transformation T(Transformation::Identity());
T.template topLeftCorner<2, 2>() = rotation();
return T;
}
template <typename _Derived>
typename SO2Base<_Derived>::Rotation
SO2Base<_Derived>::rotation() const
{
using std::sin;
using std::cos;
const Scalar theta = angle();
return (Rotation() << cos(theta), -sin(theta),
sin(theta), cos(theta)).finished();
}
template <typename _Derived>
typename SO2Base<_Derived>::LieGroup
SO2Base<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
if (J_minv_m)
J_minv_m->setConstant(Scalar(-1));
return LieGroup(real(), -imag());
}
template <typename _Derived>
typename SO2Base<_Derived>::Tangent
SO2Base<_Derived>::log(OptJacobianRef J_t_m) const
{
if (J_t_m)
J_t_m->setConstant(Scalar(1));
return Tangent(angle());
}
template <typename _Derived>
typename SO2Base<_Derived>::Tangent
SO2Base<_Derived>::lift(OptJacobianRef J_t_m) const
{
return log(J_t_m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename SO2Base<_Derived>::LieGroup
SO2Base<_Derived>::compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb) const
{
using std::abs;
static_assert(
std::is_base_of<SO2Base<_DerivedOther>, _DerivedOther>::value,
"Argument does not inherit from SE2Base !");
if (J_mc_ma)
J_mc_ma->setConstant(Scalar(1));
if (J_mc_mb)
J_mc_mb->setConstant(Scalar(1));
const auto& m_so2 = static_cast<const SO2Base<_DerivedOther>&>(m);
Scalar ret_real = real() * m_so2.real() - imag() * m_so2.imag();
Scalar ret_imag = real() * m_so2.imag() + imag() * m_so2.real();
const Scalar ret_sqnorm = ret_real*ret_real+ret_imag*ret_imag;
if (abs(ret_sqnorm-Scalar(1)) > Constants<Scalar>::eps)
{
const Scalar scale = approxSqrtInv(ret_sqnorm);
ret_real *= scale;
ret_imag *= scale;
}
return LieGroup(ret_real, ret_imag);
}
template <typename _Derived>
template <typename _EigenDerived>
Eigen::Matrix<typename SO2Base<_Derived>::Scalar, 2, 1>
SO2Base<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 1>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 2>>> J_vout_v) const
{
assert_vector_dim(v, 2);
const Rotation R(rotation());
if (J_vout_m)
{
J_vout_m->noalias() = R * skew(Scalar(1)) * v;
}
if (J_vout_v)
{
(*J_vout_v) = R;
}
return R * v;
}
template <typename _Derived>
typename SO2Base<_Derived>::Jacobian
SO2Base<_Derived>::adj() const
{
static const Jacobian adj = Jacobian::Constant(Scalar(1));
return adj;
}
// SO2 specific function
template <typename _Derived>
/*const*/ typename SO2Base<_Derived>::Scalar/*&*/
SO2Base<_Derived>::real() const
{
return coeffs().x();
}
template <typename _Derived>
/*const*/ typename SO2Base<_Derived>::Scalar/*&*/
SO2Base<_Derived>::imag() const
{
return coeffs().y();
}
template <typename _Derived>
typename SO2Base<_Derived>::Scalar
SO2Base<_Derived>::angle() const
{
using std::atan2;
return atan2(imag(), real());
}
//template <typename _Derived>
//typename SO2Base<_Derived>::Scalar&
//SO2Base<_Derived>::real()
//{
// return coeffs.x();
//}
//template <typename _Derived>
//typename SO2Base<_Derived>::Scalar&
//SO2Base<_Derived>::imag()
//{
// return coeffs.y();
//}
template <typename _Derived>
void SO2Base<_Derived>::normalize()
{
coeffs().normalize();
}
namespace internal {
//! @brief Random specialization for SO2Base objects.
template <typename Derived>
struct RandomEvaluatorImpl<SO2Base<Derived>>
{
template <typename T>
static void run(T& m)
{
using Tangent = typename LieGroupBase<Derived>::Tangent;
m = Tangent::Random().exp();
}
};
//! @brief Assignment assert specialization for SO2Base objects
template <typename Derived>
struct AssignmentEvaluatorImpl<SO2Base<Derived>>
{
template <typename T>
static void run_impl(const T& data)
{
using std::abs;
MANIF_ASSERT(
abs(data.norm()-typename SO2Base<Derived>::Scalar(1)) <
Constants<typename SO2Base<Derived>::Scalar>::eps,
"SO2 assigned data not normalized !",
invalid_argument
);
MANIF_UNUSED_VARIABLE(data);
}
};
//! @brief Cast specialization for SO2Base objects.
template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SO2Base<Derived>, NewScalar> {
template <typename T>
static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
return typename Derived::template LieGroupTemplate<NewScalar>(NewScalar(o.angle()));
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SO2_BASE_H_ */

91
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so2/SO2_map.h vendored Normal file
View File

@ -0,0 +1,91 @@
#ifndef _MANIF_MANIF_SO2_MAP_H_
#define _MANIF_MANIF_SO2_MAP_H_
#include "manif/impl/so2/SO2.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SO2<_Scalar>,0> >
: public traits<SO2<_Scalar>>
{
using typename traits<SO2<_Scalar>>::Scalar;
using traits<SO2<Scalar>>::RepSize;
using Base = SO2Base<Eigen::Map<SO2<Scalar>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits< Eigen::Map<const SO2<_Scalar>,0> >
: public traits<const SO2<_Scalar>>
{
using typename traits<const SO2<_Scalar>>::Scalar;
using traits<const SO2<Scalar>>::RepSize;
using Base = SO2Base<Eigen::Map<const SO2<Scalar>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SO2
*/
template <class _Scalar>
class Map<manif::SO2<_Scalar>, 0>
: public manif::SO2Base<Map<manif::SO2<_Scalar>, 0> >
{
using Base = manif::SO2Base<Map<manif::SO2<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_GROUP_MAP_ASSIGN_OP(SO2)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SO2
*/
template <class _Scalar>
class Map<const manif::SO2<_Scalar>, 0>
: public manif::SO2Base<Map<const manif::SO2<_Scalar>, 0> >
{
using Base = manif::SO2Base<Map<const manif::SO2<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SO2_MAP_H_ */

33
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so2/SO2_properties.h vendored Normal file
View File

@ -0,0 +1,33 @@
#ifndef _MANIF_MANIF_SO2_PROPERTIES_H_
#define _MANIF_MANIF_SO2_PROPERTIES_H_
#include "manif/impl/traits.h"
namespace manif {
// Forward declaration
template <typename _Derived> struct SO2Base;
template <typename _Derived> struct SO2TangentBase;
namespace internal {
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SO2Base<_Derived>>
{
static constexpr int Dim = 2; /// @brief Space dimension
static constexpr int DoF = 1; /// @brief Degrees of freedom
};
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SO2TangentBase<_Derived>>
{
static constexpr int Dim = 2; /// @brief Space dimension
static constexpr int DoF = 1; /// @brief Degrees of freedom
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SO2_PROPERTIES_H_ */

183
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so3/SO3.h vendored Normal file
View File

@ -0,0 +1,183 @@
#ifndef _MANIF_MANIF_SO3_H_
#define _MANIF_MANIF_SO3_H_
#include "manif/impl/so3/SO3_base.h"
namespace manif {
// Forward declare for type traits specialization
template <typename _Scalar> struct SO3;
template <typename _Scalar> struct SO3Tangent;
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SO3<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SO3<_Scalar>;
using Tangent = SO3Tangent<_Scalar>;
using Base = SO3Base<SO3<_Scalar>>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = 4;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using Transformation = Eigen::Matrix<Scalar, 4, 4>;
using Rotation = Eigen::Matrix<Scalar, Dim, Dim>;
using Vector = Eigen::Matrix<Scalar, Dim, 1>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// LieGroup
//
/**
* @brief Represents an element of SO3.
*/
template <typename _Scalar>
struct SO3 : SO3Base<SO3<_Scalar>>
{
private:
using Base = SO3Base<SO3<_Scalar>>;
using Type = SO3<_Scalar>;
using QuaternionDataType = Eigen::Quaternion<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
using Base::quat;
using Base::normalize;
SO3() = default;
~SO3() = default;
MANIF_COPY_CONSTRUCTOR(SO3)
MANIF_MOVE_CONSTRUCTOR(SO3)
// Copy constructor given base
template <typename _DerivedOther>
SO3(const LieGroupBase<_DerivedOther>& o);
MANIF_GROUP_ASSIGN_OP(SO3)
/**
* @brief Constructor given a unit quaternion.
* @param[in] q A unit quaternion.
* @throws manif::invalid_argument on un-normalized quaternion.
*/
SO3(const QuaternionDataType& q);
/**
* @brief Constructor given the quaternion's coefficients.
* @param[in] x The x-components of a unit quaternion.
* @param[in] y The x-components of a unit quaternion.
* @param[in] z The x-components of a unit quaternion.
* @param[in] w The x-components of a unit quaternion.
* @throws manif::invalid_argument on un-normalized quaternion.
*/
SO3(const Scalar x, const Scalar y,
const Scalar z, const Scalar w);
/**
* @brief Constructor given an angle axis.
*/
SO3(const Eigen::AngleAxis<Scalar>& angle_axis);
/**
* @brief Constructor given Euler angles.
*/
SO3(const Scalar roll, const Scalar pitch,
const Scalar yaw);
DataType& coeffs();
const DataType& coeffs() const;
protected:
DataType data_;
};
MANIF_EXTRA_GROUP_TYPEDEF(SO3)
template <typename _Scalar>
template <typename _DerivedOther>
SO3<_Scalar>::SO3(const LieGroupBase<_DerivedOther>& o)
: SO3(o.coeffs())
{
//
}
template <typename _Scalar>
SO3<_Scalar>::SO3(const QuaternionDataType& q)
: SO3(q.coeffs())
{
//
}
template <typename _Scalar>
SO3<_Scalar>::SO3(const Scalar x, const Scalar y,
const Scalar z, const Scalar w)
: SO3((DataType() << x, y, z, w).finished())
{
//
}
template <typename _Scalar>
SO3<_Scalar>::SO3(const Eigen::AngleAxis<Scalar>& angle_axis)
: SO3(QuaternionDataType(angle_axis).coeffs())
{
}
template <typename _Scalar>
SO3<_Scalar>::SO3(const Scalar roll,
const Scalar pitch,
const Scalar yaw)
: SO3(Eigen::AngleAxis<Scalar>(yaw, Eigen::Matrix<Scalar, 3, 1>::UnitZ()) *
Eigen::AngleAxis<Scalar>(pitch, Eigen::Matrix<Scalar, 3, 1>::UnitY()) *
Eigen::AngleAxis<Scalar>(roll, Eigen::Matrix<Scalar, 3, 1>::UnitX()) )
{
//
}
template <typename _Scalar>
typename SO3<_Scalar>::DataType&
SO3<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SO3<_Scalar>::DataType&
SO3<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SO3_H_ */

112
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so3/SO3Tangent.h vendored Normal file
View File

@ -0,0 +1,112 @@
#ifndef _MANIF_MANIF_SO3TANGENT_H_
#define _MANIF_MANIF_SO3TANGENT_H_
#include "manif/impl/so3/SO3Tangent_base.h"
namespace manif {
namespace internal {
//! Traits specialization
template <typename _Scalar>
struct traits<SO3Tangent<_Scalar>>
{
using Scalar = _Scalar;
using LieGroup = SO3<_Scalar>;
using Tangent = SO3Tangent<_Scalar>;
using Base = SO3TangentBase<Tangent>;
static constexpr int Dim = LieGroupProperties<Base>::Dim;
static constexpr int DoF = LieGroupProperties<Base>::DoF;
static constexpr int RepSize = DoF;
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
using LieAlg = Eigen::Matrix<Scalar, 3, 3>;
};
} /* namespace internal */
} /* namespace manif */
namespace manif {
//
// Tangent
//
/**
* @brief Represents an element of tangent space of SO3.
*/
template <typename _Scalar>
struct SO3Tangent : SO3TangentBase<SO3Tangent<_Scalar>>
{
private:
using Base = SO3TangentBase<SO3Tangent<_Scalar>>;
using Type = SO3Tangent<_Scalar>;
protected:
using Base::derived;
public:
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
SO3Tangent() = default;
~SO3Tangent() = default;
MANIF_COPY_CONSTRUCTOR(SO3Tangent)
MANIF_MOVE_CONSTRUCTOR(SO3Tangent)
// Copy constructor given base
template <typename _DerivedOther>
SO3Tangent(const TangentBase<_DerivedOther>& o);
MANIF_TANGENT_ASSIGN_OP(SO3Tangent)
// Tangent common API
DataType& coeffs();
const DataType& coeffs() const;
// SO3Tangent specific API
protected:
DataType data_;
};
MANIF_EXTRA_TANGENT_TYPEDEF(SO3Tangent);
template <typename _Scalar>
template <typename _DerivedOther>
SO3Tangent<_Scalar>::SO3Tangent(const TangentBase<_DerivedOther>& o)
: data_(o.coeffs())
{
//
}
template <typename _Scalar>
typename SO3Tangent<_Scalar>::DataType&
SO3Tangent<_Scalar>::coeffs()
{
return data_;
}
template <typename _Scalar>
const typename SO3Tangent<_Scalar>::DataType&
SO3Tangent<_Scalar>::coeffs() const
{
return data_;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_SO3TANGENT_H_ */

342
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so3/SO3Tangent_base.h vendored Normal file
View File

@ -0,0 +1,342 @@
#ifndef _MANIF_MANIF_SO3TANGENT_BASE_H_
#define _MANIF_MANIF_SO3TANGENT_BASE_H_
#include "manif/impl/so3/SO3_properties.h"
#include "manif/impl/tangent_base.h"
namespace manif {
//
// Tangent
//
/**
* @brief The base class of the SO3 tangent.
* @note See Appendix B.
*/
template <typename _Derived>
struct SO3TangentBase : TangentBase<_Derived>
{
private:
using Base = TangentBase<_Derived>;
using Type = SO3TangentBase<_Derived>;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_OPERATOR
using AngBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
using ConstAngBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;
// Tangent common API
using Base::coeffs;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SO3TangentBase)
public:
MANIF_TANGENT_ML_ASSIGN_OP(SO3TangentBase)
/**
* @brief Hat operator of SO3.
* @return An element of the Lie algebra so3 (skew-symmetric matrix).
* @note See example 3 of the paper.
*/
LieAlg hat() const;
/**
* @brief Get the SO3 element.
* @param[out] -optional- J_m_t Jacobian of the SO3 element wrt this.
* @return The SO3 element.
* @note This is the exp() map with the argument in vector form.
* @note See Eq. (132) and Eq. (143).
*/
LieGroup exp(OptJacobianRef J_m_t = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t = {}) const;
/**
* Get the right Jacobian of SO3.
* @note See Eq. (143).
*/
Jacobian rjac() const;
/**
* Get the left Jacobian of SO3.
* @note See Eq. (145).
*/
Jacobian ljac() const;
/**
* Get the inverse of the right Jacobian of SO3.
* @note See Eq. (144).
* @see rjac.
*/
Jacobian rjacinv() const;
/**
* Get the inverse of the left Jacobian of SO3.
* @note See Eq. (146).
* @see ljac.
*/
Jacobian ljacinv() const;
/**
* @brief
* @return
*/
Jacobian smallAdj() const;
// SO3Tangent specific API
//! @brief
Scalar x() const;
//! @brief
Scalar y() const;
//! @brief
Scalar z() const;
//! @brief Get the angular part.
AngBlock ang();
const ConstAngBlock ang() const;
};
template <typename _Derived>
typename SO3TangentBase<_Derived>::LieGroup
SO3TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
using std::sqrt;
using std::cos;
using std::sin;
const DataType& theta_vec = coeffs();
const Scalar theta_sq = theta_vec.squaredNorm();
if (theta_sq > Constants<Scalar>::eps)
{
const Scalar theta = sqrt(theta_sq);
if (J_m_t)
{
const LieAlg W = hat();
J_m_t->setIdentity();
J_m_t->noalias() -= (Scalar(1.0) - cos(theta)) / theta_sq * W;
J_m_t->noalias() += (theta - sin(theta)) / (theta_sq * theta) * W * W;
}
return LieGroup( Eigen::AngleAxis<Scalar>(theta, theta_vec.normalized()) );
}
else
{
if (J_m_t)
{
J_m_t->setIdentity();
J_m_t->noalias() -= Scalar(0.5) * hat();
}
return LieGroup(x()/Scalar(2), y()/Scalar(2), z()/Scalar(2), Scalar(1));
}
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::LieGroup
SO3TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
return exp(J_m_t);
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::Jacobian
SO3TangentBase<_Derived>::rjac() const
{
return ljac().transpose();
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::Jacobian
SO3TangentBase<_Derived>::ljac() const
{
using std::sqrt;
using std::cos;
using std::sin;
const Scalar theta_sq = coeffs().squaredNorm();
const LieAlg W = hat();
// Small angle approximation
if (theta_sq <= Constants<Scalar>::eps)
return Jacobian::Identity() + Scalar(0.5) * W;
const Scalar theta = sqrt(theta_sq); // rotation angle
return Jacobian::Identity() +
(Scalar(1) - cos(theta)) / theta_sq * W +
(theta - sin(theta)) / (theta_sq * theta) * W * W;
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::Jacobian
SO3TangentBase<_Derived>::rjacinv() const
{
return ljacinv().transpose();
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::Jacobian
SO3TangentBase<_Derived>::ljacinv() const
{
using std::sqrt;
using std::cos;
using std::sin;
const Scalar theta_sq = coeffs().squaredNorm();
const LieAlg W = hat();
if (theta_sq <= Constants<Scalar>::eps)
return Jacobian::Identity() - Scalar(0.5) * W;
const Scalar theta = sqrt(theta_sq); // rotation angle
return Jacobian::Identity() -
Scalar(0.5) * W +
(Scalar(1) / theta_sq - (Scalar(1) + cos(theta)) / (Scalar(2) * theta * sin(theta))) *
W * W;
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::Jacobian
SO3TangentBase<_Derived>::smallAdj() const
{
return hat();
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::LieAlg
SO3TangentBase<_Derived>::hat() const
{
return skew(coeffs());
}
// SO3Tangent specifics
template <typename _Derived>
typename SO3TangentBase<_Derived>::Scalar
SO3TangentBase<_Derived>::x() const
{
return coeffs()(0);
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::Scalar
SO3TangentBase<_Derived>::y() const
{
return coeffs()(1);
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::Scalar
SO3TangentBase<_Derived>::z() const
{
return coeffs()(2);
}
template <typename _Derived>
typename SO3TangentBase<_Derived>::AngBlock
SO3TangentBase<_Derived>::ang()
{
return coeffs().template tail<3>();
}
template <typename _Derived>
const typename SO3TangentBase<_Derived>::ConstAngBlock
SO3TangentBase<_Derived>::ang() const
{
return coeffs().template tail<3>();
}
namespace internal {
//! @brief Generator specialization for SO3TangentBase objects.
template <typename Derived>
struct GeneratorEvaluator<SO3TangentBase<Derived>>
{
static typename SO3TangentBase<Derived>::LieAlg
run(const unsigned int i)
{
using LieAlg = typename SO3TangentBase<Derived>::LieAlg;
using Scalar = typename SO3TangentBase<Derived>::Scalar;
switch (i)
{
case 0:
{
static const LieAlg E0(
(LieAlg() << Scalar(0), Scalar(0), Scalar( 0),
Scalar(0), Scalar(0), Scalar(-1),
Scalar(0), Scalar(1), Scalar( 0) ).finished());
return E0;
}
case 1:
{
static const LieAlg E1(
(LieAlg() << Scalar( 0), Scalar(0), Scalar(1),
Scalar( 0), Scalar(0), Scalar(0),
Scalar(-1), Scalar(0), Scalar(0) ).finished());
return E1;
}
case 2:
{
static const LieAlg E2(
(LieAlg() << Scalar(0), Scalar(-1), Scalar(0),
Scalar(1), Scalar( 0), Scalar(0),
Scalar(0), Scalar( 0), Scalar(0) ).finished());
return E2;
}
default:
MANIF_THROW("Index i must be in [0,2]!", invalid_argument);
break;
}
return LieAlg{};
}
};
//! @brief Random specialization for SO3TangentBase objects.
template <typename Derived>
struct RandomEvaluatorImpl<SO3TangentBase<Derived>>
{
static void run(SO3TangentBase<Derived>& m)
{
// In ball of radius PI
m.coeffs() = randPointInBall(MANIF_PI).template cast<typename Derived::Scalar>();
}
};
//! @brief Vee specialization for SO3TangentBase objects.
template <typename Derived>
struct VeeEvaluatorImpl<SO3TangentBase<Derived>> {
template <typename TL, typename TR>
static void run(TL& t, const TR& v) {
t.coeffs() << v(2, 1), v(0, 2), v(1, 0);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SO3TANGENT_BASE_H_ */

89
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so3/SO3Tangent_map.h vendored Normal file
View File

@ -0,0 +1,89 @@
#ifndef _MANIF_MANIF_SO3TANGENT_MAP_H_
#define _MANIF_MANIF_SO3TANGENT_MAP_H_
#include "manif/impl/so3/SO3Tangent.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SO3Tangent<_Scalar>,0> >
: public traits<SO3Tangent<_Scalar>>
{
using typename traits<SO3Tangent<_Scalar>>::Scalar;
using traits<SO3Tangent<_Scalar>>::DoF;
using DataType = ::Eigen::Map<Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SO3TangentBase<Eigen::Map<SO3Tangent<Scalar>, 0>>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits< Eigen::Map<const SO3Tangent<_Scalar>,0> >
: public traits<const SO3Tangent<_Scalar>>
{
using typename traits<const SO3Tangent<_Scalar>>::Scalar;
using traits<const SO3Tangent<_Scalar>>::DoF;
using DataType = ::Eigen::Map<const Eigen::Matrix<Scalar, DoF, 1>, 0>;
using Base = SO3TangentBase<Eigen::Map<const SO3Tangent<Scalar>, 0>>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SO3Tangent
*/
template <class _Scalar>
class Map<manif::SO3Tangent<_Scalar>, 0>
: public manif::SO3TangentBase<Map<manif::SO3Tangent<_Scalar>, 0> >
{
using Base = manif::SO3TangentBase<Map<manif::SO3Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(Scalar* coeffs) : data_(coeffs) { }
MANIF_TANGENT_MAP_ASSIGN_OP(SO3Tangent)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SO3Tangent
*/
template <class _Scalar>
class Map<const manif::SO3Tangent<_Scalar>, 0>
: public manif::SO3TangentBase<Map<const manif::SO3Tangent<_Scalar>, 0> >
{
using Base = manif::SO3TangentBase<Map<const manif::SO3Tangent<_Scalar>, 0> >;
public:
MANIF_TANGENT_TYPEDEF
MANIF_INHERIT_TANGENT_API
MANIF_INHERIT_TANGENT_OPERATOR
Map(const Scalar* coeffs) : data_(coeffs) { }
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SO3TANGENT_MAP_H_ */

440
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so3/SO3_base.h vendored Normal file
View File

@ -0,0 +1,440 @@
#ifndef _MANIF_MANIF_SO3_BASE_H_
#define _MANIF_MANIF_SO3_BASE_H_
#include "manif/impl/so3/SO3_properties.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/utils.h"
namespace manif {
//
// LieGroup
//
/**
* @brief The base class of the SO3 group.
* @note See Appendix B of the paper.
*/
template <typename _Derived>
struct SO3Base : LieGroupBase<_Derived>
{
private:
using Base = LieGroupBase<_Derived>;
using Type = SO3Base<_Derived>;
public:
MANIF_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_AUTO_API
MANIF_INHERIT_GROUP_OPERATOR
using Base::coeffs;
using Rotation = typename internal::traits<_Derived>::Rotation;
using Transformation = typename internal::traits<_Derived>::Transformation;
using QuaternionDataType = Eigen::Quaternion<Scalar>;
protected:
using Base::derived;
MANIF_DEFAULT_CONSTRUCTOR(SO3Base)
public:
MANIF_GROUP_ML_ASSIGN_OP(SO3Base)
// LieGroup common API
/**
* @brief Get the inverse of this.
* @param[out] -optional- J_minv_m Jacobian of the inverse wrt this.
* @note q^-1 = q*. See Eq. (140).
*/
LieGroup inverse(OptJacobianRef J_minv_m = {}) const;
/**
* @brief Get the SO3 corresponding Lie algebra element in vector form.
* @param[out] -optional- J_t_m Jacobian of the tangent wrt to this.
* @return The SO3 tangent of this.
* @note This is the log() map in vector form.
* @note See Eq. (133) & Eq. (144).
* @see SO3Tangent.
*/
Tangent log(OptJacobianRef J_t_m = {}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref log instead.
*/
MANIF_DEPRECATED
Tangent lift(OptJacobianRef J_t_m = {}) const;
/**
* @brief Composition of this and another SO3 element.
* @param[in] m Another SO3 element.
* @param[out] -optional- J_mc_ma Jacobian of the composition wrt this.
* @param[out] -optional- J_mc_mb Jacobian of the composition wrt m.
* @return The composition of 'this . m'.
* @note Quaternion product.
* @note See Eqs. (141,142).
*/
template <typename _DerivedOther>
LieGroup compose(const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma = {},
OptJacobianRef J_mc_mb = {}) const;
/**
* @brief Rotation action on a 3-vector.
* @param v A 2-vector.
* @param[out] -optional- J_vout_m The Jacobian of the new object wrt this.
* @param[out] -optional- J_vout_v The Jacobian of the new object wrt input object.
* @return The rotated 3-vector.
* @note See Eq (136), Eqs. (150,151)
*/
template <typename _EigenDerived>
Eigen::Matrix<Scalar, 3, 1>
act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_m = {},
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v = {}) const;
/**
* @brief Get the adjoint of SO3 at this.
* @note See Eq. (139).
*/
Jacobian adj() const;
// SO3 specific functions
/**
* @brief Get the transformation matrix (3D isometry).
* @note T = | R 0 |
* | 0 1 |
*/
Transformation transform() const;
//! @brief Get a rotation matrix.
Rotation rotation() const;
//! @brief Get the x component of the quaternion.
Scalar x() const;
//! @brief Get the y component of the quaternion.
Scalar y() const;
//! @brief Get the z component of the quaternion.
Scalar z() const;
//! @brief Get the w component of the quaternion.
Scalar w() const;
//! @brief Get quaternion.
QuaternionDataType quat() const;
/**
* @brief Normalize the underlying quaternion.
*/
void normalize();
/**
* @brief Set the rotational as a quaternion.
* @param quaternion a unitary quaternion
*/
void quat(const QuaternionDataType& quaternion);
/**
* @brief Set the rotational as a quaternion.
* @param quaternion an Eigen::Vector representing a unitary quaternion
*/
template <typename _EigenDerived>
void quat(const Eigen::MatrixBase<_EigenDerived>& quaternion);
};
template <typename _Derived>
typename SO3Base<_Derived>::Transformation
SO3Base<_Derived>::transform() const
{
Transformation T = Transformation::Identity();
T.template topLeftCorner<3,3>() = rotation();
return T;
}
template <typename _Derived>
typename SO3Base<_Derived>::Rotation
SO3Base<_Derived>::rotation() const
{
return quat().matrix();
}
template <typename _Derived>
typename SO3Base<_Derived>::LieGroup
SO3Base<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
if (J_minv_m)
{
*J_minv_m = -rotation();
}
/// @todo, conjugate doc :
/// equal to the multiplicative inverse if
/// the quaternion is normalized
return LieGroup(quat().conjugate());
}
template <typename _Derived>
typename SO3Base<_Derived>::Tangent
SO3Base<_Derived>::log(OptJacobianRef J_t_m) const
{
using std::sqrt;
using std::atan2;
Tangent tan;
Scalar log_coeff;
const Scalar sin_angle_squared = coeffs().template head<3>().squaredNorm();
if (sin_angle_squared > Constants<Scalar>::eps)
{
const Scalar sin_angle = sqrt(sin_angle_squared);
const Scalar cos_angle = w();
/** @note If (cos_angle < 0) then angle >= pi/2 ,
* means : angle for angle_axis vector >= pi (== 2*angle)
* |-> results in correct rotation but not a normalized angle_axis vector
*
* In that case we observe that 2 * angle ~ 2 * angle - 2 * pi,
* which is equivalent saying
*
* angle - pi = atan(sin(angle - pi), cos(angle - pi))
* = atan(-sin(angle), -cos(angle))
*/
const Scalar two_angle = Scalar(2.0) * ((cos_angle < Scalar(0.0)) ?
Scalar(atan2(-sin_angle, -cos_angle)) :
Scalar(atan2( sin_angle, cos_angle)));
log_coeff = two_angle / sin_angle;
}
else
{
// small-angle approximation
log_coeff = Scalar(2.0);
}
tan = Tangent(coeffs().template head<3>() * log_coeff);
// using std::atan2;
// Scalar n = coeffs().template head<3>().norm();
// Scalar angle(0);
// typename Tangent::DataType axis(1,0,0);
// if (n<Constants<Scalar>::eps)
// n = coeffs().template head<3>().stableNorm();
// if (n > Scalar(0))
// {
// angle = Scalar(2)*atan2(n, w());
// axis = coeffs().template head<3>() / n;
// }
// tan = Tangent(axis*angle);
if (J_t_m)
{
J_t_m->setIdentity();
J_t_m->noalias() += Scalar(0.5) * tan.hat();
Scalar theta2 = tan.coeffs().squaredNorm();
if (theta2 > Constants<Scalar>::eps)
{
Scalar theta = sqrt(theta2); // rotation angle
J_t_m->noalias() +=
(Scalar(1) / theta2 - (Scalar(1) + cos(theta)) / (Scalar(2) * theta * sin(theta))) *
tan.hat() * tan.hat();
}
}
return tan;
}
template <typename _Derived>
typename SO3Base<_Derived>::Tangent
SO3Base<_Derived>::lift(OptJacobianRef J_t_m) const
{
return log(J_t_m);
}
template <typename _Derived>
template <typename _DerivedOther>
typename SO3Base<_Derived>::LieGroup
SO3Base<_Derived>::compose(
const LieGroupBase<_DerivedOther>& m,
OptJacobianRef J_mc_ma,
OptJacobianRef J_mc_mb) const
{
using std::abs;
static_assert(
std::is_base_of<SO3Base<_DerivedOther>, _DerivedOther>::value,
"Argument does not inherit from S03Base !");
const auto& m_SO3 = static_cast<const SO3Base<_DerivedOther>&>(m);
if (J_mc_ma)
{
*J_mc_ma = m_SO3.rotation().transpose();
}
if (J_mc_mb)
J_mc_mb->setIdentity();
QuaternionDataType ret_q = quat() * m_SO3.quat();
const Scalar ret_sqnorm = ret_q.squaredNorm();
if (abs(ret_sqnorm-Scalar(1)) > Constants<Scalar>::eps)
{
ret_q.coeffs() *= approxSqrtInv(ret_sqnorm);
}
return LieGroup(ret_q);
}
template <typename _Derived>
template <typename _EigenDerived>
Eigen::Matrix<typename SO3Base<_Derived>::Scalar, 3, 1>
SO3Base<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_m,
tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v) const
{
assert_vector_dim(v, 3);
const Rotation R(rotation());
if (J_vout_m)
{
J_vout_m->noalias() = -R * skew(v);
}
if (J_vout_v)
{
(*J_vout_v) = R;
}
return R * v;
}
template <typename _Derived>
typename SO3Base<_Derived>::Jacobian
SO3Base<_Derived>::adj() const
{
return rotation();
}
// SO3 specific
template <typename _Derived>
typename SO3Base<_Derived>::Scalar
SO3Base<_Derived>::x() const
{
return coeffs().x();
}
template <typename _Derived>
typename SO3Base<_Derived>::Scalar
SO3Base<_Derived>::y() const
{
return coeffs().y();
}
template <typename _Derived>
typename SO3Base<_Derived>::Scalar
SO3Base<_Derived>::z() const
{
return coeffs().z();
}
template <typename _Derived>
typename SO3Base<_Derived>::Scalar
SO3Base<_Derived>::w() const
{
return coeffs().w();
}
template <typename _Derived>
typename SO3Base<_Derived>::QuaternionDataType
SO3Base<_Derived>::quat() const
{
return QuaternionDataType(coeffs());
}
template <typename _Derived>
void SO3Base<_Derived>::normalize()
{
coeffs().normalize();
}
template <typename _Derived>
void SO3Base<_Derived>::quat(const QuaternionDataType& quaternion)
{
quat(quaternion.coeffs());
}
template <typename _Derived>
template <typename _EigenDerived>
void SO3Base<_Derived>::quat(const Eigen::MatrixBase<_EigenDerived>& quaternion)
{
using std::abs;
assert_vector_dim(quaternion, 4);
MANIF_ASSERT(abs(quaternion.norm()-Scalar(1)) <
Constants<Scalar>::eps,
"The quaternion is not normalized !",
invalid_argument);
coeffs() = quaternion;
}
namespace internal {
//! @brief Random specialization for SO3Base objects.
template <typename Derived>
struct RandomEvaluatorImpl<SO3Base<Derived>>
{
template <typename T>
static void run(T& m)
{
using Scalar = typename SO3Base<Derived>::Scalar;
using LieGroup = typename SO3Base<Derived>::LieGroup;
m = LieGroup(randQuat<Scalar>());
}
};
//! @brief Assignment assert specialization for SO3Base objects
template <typename Derived>
struct AssignmentEvaluatorImpl<SO3Base<Derived>>
{
template <typename T>
static void run_impl(const T& data)
{
using std::abs;
MANIF_ASSERT(
abs(data.norm()-typename SO3Base<Derived>::Scalar(1)) <
Constants<typename SO3Base<Derived>::Scalar>::eps,
"SO3 assigned data not normalized !",
manif::invalid_argument
);
MANIF_UNUSED_VARIABLE(data);
}
};
//! @brief Cast specialization for SO3Base objects.
template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SO3Base<Derived>, NewScalar> {
template <typename T>
static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
const typename SO3Base<Derived>::QuaternionDataType q = o.quat();
return typename Derived::template LieGroupTemplate<NewScalar>(
q.template cast<NewScalar>().normalized()
);
}
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SO3_BASE_H_ */

95
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so3/SO3_map.h vendored Normal file
View File

@ -0,0 +1,95 @@
#ifndef _MANIF_MANIF_SO3_MAP_H_
#define _MANIF_MANIF_SO3_MAP_H_
#include "manif/impl/so3/SO3.h"
namespace manif {
namespace internal {
//! @brief traits specialization for Eigen Map
template <typename _Scalar>
struct traits< Eigen::Map<SO3<_Scalar>,0> >
: public traits<SO3<_Scalar>>
{
using typename traits<SO3<_Scalar>>::Scalar;
using traits<SO3<Scalar>>::RepSize;
using Base = SO3Base<Eigen::Map<SO3<Scalar>, 0>>;
using DataType = Eigen::Map<Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
//! @brief traits specialization for Eigen Map const
template <typename _Scalar>
struct traits< Eigen::Map<const SO3<_Scalar>,0> >
: public traits<const SO3<_Scalar>>
{
using typename traits<const SO3<_Scalar>>::Scalar;
using traits<const SO3<Scalar>>::RepSize;
using Base = SO3Base<Eigen::Map<const SO3<Scalar>, 0>>;
using DataType = Eigen::Map<const Eigen::Matrix<Scalar, RepSize, 1>, 0>;
};
} /* namespace internal */
} /* namespace manif */
namespace Eigen {
/**
* @brief Specialization of Map for manif::SO3
*/
template <class _Scalar>
class Map<manif::SO3<_Scalar>, 0>
: public manif::SO3Base<Map<manif::SO3<_Scalar>, 0> >
{
using Base = manif::SO3Base<Map<manif::SO3<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
Map(Scalar* coeffs) : data_(coeffs) { }
Map(Map&&) = default;
MANIF_GROUP_MAP_ASSIGN_OP(SO3)
DataType& coeffs() { return data_; }
const DataType& coeffs() const { return data_; }
protected:
DataType data_;
};
/**
* @brief Specialization of Map for const manif::SO3
*/
template <class _Scalar>
class Map<const manif::SO3<_Scalar>, 0>
: public manif::SO3Base<Map<const manif::SO3<_Scalar>, 0> >
{
using Base = manif::SO3Base<Map<const manif::SO3<_Scalar>, 0> >;
public:
MANIF_COMPLETE_GROUP_TYPEDEF
MANIF_INHERIT_GROUP_API
using Base::transform;
using Base::rotation;
Map(const Scalar* coeffs) : data_(coeffs) { }
Map(Map&&) = default;
const DataType& coeffs() const { return data_; }
protected:
const DataType data_;
};
} /* namespace Eigen */
#endif /* _MANIF_MANIF_SO3_MAP_H_ */

33
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/so3/SO3_properties.h vendored Normal file
View File

@ -0,0 +1,33 @@
#ifndef _MANIF_MANIF_SO3_PROPERTIES_H_
#define _MANIF_MANIF_SO3_PROPERTIES_H_
#include "manif/impl/traits.h"
namespace manif {
// Forward declaration
template <typename _Derived> struct SO3Base;
template <typename _Derived> struct SO3TangentBase;
namespace internal {
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SO3Base<_Derived>>
{
static constexpr int Dim = 3; /// @brief Space dimension
static constexpr int DoF = 3; /// @brief Degrees of freedom
};
//! traits specialization
template <typename _Derived>
struct LieGroupProperties<SO3TangentBase<_Derived>>
{
static constexpr int Dim = 3; /// @brief Space dimension
static constexpr int DoF = 3; /// @brief Degrees of freedom
};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_SO3_PROPERTIES_H_ */

978
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/tangent_base.h vendored Normal file
View File

@ -0,0 +1,978 @@
#ifndef _MANIF_MANIF_TANGENT_BASE_H_
#define _MANIF_MANIF_TANGENT_BASE_H_
#include "manif/impl/macro.h"
#include "manif/impl/traits.h"
#include "manif/impl/generator.h"
#include "manif/impl/random.h"
#include "manif/impl/bracket.h"
#include "manif/impl/vee.h"
#include "manif/impl/eigen.h"
#include "manif/constants.h"
#include <tl/optional.hpp>
namespace manif {
/**
* @brief Base class for Lie groups' tangents.
* Defines the minimum common API.
* @see LieGroupBase.
*/
template <class _Derived>
struct TangentBase
{
static constexpr int Dim = internal::traits<_Derived>::Dim;
static constexpr int RepSize = internal::traits<_Derived>::RepSize;
static constexpr int DoF = internal::traits<_Derived>::DoF;
using Scalar = typename internal::traits<_Derived>::Scalar;
using LieGroup = typename internal::traits<_Derived>::LieGroup;
using Tangent = typename internal::traits<_Derived>::Tangent;
using DataType = typename internal::traits<_Derived>::DataType;
using Jacobian = typename internal::traits<_Derived>::Jacobian;
using LieAlg = typename internal::traits<_Derived>::LieAlg;
using InnerWeightsMatrix = Jacobian;
using OptJacobianRef = tl::optional<Eigen::Ref<Jacobian>>;
template <typename _Scalar>
using TangentTemplate = typename internal::traitscast<Tangent, _Scalar>::cast;
protected:
MANIF_DEFAULT_CONSTRUCTOR(TangentBase)
public:
/**
* @brief Assignment operator.
* @param[in] t An element of the same Tangent group.
* @return A reference to this.
*/
_Derived& operator =(const TangentBase& t);
/**
* @brief Assignment operator.
* @param[in] t An element of the same Tangent group.
* @return A reference to this.
*/
template <typename _DerivedOther>
_Derived& operator =(const TangentBase<_DerivedOther>& t);
/**
* @brief Assignment operator.
* @param[in] t A DataType object.
* @return A reference to this.
* @see DataType.
*/
template <typename _EigenDerived>
_Derived& operator =(const Eigen::MatrixBase<_EigenDerived>& v);
//! @brief Access the underlying data by reference
DataType& coeffs();
//! @brief Access the underlying data by const reference
const DataType& coeffs() const;
//! @brief Access the underlying data by pointer
Scalar* data();
//! @brief Access the underlying data by const pointer
const Scalar* data() const;
//! @brief Cast the Tangent object to a copy
//! of a different scalar type
template <class _NewScalar>
TangentTemplate<_NewScalar> cast() const;
// Common Tangent API
/**
* @brief Set the Tangent object this to Zero.
* @return A reference to this.
*/
_Derived& setZero();
/**
* @brief Set the LieGroup object this to a random value.
* @return A reference to this.
*/
_Derived& setRandom();
/**
* @brief Set the Tangent object this from an object in the Lie algebra.
* @param[in] a An object in the Lie algebra.
* @return A reference to this.
*/
template <typename _EigenDerived>
_Derived& setVee(const Eigen::MatrixBase<_EigenDerived>& a);
// Minimum API
// Those functions must be implemented in the Derived class !
/**
* @brief Get the ith basis element of the Lie Algebra.
* @return the ith basis element of the Lie Algebra.
*/
LieAlg generator(const int i) const;
/**
* @brief Get the weight matrix of the Weighted Euclidean inner product,
* relative to the space basis.
* @return the weight matrix.
* @see generator
*/
InnerWeightsMatrix innerWeights() const;
/**
* @brief Get inner product of this and another Tangent
* weighted by W.
* @return The inner product of this and t.
* @note ip = v0' . W . v1
* @see innerWeights()
*/
template <typename _DerivedOther>
Scalar inner(const TangentBase<_DerivedOther>& t) const;
/**
* @brief Get the Euclidean weighted norm.
* @return The Euclidean weighted norm.
* @see innerWeights()
* @see squaredWeightedNorm()
*/
Scalar weightedNorm() const;
/**
* @brief Get the squared Euclidean weighted norm.
* @return The squared Euclidean weighted norm.
* @see innerWeights()
* @see WeightedNorm()
*/
Scalar squaredWeightedNorm() const;
/**
* @brief Hat operator of the Tangent element.
* @return The isomorphic element in the Lie algebra.
* @note See Eq. (10).
*/
LieAlg hat() const;
/**
* @brief Get the Lie group element
* @param[out] -optional- J_m_t Jacobian of the Lie groupe element wrt this.
* @return Associated Lie group element.
* @note This is the exp() map with the argument in vector form.
* @note See Eq. (23).
*/
LieGroup exp(OptJacobianRef J_m_t =
OptJacobianRef{}) const;
/**
* @brief This function is deprecated.
* Please considere using
* @ref exp instead.
*/
MANIF_DEPRECATED
LieGroup retract(OptJacobianRef J_m_t =
OptJacobianRef{}) const;
/**
* @brief Right oplus operation of the Lie group.
* @param[in] t An element of the tangent of the Lie group.
* @param[out] -optional- J_mout_t Jacobian of the oplus operation wrt this.
* @param[out] -optional- J_mout_m Jacobian of the oplus operation wrt group element.
* @return An element of the Lie group.
* @note See Eq. (25).
*/
LieGroup rplus(const LieGroup& m,
OptJacobianRef J_mout_t = {},
OptJacobianRef J_mout_m = {}) const;
/**
* @brief Left oplus operation of the Lie group.
* @param[in] t An element of the tangent of the Lie group.
* @param[out] -optional- J_mout_t Jacobian of the oplus operation wrt this.
* @param[out] -optional- J_mout_m Jacobian of the oplus operation wrt the group element.
* @return An element of the Lie group.
* @note See Eq. (27).
*/
LieGroup lplus(const LieGroup& m,
OptJacobianRef J_mout_t = {},
OptJacobianRef J_mout_m = {}) const;
/**
* @brief An alias for the right oplus operation.
* @see rplus
*/
LieGroup plus(const LieGroup& m,
OptJacobianRef J_mout_t = {},
OptJacobianRef J_mout_m = {}) const;
template <typename _DerivedOther>
Tangent plus(const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_ta = {},
OptJacobianRef J_mout_tb = {}) const;
template <typename _DerivedOther>
Tangent minus(const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_ta = {},
OptJacobianRef J_mout_tb = {}) const;
/**
* @brief Get the right Jacobian.
* @note this is the right Jacobian of @ref exp, what is commonly known as "the right Jacobian".
* @note See Eq. (41) for the right Jacobian of general functions.
* @note See Eqs. (126,143,163,179,191) for implementations of the right Jacobian of @ref exp.
*/
Jacobian rjac() const;
/**
* @brief Get the left Jacobian.
* @note this is the left Jacobian of @ref exp, what is commonly known as "the left Jacobian".
* @note See Eq. (44) for the left Jacobian of general functions.
* @note See Eqs. (126,145,164,179,191) for implementations of the left Jacobian of @ref exp.
*/
Jacobian ljac() const;
/// @note Calls Derived's 'overload'
template <typename U = _Derived>
typename std::enable_if<
internal::has_rjacinv<U>::value,
typename TangentBase<U>::Jacobian>::type rjacinv() const;
/// @note Calls Base default impl
template <typename U = _Derived>
typename std::enable_if<
! internal::has_rjacinv<U>::value,
typename TangentBase<U>::Jacobian>::type rjacinv() const;
/// @note Calls Derived's 'overload'
template <typename U = _Derived>
typename std::enable_if<
internal::has_ljacinv<U>::value,
typename TangentBase<U>::Jacobian>::type ljacinv() const;
/// @note Calls Base default impl
template <typename U = _Derived>
typename std::enable_if<
! internal::has_ljacinv<U>::value,
typename TangentBase<U>::Jacobian>::type ljacinv() const;
/**
* @brief
* @return [description]
*/
Jacobian smallAdj() const;
/**
* @brief Compute the Lie bracket [this,b] in vector form.
*
* @tparam _DerivedOther
* @param b Another tangent object of the same group.
* @return The Lie bracket [this,b] in vector form.
*/
template <typename _DerivedOther>
Tangent bracket(const TangentBase<_DerivedOther>& b) const;
/**
* @brief Evaluate whether this and v are 'close'.
* @details This evaluation is performed element-wise.
* @param[in] v A vector.
* @param[in] eps Threshold for equality copmarison.
* @return true if the Tangent element t is 'close' to this,
* false otherwise.
*/
template <typename _EigenDerived>
bool isApprox(const Eigen::MatrixBase<_EigenDerived>& v,
const Scalar eps = Constants<Scalar>::eps) const;
/**
* @brief Evaluate whether this and t are 'close'.
* @details This evaluation is performed element-wise.
* @param[in] t An element of the same Tangent group.
* @param[in] eps Threshold for equality copmarison.
* @return true if the Tangent element t is 'close' to this,
* false otherwise.
*/
template <typename _DerivedOther>
bool isApprox(const TangentBase<_DerivedOther>& t,
const Scalar eps = Constants<Scalar>::eps) const;
// Some operators
// Copy assignment
template <typename T>
auto operator <<(T&& v)
->decltype( std::declval<DataType>().operator<<(std::forward<T>(v)) );
// Math
//! @brief Equivalent to v * -1.
Tangent operator -() const;
/**
* @brief Left oplus operator.
* @see lplus.
*/
LieGroup operator +(const LieGroup& m) const;
//! @brief In-place plus operator, simple vector in-place plus operation.
template <typename _DerivedOther>
_Derived& operator +=(const TangentBase<_DerivedOther>& t);
//! @brief In-place minus operator, simple vector in-place minus operation.
template <typename _DerivedOther>
_Derived& operator -=(const TangentBase<_DerivedOther>& t);
//! @brief In-place plus operator, simple vector in-place plus operation.
template <typename _EigenDerived>
_Derived& operator +=(const Eigen::MatrixBase<_EigenDerived>& v);
//! @brief In-place minus operator, simple vector in-place minus operation.
template <typename _EigenDerived>
_Derived& operator -=(const Eigen::MatrixBase<_EigenDerived>& v);
//! @brief Multiply the underlying vector with a scalar.
Tangent operator *=(const Scalar scalar);
//! @brief Divide the underlying vector with a scalar.
Tangent operator /=(const Scalar scalar);
//! Access the ith coeffs
auto operator [](const unsigned int i) const -> decltype(coeffs()[i]){
return coeffs()[i];
}
//! Access the ith coeffs
auto operator [](const unsigned int i) -> decltype(coeffs()[i]){
return coeffs()[i];
}
//! @brief The size of the underlying vector
constexpr unsigned int size() const {
return RepSize;
}
// static helpers
//! Static helper the create a Tangent object set to Zero.
static Tangent Zero();
//! Static helper the create a random Tangent object.
static Tangent Random();
//! Static helper to get a Basis of the Lie group.
static LieAlg Generator(const int i);
//! Static helper to get a Basis of the Lie group.
static InnerWeightsMatrix InnerWeights();
/**
* @brief Compute the Lie bracket [a,b] in vector form.
*
* @tparam _DerivedOther
* @param a A Tangent object.
* @param b A second Tangent object.
* @return The Lie bracket [a,b] in vector form.
*/
template <typename _DerivedOther>
static Tangent Bracket(
const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b
);
/**
* @brief Instantiate a Tangent from a Lie algebra object.
*
* @tparam _EigenDerived
* @param alg A tangent object expressed in the Lie algebra.
* @return a Tangent object.
* @see hat
*/
template <typename _EigenDerived>
static Tangent Vee(const Eigen::MatrixBase<_EigenDerived>& alg);
protected:
inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); }
inline const _Derived& derived() const & noexcept { return *static_cast< const _Derived* >(this); }
};
template <typename _Derived>
constexpr int TangentBase<_Derived>::Dim;
template <typename _Derived>
constexpr int TangentBase<_Derived>::DoF;
template <typename _Derived>
constexpr int TangentBase<_Derived>::RepSize;
// Copy
template <typename _Derived>
_Derived&
TangentBase<_Derived>::operator =(const TangentBase& t)
{
coeffs() = t.coeffs();
return derived();
}
template <typename _Derived>
template <typename _DerivedOther>
_Derived&
TangentBase<_Derived>::operator =(const TangentBase<_DerivedOther>& t)
{
coeffs() = t.coeffs();
return derived();
}
template <typename _Derived>
template <typename _EigenDerived>
_Derived&
TangentBase<_Derived>::operator =(const Eigen::MatrixBase<_EigenDerived>& v)
{
coeffs() = v;
return derived();
}
template <typename _Derived>
typename TangentBase<_Derived>::DataType&
TangentBase<_Derived>::coeffs()
{
return derived().coeffs();
}
template <typename _Derived>
const typename TangentBase<_Derived>::DataType&
TangentBase<_Derived>::coeffs() const
{
return derived().coeffs();
}
template <class _Derived>
typename TangentBase<_Derived>::Scalar*
TangentBase<_Derived>::data()
{
return derived().coeffs().data();
}
template <class _Derived>
const typename TangentBase<_Derived>::Scalar*
TangentBase<_Derived>::data() const
{
return derived().coeffs().data();
}
template <typename _Derived>
template <class _NewScalar>
typename TangentBase<_Derived>::template TangentTemplate<_NewScalar>
TangentBase<_Derived>::cast() const
{
return TangentTemplate<_NewScalar>(coeffs().template cast<_NewScalar>());
}
template <class _Derived>
_Derived& TangentBase<_Derived>::setZero()
{
coeffs().setZero();
return derived();
}
template <class _Derived>
_Derived& TangentBase<_Derived>::setRandom()
{
internal::RandomEvaluator<
typename internal::traits<_Derived>::Base>(
derived()).run();
return derived();
}
template <class _Derived>
template <typename _EigenDerived>
_Derived& TangentBase<_Derived>::setVee(const Eigen::MatrixBase<_EigenDerived>& a) {
internal::VeeEvaluator<typename internal::traits<_Derived>::Base>(derived()).run(a);
return derived();
}
template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
return derived().exp(J_m_t);
}
template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
return derived().exp(J_m_t);
}
template <typename _Derived>
typename TangentBase<_Derived>::LieAlg
TangentBase<_Derived>::generator(const int i) const
{
return Generator(i);
}
template <typename _Derived>
typename TangentBase<_Derived>::InnerWeightsMatrix
TangentBase<_Derived>::innerWeights() const
{
return InnerWeights();
}
template <typename _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Scalar
TangentBase<_Derived>::inner(const TangentBase<_DerivedOther>& t) const
{
return (coeffs().transpose() * InnerWeights() * t.coeffs())(0);
}
template <class _Derived>
typename TangentBase<_Derived>::Scalar
TangentBase<_Derived>::weightedNorm() const
{
using std::sqrt;
return sqrt( squaredWeightedNorm() );
}
template <class _Derived>
typename TangentBase<_Derived>::Scalar
TangentBase<_Derived>::squaredWeightedNorm() const
{
return (coeffs().transpose() * InnerWeights() * coeffs())(0);
}
template <class _Derived>
typename TangentBase<_Derived>::LieAlg
TangentBase<_Derived>::hat() const
{
return derived().hat();
}
template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::rplus(const LieGroup& m,
OptJacobianRef J_mout_t,
OptJacobianRef J_mout_m) const
{
return m.rplus(derived(), J_mout_m, J_mout_t);
}
template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::lplus(const LieGroup& m,
OptJacobianRef J_mout_t,
OptJacobianRef J_mout_m) const
{
return m.lplus(derived(), J_mout_m, J_mout_t);
}
template <class _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::plus(const LieGroup& m,
OptJacobianRef J_mout_t,
OptJacobianRef J_mout_m) const
{
return m.lplus(derived(), J_mout_m, J_mout_t);
}
template <class _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::plus(const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_ta,
OptJacobianRef J_mout_tb) const
{
if (J_mout_ta)
J_mout_ta->setIdentity();
if (J_mout_tb)
J_mout_tb->setIdentity();
return *this + t;
}
template <class _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::minus(const TangentBase<_DerivedOther>& t,
OptJacobianRef J_mout_ta,
OptJacobianRef J_mout_tb) const
{
if (J_mout_ta)
J_mout_ta->setIdentity();
if (J_mout_tb)
{
J_mout_tb->setIdentity();
(*J_mout_tb) *= Scalar(-1);
}
return *this - t;
}
template <class _Derived>
typename TangentBase<_Derived>::Jacobian
TangentBase<_Derived>::rjac() const
{
return derived().rjac();
}
template <class _Derived>
typename TangentBase<_Derived>::Jacobian
TangentBase<_Derived>::ljac() const
{
return derived().ljac();
}
template <class _Derived>
template <typename U>
typename std::enable_if<
internal::has_rjacinv<U>::value,
typename TangentBase<U>::Jacobian>::type
TangentBase<_Derived>::rjacinv() const
{
return derived().rjacinv();
}
template <class _Derived>
template <typename U>
typename std::enable_if<
! internal::has_rjacinv<U>::value,
typename TangentBase<U>::Jacobian>::type
TangentBase<_Derived>::rjacinv() const
{
return derived().rjac().inverse();
}
template <class _Derived>
template <typename U>
typename std::enable_if<
internal::has_ljacinv<U>::value,
typename TangentBase<U>::Jacobian>::type
TangentBase<_Derived>::ljacinv() const
{
return derived().ljacinv();
}
template <class _Derived>
template <typename U>
typename std::enable_if<
! internal::has_ljacinv<U>::value,
typename TangentBase<U>::Jacobian>::type
TangentBase<_Derived>::ljacinv() const
{
return derived().ljac().inverse();
}
template <class _Derived>
typename TangentBase<_Derived>::Jacobian
TangentBase<_Derived>::smallAdj() const
{
return derived().smallAdj();
}
template <typename _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::bracket(
const TangentBase<_DerivedOther>& b
) const {
return internal::BracketEvaluator<
typename internal::traits<_Derived>::Base,
typename internal::traits<_DerivedOther>::Base
>(derived(), b.derived()).run();
}
template <typename _Derived>
template <typename _EigenDerived>
bool TangentBase<_Derived>::isApprox(
const Eigen::MatrixBase<_EigenDerived>& t,
const Scalar eps) const
{
using std::min;
bool result = false;
if (min(coeffs().norm(), t.norm()) < eps)
{
result = ((coeffs() - t).isZero(eps));
}
else
{
result = (coeffs().isApprox(t, eps));
}
return result;
}
template <typename _Derived>
template <typename _DerivedOther>
bool TangentBase<_Derived>::isApprox(
const TangentBase<_DerivedOther>& t,
const Scalar eps) const
{
return isApprox(t.coeffs(), eps);
}
// Operators
template <typename _Derived>
template <typename T>
auto TangentBase<_Derived>::operator <<(T&& v)
->decltype( std::declval<DataType>().operator<<(std::forward<T>(v)) )
{
return coeffs().operator<<(std::forward<T>(v));
}
// Static helper
template <class _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::Zero()
{
static const Tangent t(DataType::Zero());
return t;
}
template <class _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::Random()
{
return Tangent().setRandom();
}
template <typename _Derived>
typename TangentBase<_Derived>::LieAlg
TangentBase<_Derived>::Generator(const int i)
{
return internal::GeneratorEvaluator<
typename internal::traits<_Derived>::Base>::run(i);
}
template <typename _Derived>
typename TangentBase<_Derived>::InnerWeightsMatrix
TangentBase<_Derived>::InnerWeights()
{
return internal::InnerWeightsEvaluator<
typename internal::traits<_Derived>::Base>::run();
}
template <typename _Derived>
template <typename _DerivedOther>
typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Bracket(
const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b
) {
return a.bracket(b);
}
template <typename _Derived>
template <typename _EigenDerived>
typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Vee(
const Eigen::MatrixBase<_EigenDerived>& alg
) {
return Tangent().setVee(alg);
}
// Math
template <typename _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::operator -() const
{
return Tangent(-coeffs());
}
template <typename _Derived>
typename TangentBase<_Derived>::LieGroup
TangentBase<_Derived>::operator +(const LieGroup& m) const
{
return m.lplus(derived());
}
template <typename _Derived>
template <typename _DerivedOther>
_Derived& TangentBase<_Derived>::operator +=(
const TangentBase<_DerivedOther>& t)
{
coeffs() += t.coeffs();
return derived();
}
template <typename _Derived>
template <typename _DerivedOther>
_Derived& TangentBase<_Derived>::operator -=(
const TangentBase<_DerivedOther>& t)
{
coeffs() -= t.coeffs();
return derived();
}
template <typename _Derived, typename _DerivedOther>
typename TangentBase<_Derived>::Tangent
operator +(const TangentBase<_Derived>& ta,
const TangentBase<_DerivedOther>& tb)
{
typename TangentBase<_Derived>::Tangent tc(ta);
return tc += tb;
}
template <typename _Derived, typename _DerivedOther>
typename TangentBase<_Derived>::Tangent
operator -(const TangentBase<_Derived>& ta,
const TangentBase<_DerivedOther>& tb)
{
typename TangentBase<_Derived>::Tangent tc(ta);
return tc -= tb;
}
template <typename _Derived>
template <typename _EigenDerived>
_Derived& TangentBase<_Derived>::operator +=(
const Eigen::MatrixBase<_EigenDerived>& v)
{
coeffs() += v;
return derived();
}
template <typename _Derived>
template <typename _EigenDerived>
_Derived& TangentBase<_Derived>::operator -=(
const Eigen::MatrixBase<_EigenDerived>& v)
{
coeffs() -= v;
return derived();
}
template <typename _Derived, typename _EigenDerived>
typename TangentBase<_Derived>::Tangent
operator +(const TangentBase<_Derived>& t,
const Eigen::MatrixBase<_EigenDerived>& v)
{
typename TangentBase<_Derived>::Tangent ret(t);
return ret += v;
}
template <typename _Derived, typename _EigenDerived>
typename TangentBase<_Derived>::Tangent
operator -(const TangentBase<_Derived>& t,
const Eigen::MatrixBase<_EigenDerived>& v)
{
typename TangentBase<_Derived>::Tangent ret(t);
return ret -= v;
}
template <typename _EigenDerived, typename _Derived>
auto
operator +(const Eigen::MatrixBase<_EigenDerived>& v,
const TangentBase<_Derived>& t)
-> decltype(v + t.coeffs())
{
return v + t.coeffs();
}
template <typename _EigenDerived, typename _Derived>
auto
operator -(const Eigen::MatrixBase<_EigenDerived>& v,
const TangentBase<_Derived>& t)
-> decltype(v - t.coeffs())
{
return v - t.coeffs();
}
template <typename _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::operator *=(const Scalar scalar)
{
coeffs() *= scalar;
return derived();
}
template <typename _Derived>
typename TangentBase<_Derived>::Tangent
TangentBase<_Derived>::operator /=(const Scalar scalar)
{
coeffs() /= scalar;
return derived();
}
template <typename _Derived>
typename TangentBase<_Derived>::Tangent
operator *(const TangentBase<_Derived>& t,
const typename _Derived::Scalar scalar)
{
typename TangentBase<_Derived>::Tangent ret(t);
return ret *= scalar;
}
template <typename _Derived>
typename TangentBase<_Derived>::Tangent
operator *(const typename _Derived::Scalar scalar,
const TangentBase<_Derived>& t)
{
return t * scalar;
}
template <typename _Derived>
typename TangentBase<_Derived>::Tangent
operator /(const TangentBase<_Derived>& t,
const typename _Derived::Scalar scalar)
{
typename TangentBase<_Derived>::Tangent ret(t);
return ret /= scalar;
}
template <class _DerivedOther>
typename TangentBase<_DerivedOther>::Tangent
operator *(const typename TangentBase<_DerivedOther>::Jacobian& J,
const TangentBase<_DerivedOther>& t)
{
return typename TangentBase<_DerivedOther>::Tangent(
typename TangentBase<_DerivedOther>::DataType(J*t.coeffs()));
}
template <typename _Derived, typename _DerivedOther>
bool operator ==(
const TangentBase<_Derived>& ta,
const TangentBase<_DerivedOther>& tb)
{
return ta.isApprox(tb);
}
template <typename _Derived, typename _EigenDerived>
bool operator ==(
const TangentBase<_Derived>& t,
const Eigen::MatrixBase<_EigenDerived>& v)
{
return t.isApprox(v);
}
template <typename _Derived, typename _DerivedOther>
bool operator !=(
const TangentBase<_Derived>& ta,
const TangentBase<_DerivedOther>& tb)
{
return !(ta == tb);
}
template <typename _Derived, typename _EigenDerived>
bool operator !=(
const TangentBase<_Derived>& t,
const Eigen::MatrixBase<_EigenDerived>& v)
{
return !(t == v);
}
// Utils
template <typename _Stream, typename _Derived>
_Stream& operator << (
_Stream& s,
const manif::TangentBase<_Derived>& m)
{
s << m.coeffs().transpose();
return s;
}
} /* namespace manif */
#endif /* _MANIF_MANIF_TANGENT_BASE_H_ */

181
dependency/x86/third_party/mainif/0.0.5/include/manif/impl/traits.h vendored Normal file
View File

@ -0,0 +1,181 @@
#ifndef _MANIF_MANIF_TRAITS_H_
#define _MANIF_MANIF_TRAITS_H_
#include <array>
#include <type_traits>
namespace manif {
template <typename Derived> struct LieGroupBase;
template <typename Derived> struct TangentBase;
namespace internal {
//template <typename T>
//inline constexpr bool constexpr_false() {
// return false;
//}
template <typename T>
struct constexpr_false : std::false_type {};
template <typename T> struct traits;
/// @note the following is from the Eigen library
/// here we say once and for all that traits<const T> == traits<T>
///
/// When constness must affect traits, it has to be constness on
/// template parameters on which T itself depends.
/// For example, traits<Map<const T> > != traits<Map<T> >, but
/// traits<const Map<T> > == traits<Map<T> >
template<typename T> struct traits<const T> : traits<T> {};
/**
* @brief LieGroupProperties, a traits for defining some
* group properties. E.g. Space dimension, Degrees of Freedom ...
*/
template <typename _LieGroupBase> struct LieGroupProperties;
//! @brief A traits helper to cast
//!
//! @note e.g. template <typename NewScalar> using LieGroupTemplate = typename traitscast<LieGroup, NewScalar>::cast;
template <typename _Class, typename _NewScalar>
struct traitscast;
/**
* @brief Traits to change the scalar type of a template class
* @note given using FooDouble = Foo<double, Ts...>
* using FooFloat = typename traitscast<FooDouble, float>::cast;
*/
template <
template <typename, template<typename> class...> class _Class,
typename _Scalar,
typename _NewScalar,
template<typename> class ... Ts>
struct traitscast<_Class<_Scalar, Ts...>, _NewScalar>
{
using cast = _Class<_NewScalar, Ts...>;
};
/**
* @brief Specialization for Rn.
*/
template <
template <typename, unsigned int> class _Class,
typename _NewScalar,
typename _Scalar,
unsigned int _Dim>
struct traitscast<_Class<_Scalar, _Dim>, _NewScalar>
{
using cast = _Class<_NewScalar, _Dim>;
};
//! @brief A traits for detecting AutoDiff scalar types
template <typename Scalar> struct is_ad : std::integral_constant<bool, false> { };
//! @brief Has function 'rjacinv' traits
template<class, typename T> struct has_rjacinv_impl : std::false_type {};
template<typename T> struct
has_rjacinv_impl<decltype( void(std::declval<T>().rjacinv()) ), T> : std::true_type {};
template<typename T> struct has_rjacinv : has_rjacinv_impl<void, T> {};
//! @brief Has function 'ljacinv' traits
template<class, typename T> struct has_ljacinv_impl : std::false_type {};
template<typename T> struct
has_ljacinv_impl<decltype( void(std::declval<T>().ljacinv()) ), T> : std::true_type {};
template<typename T> struct has_ljacinv : has_ljacinv_impl<void, T> {};
/**
* @brief intseq: std::integer_sequence-equivalent
*/
template<int ... _I>
struct intseq
{
using value_type = int;
static constexpr std::size_t size() noexcept {return sizeof...(_I);}
};
/**
* @brief concatenate two intseqs
*/
template<typename _Seq1, typename _Seq2>
struct intseq_join;
template<template<int ...> class _IntSeq, int ... _I1, int ... _I2>
struct intseq_join<_IntSeq<_I1...>, _IntSeq<_I2...>>
{
using type = _IntSeq<_I1..., _I2...>;
};
/**
* @brief generate intseq 0, 1, ..., N-1
*/
template<std::size_t _N>
struct make_intseq
{
using type = typename intseq_join<
typename make_intseq<_N - 1>::type, intseq<_N - 1>
>::type;
};
template<>
struct make_intseq<0>
{
using type = intseq<>;
};
template <std::size_t _N>
using make_intseq_t = typename make_intseq<_N>::type;
template <typename T>
constexpr T accumulate(T t) {
return t;
}
template <typename T, typename... Rest>
constexpr T accumulate(T t, Rest... rest) {
return t + accumulate(rest...);
}
template <int N, int i, int j, int ...Args> struct compute_indices_gen {
static constexpr std::array<int, sizeof...(Args)+1> get() {
return compute_indices_gen<N-1, i+j, Args..., i+j>::get();
}
};
template <int i, int j, int ...Args> struct compute_indices_gen<1, i, j, Args...> {
static constexpr std::array<int, sizeof...(Args)+1> get() {
return { { 0, Args... } };
}
};
//! @brief Compute indices given block-sizes
template <int... Args> constexpr std::array<int, sizeof...(Args)> compute_indices() {
return compute_indices_gen<sizeof...(Args), 0, Args...>::get();
}
// traits to check if a template parameter has a LieGroupBase base.
template <typename Derived>
void test_lie_group_base(LieGroupBase<Derived>&& s) {}
template <class, typename T> struct is_manif_group_impl : std::false_type {};
template <typename T> struct
is_manif_group_impl<decltype(test_lie_group_base(std::declval<T>())), T>
: std::true_type {};
template <typename T> struct is_manif_group
: is_manif_group_impl<void, T> {};
// traits to check if a template parameter has a TangentBase base.
template <typename Derived>
void test_tangent_base(TangentBase<Derived>&& s) {}
template <class, typename T> struct is_manif_tangent_impl : std::false_type {};
template <typename T> struct
is_manif_tangent_impl<decltype(test_tangent_base(std::declval<T>())), T>
: std::true_type {};
template <typename T> struct is_manif_tangent
: is_manif_tangent_impl<void, T> {};
} /* namespace internal */
} /* namespace manif */
#endif /* _MANIF_MANIF_TRAITS_H_ */

Some files were not shown because too many files have changed in this diff Show More