feat:add ik closed-from solution
This commit is contained in:
parent
7d03c0eeaf
commit
5afd63eabe
@ -64,6 +64,7 @@ include_directories(
|
||||
${PROTO_BINARY_DIR}
|
||||
${PROJECT_SOURCE_DIR}/include
|
||||
${PROJECT_SOURCE_DIR}/src/devices
|
||||
${PROJECT_SOURCE_DIR}/src/utils
|
||||
${PROJECT_SOURCE_DIR}/third_party
|
||||
)
|
||||
|
||||
|
||||
1853
data/ik_psi_sweep.csv
Normal file
1853
data/ik_psi_sweep.csv
Normal file
File diff suppressed because it is too large
Load Diff
59
data/plot_data.py
Normal file
59
data/plot_data.py
Normal file
@ -0,0 +1,59 @@
|
||||
import pandas as pd
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
df = pd.read_csv('/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv')
|
||||
|
||||
joints = ['q1','q2','q3','q4','q5','q6','q7']
|
||||
fig, axes = plt.subplots(len(joints), 1, figsize=(10, 2.6*len(joints)), sharex=True)
|
||||
x = df['psi'].to_numpy()
|
||||
|
||||
for ax, col in zip(axes, joints):
|
||||
y = df[col].to_numpy()
|
||||
|
||||
# 过滤 NaN
|
||||
mask = ~np.isnan(x) & ~np.isnan(y)
|
||||
xv, yv = x[mask], y[mask]
|
||||
if len(xv) == 0:
|
||||
ax.set_title(f'{col}: no data')
|
||||
continue
|
||||
|
||||
# 散点图（不画连线）
|
||||
ax.scatter(xv, yv, s=10, alpha=0.8) # s 调整点大小
|
||||
|
||||
# 极值与对应 psi
|
||||
i_min = int(np.argmin(yv))
|
||||
i_max = int(np.argmax(yv))
|
||||
y_min, psi_min = float(yv[i_min]), float(xv[i_min])
|
||||
y_max, psi_max = float(yv[i_max]), float(xv[i_max])
|
||||
|
||||
# 水平红虚线（值）
|
||||
ax.axhline(y_min, color='red', linestyle='--', linewidth=1)
|
||||
ax.axhline(y_max, color='red', linestyle='--', linewidth=1)
|
||||
# 垂直红虚线（psi）
|
||||
ax.axvline(psi_min, color='red', linestyle='--', linewidth=1, alpha=0.7)
|
||||
ax.axvline(psi_max, color='red', linestyle='--', linewidth=1, alpha=0.7)
|
||||
|
||||
# 极值点标记
|
||||
ax.plot(psi_min, y_min, 'ro', markersize=4)
|
||||
ax.plot(psi_max, y_max, 'ro', markersize=4)
|
||||
|
||||
# 标注：值 + psi
|
||||
ax.annotate(f"min={y_min:.4f}\nψ={psi_min:.4f}",
|
||||
xy=(psi_min, y_min), xytext=(8, -10),
|
||||
textcoords='offset points', color='red',
|
||||
ha='left', va='top', fontsize=9,
|
||||
bbox=dict(boxstyle="round,pad=0.2", fc="white", ec="none"))
|
||||
ax.annotate(f"max={y_max:.4f}\nψ={psi_max:.4f}",
|
||||
xy=(psi_max, y_max), xytext=(8, 10),
|
||||
textcoords='offset points', color='red',
|
||||
ha='left', va='bottom', fontsize=9,
|
||||
bbox=dict(boxstyle="round,pad=0.2", fc="white", ec="none"))
|
||||
|
||||
ax.set_ylabel(f"{col} (rad)")
|
||||
ax.grid(True, alpha=0.35)
|
||||
|
||||
axes[-1].set_xlabel('psi (rad)')
|
||||
fig.suptitle('IK joints vs arm-angle psi', y=0.995)
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
@ -20,6 +20,8 @@ add_library(utils STATIC
|
||||
math/se3.cpp
|
||||
math/so3.cpp
|
||||
controller/cartesian_controller.cpp
|
||||
srs_ik/srs_ik_slover.cpp
|
||||
srs_ik/ik_limit_analyzer.cpp
|
||||
)
|
||||
|
||||
target_include_directories(utils PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@ -32,3 +34,48 @@ target_link_libraries(utils PRIVATE
|
||||
)
|
||||
|
||||
add_library(cmvr_es::utils ALIAS utils)
|
||||
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Unit test
|
||||
# --------------------------------------------------------
|
||||
find_package(glog REQUIRED)
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(fcl REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
|
||||
)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/manif/0.0.5/include
|
||||
)
|
||||
|
||||
|
||||
link_directories(
|
||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
add_executable(srs_ik_test
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/srs_ik/srs_ik_test.cpp
|
||||
)
|
||||
|
||||
|
||||
target_link_libraries(srs_ik_test
|
||||
PRIVATE
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog::glog
|
||||
proto-objects
|
||||
ccd
|
||||
fcl
|
||||
${OpenCV_LIBS}
|
||||
Eigen3::Eigen
|
||||
OsqpEigen::OsqpEigen
|
||||
cmvr_es::utils
|
||||
)
|
||||
|
||||
629
src/utils/srs_ik/ik_limit_analyzer.cpp
Normal file
629
src/utils/srs_ik/ik_limit_analyzer.cpp
Normal file
@ -0,0 +1,629 @@
|
||||
//
|
||||
// Created by lgv on 2025/11/3.
|
||||
//
|
||||
|
||||
#include "ik_limit_analyzer.h"
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
using namespace cmvr::utils;
|
||||
|
||||
double IkLimitAnalyzer::normalize_angle(double angle) {
|
||||
// 将角度转换为 [0, 2π) 范围
|
||||
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
|
||||
|
||||
// 如果 a 为负数，则添加 2π，使其位于 [0, 2π) 范围
|
||||
if (a < 0.0) {
|
||||
a += 2.0 * M_PI;
|
||||
}
|
||||
|
||||
// 将角度范围调整到 [-π, π] 范围
|
||||
return a - M_PI;
|
||||
}
|
||||
|
||||
bool IkLimitAnalyzer::check_tan_solution(double an, double ad, double bn, double bd, double cn, double cd,
|
||||
double theta_target, double psi) {
|
||||
// 由 ψ 复原 θ：θ = atan2( N, D )
|
||||
// N = an*sinψ + bn*cosψ + cn
|
||||
// D = ad*sinψ + bd*cosψ + cd
|
||||
double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
|
||||
double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
|
||||
double theta = std::atan2(N, D);
|
||||
double err = normalize_angle(theta - theta_target);
|
||||
return std::abs(err) < 1e-6;
|
||||
}
|
||||
|
||||
|
||||
std::vector<double> IkLimitAnalyzer::calc_tan_solution(double an, double ad, double bn, double bd, double cn,
|
||||
double cd, double theta) {
|
||||
std::vector<double> out;
|
||||
|
||||
// v = tan(theta)
|
||||
double v = std::tan(theta);
|
||||
|
||||
// 二次式：ap * t^2 + bp * t + cp = 0， 其中 t = tan(ψ/2)
|
||||
double ap = v * (cd - bd) + (bn - cn);
|
||||
double bp = v * (2.0 * ad) - (2.0 * an);
|
||||
double cp = v * (bd + cd) - (bn + cn);
|
||||
|
||||
double D = bp * bp - 4.0 * ap * cp;
|
||||
if (D < 0.0) return out;
|
||||
D = std::max(0.0, D);
|
||||
double sqrtD = std::sqrt(D);
|
||||
|
||||
// ψ = 2 * atan2( -(bp ± sqrtD), 2*ap )
|
||||
double psi1 = 2.0 * std::atan2(-(bp - sqrtD), 2.0 * ap);
|
||||
double psi2 = 2.0 * std::atan2(-(bp + sqrtD), 2.0 * ap);
|
||||
|
||||
psi1 = normalize_angle(psi1);
|
||||
psi2 = normalize_angle(psi2);
|
||||
|
||||
|
||||
if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi1))
|
||||
out.push_back(psi1);
|
||||
|
||||
if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi2) &&
|
||||
std::abs(psi2 - psi1) > EPS)
|
||||
out.push_back(psi2);
|
||||
|
||||
std::sort(out.begin(), out.end());
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
// std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double an, double ad, double bn, double bd,
|
||||
// double cn, double cd, double joint_l,
|
||||
// double joint_u) {
|
||||
// // joint_l = normalize_angle(joint_l);
|
||||
// // joint_u = normalize_angle(joint_u);
|
||||
//
|
||||
//
|
||||
// // 1) 微分系数（保持与 MATLAB 一致）
|
||||
// double at = bd * cn - bn * cd;
|
||||
// double bt = an * cd - ad * cn;
|
||||
// double ct = an * bd - ad * bn;
|
||||
//
|
||||
// // 2) 奇异屏蔽带（式 31）：at^2 + bt^2 - ct^2 = 0
|
||||
// double dt = at * at + bt * bt - ct * ct;
|
||||
// std::vector<std::pair<double, double> > singular_allow; // 允许区间列表
|
||||
// if (std::abs(at * at + bt * bt - ct * ct) < 1e-6) {
|
||||
// double psi_sing = normalize_angle(2.0 * std::atan2(at, (bt - ct)));
|
||||
// double safe = deg2rad(7.0);
|
||||
//
|
||||
// double L = normalize_angle(psi_sing - safe);
|
||||
// double R = normalize_angle(psi_sing + safe);
|
||||
//
|
||||
// // 允许集 = [-π, L] ∪ [R, π]
|
||||
// if (L <= R) {
|
||||
// singular_allow = {{-M_PI, L}, {R, M_PI}};
|
||||
// } else {
|
||||
// // 屏蔽带跨越 -π/π
|
||||
// singular_allow = {{-M_PI, R}, {L, M_PI}};
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// // 3) 将上下限 ±jl 映射为 psi（tan 型）
|
||||
// std::vector<double> pt1 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_u);
|
||||
// std::vector<double> pt2 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_l);
|
||||
//
|
||||
// std::vector<double> ptlim;
|
||||
// ptlim.reserve(pt1.size() + pt2.size());
|
||||
// ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
|
||||
// ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
|
||||
//
|
||||
// std::vector<std::pair<double, double> > allow_pairs; // 最终允许区间
|
||||
//
|
||||
// if (!ptlim.empty()) {
|
||||
// // 3.1 排序边界
|
||||
// std::sort(ptlim.begin(), ptlim.end());
|
||||
//
|
||||
// // 3.2 分类：区间结束边界(=1) / 区间开始边界(=0)
|
||||
// std::vector<int> lim_class(ptlim.size(), 0);
|
||||
// for (size_t i = 0; i < ptlim.size(); ++i) {
|
||||
// double psi = ptlim[i];
|
||||
//
|
||||
// // tlim = atan2(N, D)
|
||||
// double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
|
||||
// double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
|
||||
// double tlim = std::atan2(N, D);
|
||||
//
|
||||
// // dθ/dψ 符号
|
||||
// double dlim = at * std::sin(psi) + bt * std::cos(psi) + ct;
|
||||
//
|
||||
// lim_class[i] = (sign(tlim) == sign(dlim)) ? 1 : 0;
|
||||
// }
|
||||
//
|
||||
// // 3.3 若首个是“进入禁止”，拼接 [-π, ... , π] 让边界从“允许”开始
|
||||
// std::vector<double> bounds = ptlim;
|
||||
// if (!lim_class.empty() && lim_class.front() == 1) {
|
||||
// bounds.insert(bounds.begin(), -M_PI);
|
||||
// bounds.push_back(M_PI);
|
||||
// }
|
||||
//
|
||||
// // 3.4 边界数组 → pair 列表
|
||||
// allow_pairs = bounds_to_pairs(bounds);
|
||||
// } else {
|
||||
// // 4) 没有任何 ψ 命中关节限：全允许或全禁止
|
||||
// double psi = 0.0;
|
||||
// double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
|
||||
// double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
|
||||
// double tlim = std::atan2(N, D);
|
||||
//
|
||||
// if (tlim > joint_l && tlim < joint_u) {
|
||||
// allow_pairs = {{-M_PI, M_PI}}; // 全允许
|
||||
// } else {
|
||||
// allow_pairs.clear(); // 全禁止
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// // 5) 若有奇异屏蔽带：做交集
|
||||
// if (!singular_allow.empty() && !allow_pairs.empty()) {
|
||||
// allow_pairs = intersect(allow_pairs, singular_allow);
|
||||
// }
|
||||
// return allow_pairs;
|
||||
// }
|
||||
|
||||
|
||||
std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double an, double ad, double bn, double bd,
|
||||
double cn, double cd, double joint_l,
|
||||
double joint_u) {
|
||||
// 1) 微分系数
|
||||
double at = bd * cn - bn * cd;
|
||||
double bt = an * cd - ad * cn;
|
||||
double ct = an * bd - ad * bn;
|
||||
|
||||
// 2) 奇异屏蔽带（式 31）：at^2 + bt^2 - ct^2 = 0
|
||||
double dt = at * at + bt * bt - ct * ct;
|
||||
std::vector<std::pair<double, double> > singular_allow; // 允许区间列表
|
||||
if (std::abs(at * at + bt * bt - ct * ct) < 1e-6) {
|
||||
double psi_sing = normalize_angle(2.0 * std::atan2(at, (bt - ct)));
|
||||
double safe = deg2rad(7.0);
|
||||
|
||||
double L = normalize_angle(psi_sing - safe);
|
||||
double R = normalize_angle(psi_sing + safe);
|
||||
|
||||
// 允许集 = [-π, L] ∪ [R, π]
|
||||
if (L <= R) {
|
||||
singular_allow = {{-M_PI, L}, {R, M_PI}};
|
||||
} else {
|
||||
// 屏蔽带跨越 -π/π
|
||||
singular_allow = {{-M_PI, R}, {L, M_PI}};
|
||||
}
|
||||
}
|
||||
|
||||
// 3) 将上下限 ±jl 映射为 psi（tan 型）
|
||||
std::vector<double> pt1 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_u);
|
||||
std::vector<double> pt2 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_l);
|
||||
|
||||
std::vector<double> ptlim;
|
||||
ptlim.reserve(pt1.size() + pt2.size());
|
||||
ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
|
||||
ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
|
||||
|
||||
|
||||
// 去重（避免重复切分点导致抖动）
|
||||
std::sort(ptlim.begin(), ptlim.end());
|
||||
ptlim.erase(std::unique(ptlim.begin(), ptlim.end(),
|
||||
[](double a, double b) { return std::abs(a - b) < 1e-12; }),
|
||||
ptlim.end());
|
||||
|
||||
std::vector<std::pair<double, double> > allow_pairs; // 最终允许区间
|
||||
|
||||
if (!ptlim.empty()) {
|
||||
// === 采样 + 交替（不再用同号判据/首段拼 [-π,π]） ===
|
||||
const double EPSP = 1e-9;
|
||||
|
||||
// 构造边界：[-π, cuts..., π]
|
||||
std::vector<double> bounds;
|
||||
bounds.reserve(ptlim.size() + 2);
|
||||
bounds.push_back(-M_PI);
|
||||
bounds.insert(bounds.end(), ptlim.begin(), ptlim.end());
|
||||
bounds.push_back(M_PI);
|
||||
|
||||
// 左侧微偏移采样，判断首段是否“允许”
|
||||
auto theta_of = [&](double psi) {
|
||||
double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
|
||||
double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
|
||||
return std::atan2(N, D);
|
||||
};
|
||||
double probe = bounds.front() + EPSP; // -π+ε
|
||||
bool allow_here = angle_in_wrap(theta_of(probe), joint_l, joint_u);
|
||||
|
||||
// 扫描生成段
|
||||
for (size_t i = 0; i + 1 < bounds.size(); ++i) {
|
||||
double L = bounds[i];
|
||||
double R = bounds[i + 1];
|
||||
if (allow_here && R > L) allow_pairs.emplace_back(L, R);
|
||||
allow_here = !allow_here; // 每越过一个边界，翻转一次，前提是θ(ψ) 连续变换
|
||||
// 也可以通过 L + EPSP 来判断该区间是否允许
|
||||
// allow_here = angle_in_wrap(theta_of(L + EPSP), joint_l, joint_u);
|
||||
}
|
||||
} else {
|
||||
// 4) 无交点：整圈全允许或全禁止（用环形比较）
|
||||
double psi = 0.0;
|
||||
double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
|
||||
double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
|
||||
double tlim = std::atan2(N, D);
|
||||
|
||||
if (angle_in_wrap(tlim, joint_l, joint_u)) {
|
||||
allow_pairs = {{-M_PI, M_PI}};
|
||||
} else {
|
||||
allow_pairs.clear();
|
||||
}
|
||||
}
|
||||
|
||||
// 5) 奇异屏蔽：与允许集求交
|
||||
if (!singular_allow.empty() && !allow_pairs.empty()) {
|
||||
allow_pairs = intersect(allow_pairs, singular_allow);
|
||||
}
|
||||
|
||||
// ++【新增】合并小段，干净输出
|
||||
allow_pairs = union_intervals(allow_pairs);
|
||||
return allow_pairs;
|
||||
}
|
||||
|
||||
|
||||
std::vector<std::pair<double, double> > IkLimitAnalyzer::intersect(
|
||||
const std::vector<std::pair<double, double> > &A, const std::vector<std::pair<double, double> > &B) {
|
||||
if (A.empty() || B.empty()) return {};
|
||||
|
||||
// 先复制并排序（按起点）
|
||||
auto SA = A, SB = B;
|
||||
std::sort(SA.begin(), SA.end(),
|
||||
[](auto &x, auto &y) { return x.first < y.first; });
|
||||
std::sort(SB.begin(), SB.end(),
|
||||
[](auto &x, auto &y) { return x.first < y.first; });
|
||||
|
||||
// 双指针求交
|
||||
std::vector<std::pair<double, double> > out;
|
||||
size_t i = 0, j = 0;
|
||||
while (i < SA.size() && j < SB.size()) {
|
||||
double L = std::max(SA[i].first, SB[j].first);
|
||||
double R = std::min(SA[i].second, SB[j].second);
|
||||
if (R > L) out.emplace_back(L, R);
|
||||
|
||||
// 谁先结束谁前进
|
||||
if (SA[i].second < SB[j].second) ++i;
|
||||
else ++j;
|
||||
}
|
||||
|
||||
// 合并可能相邻/重叠的小段
|
||||
if (out.empty()) return out;
|
||||
|
||||
constexpr double MERGE_EPS = 1e-12;
|
||||
std::vector<std::pair<double, double> > merged;
|
||||
merged.reserve(out.size());
|
||||
std::sort(out.begin(), out.end(),
|
||||
[](auto &x, auto &y) { return x.first < y.first; });
|
||||
|
||||
merged.push_back(out[0]);
|
||||
for (size_t k = 1; k < out.size(); ++k) {
|
||||
if (out[k].first <= merged.back().second + MERGE_EPS) {
|
||||
merged.back().second = std::max(merged.back().second, out[k].second);
|
||||
} else {
|
||||
merged.push_back(out[k]);
|
||||
}
|
||||
}
|
||||
return merged;
|
||||
}
|
||||
|
||||
std::vector<std::pair<double, double> > IkLimitAnalyzer::bounds_to_pairs(const std::vector<double> &bounds) {
|
||||
std::vector<std::pair<double, double> > segs;
|
||||
if (bounds.empty()) return segs;
|
||||
|
||||
// 要求：bounds 已按升序，且“以允许开始”（在 TanJointLimits 里保证了）
|
||||
if (bounds.size() % 2 != 0) {
|
||||
// 若出现奇偶不配，可按需抛异常或容错
|
||||
return segs;
|
||||
}
|
||||
|
||||
segs.reserve(bounds.size() / 2);
|
||||
for (size_t i = 0; i < bounds.size(); i += 2) {
|
||||
double L = bounds[i];
|
||||
double R = bounds[i + 1];
|
||||
if (R > L) segs.emplace_back(L, R);
|
||||
}
|
||||
return segs;
|
||||
}
|
||||
|
||||
|
||||
// std::vector<std::pair<double, double> >
|
||||
// IkLimitAnalyzer::calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u) {
|
||||
// // 对于 cos 型来说，奇异点处导数存在，但是左导数 != 右导数
|
||||
// // 论文 Analytical Inverse Kinematic Computation for 7-DOF Redundant
|
||||
// // Manipulators With Joint Limits and Its Application to Redundancy Resolution
|
||||
// // 式 39 ，40 来求奇异点
|
||||
// if (std::abs(a * a + b * b - (c - 1) * (c - 1)) < EPS) {
|
||||
// double psi_sing = 2.0 * std::atan2(a, (b - (c - 1)));
|
||||
// }
|
||||
// if (std::abs(a * a + b * b - (c + 1) * (c + 1)) < EPS) {
|
||||
// double psi_sing = 2.0 * std::atan2(a, (b - (c + 1)));
|
||||
// }
|
||||
//
|
||||
// // 1) 将关节上下限 映射到 ψ（cos 型）
|
||||
// std::vector<double> pt1 = calc_cos_solution(a, b, c, joint_l);
|
||||
// std::vector<double> pt2 = calc_cos_solution(a, b, c, joint_u);
|
||||
//
|
||||
// std::vector<double> ptlim;
|
||||
// ptlim.reserve(pt1.size() + pt2.size());
|
||||
// ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
|
||||
// ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
|
||||
//
|
||||
// std::vector<std::pair<double, double> > allow_pairs; // 输出
|
||||
//
|
||||
// if (!ptlim.empty()) {
|
||||
// // 2) 排序
|
||||
// std::sort(ptlim.begin(), ptlim.end());
|
||||
//
|
||||
// // 3) 分类：enter_avoid(=1) / enter_allow(=0)
|
||||
// std::vector<int> lim_class(ptlim.size(), 0);
|
||||
// for (size_t i = 0; i < ptlim.size(); ++i) {
|
||||
// double psi = ptlim[i];
|
||||
//
|
||||
// // θ(ψ) = conf * acos( a sinψ + b cosψ + c )
|
||||
// double ct = a * std::sin(psi) + b * std::cos(psi) + c;
|
||||
// ct = clamp(ct, -1.0, 1.0);
|
||||
// double tlim = static_cast<double>(conf) * std::acos(ct);
|
||||
//
|
||||
// // dθ/dψ = (-1/sinθ)*(a cosψ - b sinψ)，sinθ = sqrt(1-ct^2)
|
||||
// double st = std::sqrt(std::max(0.0, 1.0 - ct * ct));
|
||||
//
|
||||
// double core = (a * std::cos(psi) - b * std::sin(psi));
|
||||
// double dlim = static_cast<double>(conf) * (-1.0 / st) * core;
|
||||
//
|
||||
// // 当 psi = 0 时，处于奇异点，st = 0 其左右导数值可以参考论文
|
||||
// // Analytical Inverse Kinematic Computation for 7-DOF Redundant
|
||||
// // Manipulators With Joint Limits and Its Application to Redundancy Resolution
|
||||
// // 式 42 ，43，45，46
|
||||
//
|
||||
// lim_class[i] = (sign(tlim) == sign(dlim)) ? 1 : 0;
|
||||
// }
|
||||
//
|
||||
// // 4) 若首个是“进入禁止”，拼上 [-π, π] 让边界以“允许”开头
|
||||
// std::vector<double> bounds = ptlim;
|
||||
// if (!lim_class.empty() && lim_class.front() == 1) {
|
||||
// bounds.insert(bounds.begin(), -M_PI);
|
||||
// bounds.push_back(M_PI);
|
||||
// }
|
||||
//
|
||||
// // 5) 边界 → pair 列表
|
||||
// allow_pairs = bounds_to_pairs(bounds);
|
||||
// } else {
|
||||
// // 6) 无命中边界：全允许或全禁止（检查 ψ=0）
|
||||
// double psi = 0.0;
|
||||
// double ct0 = a * std::sin(psi) + b * std::cos(psi) + c;
|
||||
// ct0 = clamp(ct0, -1.0, 1.0);
|
||||
// double tlim0 = static_cast<double>(conf) * std::acos(ct0);
|
||||
//
|
||||
// if (tlim0 > joint_l && tlim0 < joint_u) {
|
||||
// allow_pairs = {{-M_PI, M_PI}}; // 全允许
|
||||
// } else {
|
||||
// allow_pairs.clear(); // 全禁止
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// return allow_pairs;
|
||||
// }
|
||||
|
||||
|
||||
std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
|
||||
double a, double b, double c, int conf, double joint_l, double joint_u) {
|
||||
// 1) 奇异点判断
|
||||
if (std::abs(a * a + b * b - (c - 1) * (c - 1)) < EPS) {
|
||||
double psi_sing = 2.0 * std::atan2(a, (b - (c - 1)));
|
||||
}
|
||||
if (std::abs(a * a + b * b - (c + 1) * (c + 1)) < EPS) {
|
||||
double psi_sing = 2.0 * std::atan2(a, (b - (c + 1)));
|
||||
}
|
||||
|
||||
// 2) 将关节上下限映射到 ψ（cos 型）
|
||||
std::vector<double> pt1 = calc_cos_solution(a, b, c, joint_l);
|
||||
std::vector<double> pt2 = calc_cos_solution(a, b, c, joint_u);
|
||||
|
||||
std::vector<double> ptlim;
|
||||
ptlim.reserve(pt1.size() + pt2.size());
|
||||
ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
|
||||
ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
|
||||
|
||||
|
||||
// 去重（避免重复切分点导致抖动）
|
||||
std::sort(ptlim.begin(), ptlim.end());
|
||||
ptlim.erase(std::unique(ptlim.begin(), ptlim.end(),
|
||||
[](double a, double b) { return std::abs(a - b) < 1e-12; }),
|
||||
ptlim.end());
|
||||
|
||||
std::vector<std::pair<double, double> > allow_pairs; // 输出
|
||||
|
||||
if (!ptlim.empty()) {
|
||||
// 3) 排序区间
|
||||
std::sort(ptlim.begin(), ptlim.end());
|
||||
|
||||
// 4) 采样 + 交替
|
||||
const double EPSP = 1e-9; // 偏移量
|
||||
|
||||
// 构造边界数组：[-π, cuts..., π]
|
||||
std::vector<double> bounds;
|
||||
bounds.reserve(ptlim.size() + 2);
|
||||
bounds.push_back(-M_PI);
|
||||
bounds.insert(bounds.end(), ptlim.begin(), ptlim.end());
|
||||
bounds.push_back(M_PI);
|
||||
|
||||
// 5) 采样左侧点，判断是否允许
|
||||
auto theta_of = [&](double psi) {
|
||||
double ct = a * std::sin(psi) + b * std::cos(psi) + c;
|
||||
ct = clamp(ct, -1.0, 1.0); // 保证 ct 的范围在 [-1, 1] 之间
|
||||
return static_cast<double>(conf) * std::acos(ct);
|
||||
};
|
||||
|
||||
|
||||
for (size_t i = 0; i + 1 < bounds.size(); ++i) {
|
||||
double L = bounds[i];
|
||||
double R = bounds[i + 1];
|
||||
if (R <= L) continue;
|
||||
|
||||
double probe = L + EPSP; // 左侧微偏移
|
||||
if (probe > R) probe = (L + R) * 0.5; // 极窄段兜底：取中点
|
||||
|
||||
if (angle_in_wrap(theta_of(probe), joint_l, joint_u)) {
|
||||
allow_pairs.emplace_back(L, R);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 6) 无交点的情况：全允许或全禁止
|
||||
double psi = 0.0;
|
||||
double ct = a * std::sin(psi) + b * std::cos(psi) + c;
|
||||
ct = clamp(ct, -1.0, 1.0);
|
||||
double tlim = static_cast<double>(conf) * std::acos(ct);
|
||||
|
||||
if (angle_in_wrap(tlim, joint_l, joint_u)) {
|
||||
allow_pairs = {{-M_PI, M_PI}};
|
||||
} else {
|
||||
allow_pairs.clear();
|
||||
}
|
||||
}
|
||||
|
||||
// 8) 合并小段，干净输出
|
||||
allow_pairs = union_intervals(allow_pairs);
|
||||
|
||||
return allow_pairs;
|
||||
}
|
||||
|
||||
|
||||
bool IkLimitAnalyzer::check_cos_solution(double a, double b, double c, double theta_target, double psi) {
|
||||
// // 由 ψ 复原 θ：θ = acos( a sinψ + b cosψ + c )
|
||||
// double ct = a * std::sin(psi) + b * std::cos(psi) + c;
|
||||
// ct = clamp(ct, -1.0, 1.0);
|
||||
// double theta = std::acos(ct);
|
||||
// return std::abs(theta - theta_target) < 1e-6;
|
||||
|
||||
// ct = a sinψ + b cosψ + c 应等于 cos(theta_target)
|
||||
double ct = a * std::sin(psi) + b * std::cos(psi) + c;
|
||||
ct = clamp(ct, -1.0, 1.0);
|
||||
|
||||
// 目标的 cos 值（对 conf 正负都成立）
|
||||
double v = std::cos(theta_target);
|
||||
|
||||
return std::abs(ct - v) < 1e-6;
|
||||
}
|
||||
|
||||
|
||||
std::vector<double> IkLimitAnalyzer::calc_cos_solution(double a, double b, double c, double theta) {
|
||||
std::vector<double> out;
|
||||
|
||||
// v = cos(theta)
|
||||
double v = std::cos(theta);
|
||||
|
||||
// 二次式：as * t^2 + bs * t + cs = 0， 其中 t = tan(ψ/2)
|
||||
double as = v + b - c;
|
||||
double bs = -2.0 * a;
|
||||
double cs = v - b - c;
|
||||
|
||||
double D = bs * bs - 4.0 * as * cs;
|
||||
if (D < 0.0) return out;
|
||||
D = std::max(0.0, D);
|
||||
double sqrtD = std::sqrt(D);
|
||||
|
||||
double psi1 = 2.0 * std::atan2(-(bs - sqrtD), 2.0 * as);
|
||||
double psi2 = 2.0 * std::atan2(-(bs + sqrtD), 2.0 * as);
|
||||
|
||||
psi1 = normalize_angle(psi1);
|
||||
psi2 = normalize_angle(psi2);
|
||||
|
||||
if (check_cos_solution(a, b, c, theta, psi1)) out.push_back(psi1);
|
||||
if (check_cos_solution(a, b, c, theta, psi2) &&
|
||||
std::abs(psi2 - psi1) > EPS)
|
||||
out.push_back(psi2);
|
||||
|
||||
std::sort(out.begin(), out.end());
|
||||
return out;
|
||||
}
|
||||
|
||||
bool IkLimitAnalyzer::wraps(double L, double U) {
|
||||
// 先都规约到 (-π, π]
|
||||
auto norm = [](double x) {
|
||||
while (x <= -M_PI) x += 2 * M_PI;
|
||||
while (x > M_PI) x -= 2 * M_PI;
|
||||
return x;
|
||||
};
|
||||
L = norm(L);
|
||||
U = norm(U);
|
||||
return (L > U);
|
||||
}
|
||||
|
||||
|
||||
std::vector<std::pair<double, double> > IkLimitAnalyzer::cal_offset_limits(
|
||||
double joint_l, double joint_u, double offset) {
|
||||
auto norm = [](double x) {
|
||||
while (x <= -M_PI) x += 2 * M_PI;
|
||||
while (x > M_PI) x -= 2 * M_PI;
|
||||
return x;
|
||||
};
|
||||
double pL = norm(joint_l + offset);
|
||||
double pU = norm(joint_u + offset);
|
||||
|
||||
std::vector<std::pair<double, double> > joint_ranges;
|
||||
if (!wraps(pL, pU)) {
|
||||
// 单段
|
||||
joint_ranges.push_back({pL, pU});
|
||||
} else {
|
||||
// 跨 ±π，拆成两段 [-π, pU] ∪ [pL, π]
|
||||
joint_ranges.push_back({-M_PI, pU});
|
||||
joint_ranges.push_back({pL, M_PI});
|
||||
}
|
||||
return joint_ranges;
|
||||
}
|
||||
|
||||
std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(
|
||||
double an, double ad, double bn, double bd, double cn, double cd, double joint_l, double joint_u, double offset) {
|
||||
auto joint_ranges = cal_offset_limits(joint_l, joint_u, offset);
|
||||
std::vector<std::pair<double, double> > out{};
|
||||
for (auto [L, U]: joint_ranges) {
|
||||
auto part = calc_tan_limits(an, ad, bn, bd, cn, cd, L, U);
|
||||
out.insert(out.end(), part.begin(), part.end());
|
||||
}
|
||||
out = union_intervals(out);
|
||||
return out;
|
||||
}
|
||||
|
||||
std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
|
||||
double a, double b, double c, int conf, double joint_l, double joint_u, double offset) {
|
||||
auto joint_ranges = cal_offset_limits(joint_l, joint_u, offset);
|
||||
std::vector<std::pair<double, double> > out{};
|
||||
for (auto [L, U]: joint_ranges) {
|
||||
auto part = calc_cos_limits(a, b, c, conf, L, U);
|
||||
out.insert(out.end(), part.begin(), part.end());
|
||||
}
|
||||
out = union_intervals(out);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
bool IkLimitAnalyzer::angle_in_wrap(double x, double L, double U) {
|
||||
x = normalize_angle(x);
|
||||
L = normalize_angle(L);
|
||||
U = normalize_angle(U);
|
||||
if (L <= U) return (x >= L && x <= U);
|
||||
return (x >= L || x <= U); // 跨界
|
||||
}
|
||||
|
||||
std::vector<std::pair<double, double> >
|
||||
IkLimitAnalyzer::union_intervals(const std::vector<std::pair<double, double> > &in) {
|
||||
const double MERGE_EPS = 1e-12;
|
||||
if (in.empty()) return {};
|
||||
std::vector<std::pair<double, double> > v = in;
|
||||
std::sort(v.begin(), v.end(), [](auto &a, auto &b) {
|
||||
return (a.first < b.first) || (a.first == b.first && a.second < b.second);
|
||||
});
|
||||
std::vector<std::pair<double, double> > out;
|
||||
double L = v[0].first, R = v[0].second;
|
||||
for (size_t i = 1; i < v.size(); ++i) {
|
||||
if (v[i].first <= R + MERGE_EPS) R = std::max(R, v[i].second);
|
||||
else {
|
||||
out.push_back({L, R});
|
||||
L = v[i].first;
|
||||
R = v[i].second;
|
||||
}
|
||||
}
|
||||
out.push_back({L, R});
|
||||
return out;
|
||||
}
|
||||
125
src/utils/srs_ik/ik_limit_analyzer.h
Normal file
125
src/utils/srs_ik/ik_limit_analyzer.h
Normal file
@ -0,0 +1,125 @@
|
||||
//
|
||||
// Created by lgv on 2025/11/3.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_IK_LIMIT_ANALYZER_H
|
||||
#define CMVR_ES_IK_LIMIT_ANALYZER_H
|
||||
#include <complex.h>
|
||||
#include <vector>
|
||||
#include <array>
|
||||
#include <iostream>
|
||||
|
||||
namespace cmvr {
|
||||
namespace utils {
|
||||
class IkLimitAnalyzer {
|
||||
public:
|
||||
// Tan 型关节：给定 (an,ad,bn,bd,cn,cd) 与关节极限
|
||||
// 返回 psi 的允许区间边界： [L1,R1,L2,R2,...]
|
||||
static std::vector<std::pair<double, double> >
|
||||
calc_tan_limits(double an, double ad,
|
||||
double bn, double bd,
|
||||
double cn, double cd,
|
||||
double joint_l, double joint_u);
|
||||
|
||||
static std::vector<std::pair<double, double> >
|
||||
calc_tan_limits(double an, double ad,
|
||||
double bn, double bd,
|
||||
double cn, double cd,
|
||||
double joint_l, double joint_u,double offset);
|
||||
|
||||
static std::vector<std::pair<double, double> >
|
||||
calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u);
|
||||
|
||||
static std::vector<std::pair<double, double> >
|
||||
calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u,double offset);
|
||||
|
||||
// 两个“允许集”的交集（输入与输出都是“边界数组”，并且都以允许开始）
|
||||
static std::vector<std::pair<double, double> >
|
||||
intersect(const std::vector<std::pair<double, double> > &A,
|
||||
const std::vector<std::pair<double, double> > &B);
|
||||
|
||||
|
||||
// 用于测试：打印区间
|
||||
static void print_intervals(const std::vector<std::pair<double, double> > &intervals) {
|
||||
for (const auto &interval: intervals) {
|
||||
std::cout << "[" << interval.first << ", " << interval.second << "] ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
private:
|
||||
static constexpr double EPS = 1e-9;
|
||||
|
||||
// 将角度归一化到 [-π, π] 范围内
|
||||
static double normalize_angle(double angle);
|
||||
|
||||
static double deg2rad(double deg) { return deg * M_PI / 180.0; }
|
||||
static double rad2deg(double rad) { return rad * 180.0 / M_PI; }
|
||||
|
||||
template<class T>
|
||||
static constexpr int sign(T x, T eps) {
|
||||
return (x > eps) - (x < -eps);
|
||||
}
|
||||
|
||||
template<class T>
|
||||
static constexpr int sign(T x) {
|
||||
return sign(x, T(0));
|
||||
}
|
||||
|
||||
|
||||
// tan 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解，式（32）
|
||||
static std::vector<double>
|
||||
calc_tan_solution(double an, double ad,
|
||||
double bn, double bd,
|
||||
double cn, double cd,
|
||||
double theta);
|
||||
|
||||
// cos 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解
|
||||
static std::vector<double>
|
||||
calc_cos_solution(double a, double b, double c, double theta);
|
||||
|
||||
|
||||
// 检查解的正确性 ，式（26）
|
||||
static bool check_tan_solution(double an, double ad,
|
||||
double bn, double bd,
|
||||
double cn, double cd,
|
||||
double theta_target,
|
||||
double psi);
|
||||
|
||||
// 检核：cos 型
|
||||
static bool check_cos_solution(double a, double b, double c,
|
||||
double theta_target,
|
||||
double psi);
|
||||
|
||||
|
||||
template<typename T>
|
||||
static T clamp(T v, T lo, T hi) {
|
||||
return std::max(lo, std::min(hi, v));
|
||||
}
|
||||
|
||||
|
||||
static std::vector<std::pair<double, double> >
|
||||
bounds_to_pairs(const std::vector<double> &bounds);
|
||||
|
||||
|
||||
// 判断环绕：区间 [L,U] 是否跨过 π
|
||||
static inline bool wraps(double L, double U);
|
||||
|
||||
|
||||
// 根据DH 参数中的 关节角 θi (rad) 的偏移来计算实际的限位区间
|
||||
static inline std::vector<std::pair<double, double> >
|
||||
cal_offset_limits(double joint_l, double joint_u, double offset);
|
||||
|
||||
|
||||
// 新增：环形包含（ [L,U]）
|
||||
static bool angle_in_wrap(double x, double L, double U);
|
||||
|
||||
// 新增：线性并集（输入输出都在 [-π,π] 且 L<=U；跨界已在上游拆分）
|
||||
static std::vector<std::pair<double,double>>
|
||||
union_intervals(const std::vector<std::pair<double,double>>& in);
|
||||
};
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
#endif //CMVR_ES_IK_LIMIT_ANALYZER_H
|
||||
407
src/utils/srs_ik/srs_ik_slover.cpp
Normal file
407
src/utils/srs_ik/srs_ik_slover.cpp
Normal file
@ -0,0 +1,407 @@
|
||||
//
|
||||
// Created by lgv on 2025/11/3.
|
||||
//
|
||||
|
||||
#include "srs_ik_slover.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
using namespace cmvr::utils;
|
||||
using namespace Eigen;
|
||||
|
||||
SRSIkSlover::SRSIkSlover() {
|
||||
link_lengths_ = Eigen::VectorXd(4);
|
||||
link_lengths_ << 0.0945 + 0.0765, 0.1475 + 0.1025, 0.0965 + 0.1525, 3.222;
|
||||
|
||||
// DH 参数 [d_i, alpha_i, a_i(=0), theta_offset_i]
|
||||
dh_params_ = Eigen::MatrixXd(7, 4);
|
||||
double half_pi = M_PI / 2;
|
||||
dh_params_ << link_lengths_[0], -half_pi, 0.0, 0.0,
|
||||
0.0, half_pi, 0.0, 0.0,
|
||||
link_lengths_[1], -half_pi, 0.0, 0.0,
|
||||
0.0, half_pi, 0.0, 0.0,
|
||||
link_lengths_[2], -half_pi, 0.0, half_pi,
|
||||
0.0, half_pi, 0.0, half_pi,
|
||||
link_lengths_[3], 0.0, 0.0, 0.0;
|
||||
|
||||
d_bs_ = link_lengths_[0];
|
||||
d_se_ = link_lengths_[1];
|
||||
d_ew_ = link_lengths_[2];
|
||||
d_wt_ = link_lengths_[3];
|
||||
|
||||
joints_limits_ = {
|
||||
{-half_pi, half_pi},
|
||||
{-half_pi, half_pi},
|
||||
{-half_pi, half_pi},
|
||||
{-half_pi, half_pi},
|
||||
{-half_pi, half_pi},
|
||||
{-half_pi, half_pi},
|
||||
{-half_pi, half_pi},
|
||||
};
|
||||
}
|
||||
|
||||
Eigen::Matrix3d SRSIkSlover::reference_plane(const Eigen::Vector3d &S, const Eigen::Vector3d &W) {
|
||||
double d_sw = (W - S).norm();
|
||||
Eigen::Vector3d v_sw = (W - S).normalized();
|
||||
|
||||
// 1： 计算E_r 的值
|
||||
double x = (d_sw * d_sw + d_se_ * d_se_ - d_ew_ * d_ew_) / (2 * d_sw);
|
||||
double r = sqrt(std::max(d_se_ * d_se_ - x * x, 0.0));
|
||||
Eigen::Vector3d F = S + x * v_sw;
|
||||
|
||||
Vector3d FE;
|
||||
if (v_sw.head<2>().cwiseAbs().maxCoeff() <= 1e-6) {
|
||||
FE = Vector3d(-1.0, 0.0, 0.0);
|
||||
} else {
|
||||
FE(0) = -v_sw(0) * v_sw(2) / (v_sw(0) * v_sw(0) + v_sw(1) * v_sw(1));
|
||||
FE(1) = -v_sw(1) * v_sw(2) / (v_sw(0) * v_sw(0) + v_sw(1) * v_sw(1));
|
||||
FE(2) = 1.0;
|
||||
}
|
||||
|
||||
// 根据 elbow_config 调整 E 的位置
|
||||
Vector3d E = F + elbow_config_ * r * FE.normalized();
|
||||
|
||||
Vector3d v_es = (S - E).normalized();
|
||||
Vector3d v_ew = (W - E).normalized();
|
||||
|
||||
|
||||
Vector3d R30_y = v_es;
|
||||
Vector3d R30_z = v_ew.cross(v_es);
|
||||
|
||||
|
||||
if (elbow_config_ == INWARD)
|
||||
R30_z = -R30_z;
|
||||
|
||||
auto nz = R30_z.norm();
|
||||
if (nz > 1e-12) {
|
||||
R30_z /= nz; // 或者：R30_z.normalize();
|
||||
} else {
|
||||
// 退化保护：给个正交补方向
|
||||
R30_z = Vector3d(-0.0, 1.0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
Vector3d R30_x = R30_y.cross(R30_z);
|
||||
|
||||
|
||||
Matrix3d R30;
|
||||
R30.col(0) = R30_x;
|
||||
R30.col(1) = R30_y;
|
||||
R30.col(2) = R30_z;
|
||||
|
||||
return R30;
|
||||
}
|
||||
|
||||
std::vector<double> SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose, double psi) {
|
||||
try {
|
||||
// Eigen::VectorXd joints(7);
|
||||
std::vector<double> joints(7, 0);
|
||||
|
||||
// 目标位置
|
||||
Eigen::Vector3d P_target = pose.block<3, 1>(0, 3);
|
||||
|
||||
// 肩部 S 位置 ，相对于基坐标系
|
||||
Eigen::Vector3d S(0.0, 0.0, d_bs_);
|
||||
|
||||
// P67 ： tcp 相对于 {6} 坐标系（腕心 W）的位置
|
||||
Eigen::Vector3d P67(0.0, 0.0, d_wt_);
|
||||
|
||||
// 腕心 W 位置 ，相对于基坐标系
|
||||
Eigen::Vector3d W = P_target - pose.block<3, 3>(0, 0) * P67;
|
||||
|
||||
|
||||
double d_sw = (W - S).norm();
|
||||
if ((std::abs(d_se_ + d_ew_) < (d_sw - 1e-6) || (d_sw + 1e-6) < std::abs(d_se_ - d_ew_))) {
|
||||
throw std::runtime_error("Pose outside reachable workspace, IK solve failed");
|
||||
}
|
||||
|
||||
// 计算肘部角度 关节3
|
||||
double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2 * d_se_ * d_ew_);
|
||||
cos_elbow = std::clamp(double(cos_elbow), -1.0, 1.0);
|
||||
joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow));
|
||||
|
||||
// 计算臂平面E处相对于 基坐标系 的旋转矩阵
|
||||
Eigen::Matrix3d R30 = reference_plane(S, W);
|
||||
Eigen::Matrix3d R_axis = calc_rotation_matrix((W - S).normalized(), psi);
|
||||
Eigen::Matrix3d R3 = R_axis * R30;
|
||||
|
||||
|
||||
// // 求系数矩阵
|
||||
// Eigen::Vector3d normalized_axis = (W - S).normalized();
|
||||
// double ux = normalized_axis[0], uy = normalized_axis[1], uz = normalized_axis[2];
|
||||
// Eigen::Matrix3d u_hat;
|
||||
// u_hat << 0, -uz, uy,
|
||||
// uz, 0, -ux,
|
||||
// -uy, ux, 0;
|
||||
//
|
||||
// Eigen::MatrixXd A_s = u_hat * R30;
|
||||
// Eigen::MatrixXd B_s = -u_hat * u_hat * R30;
|
||||
// Eigen::MatrixXd C_s = (Eigen::MatrixXd::Identity(3, 3) + u_hat * u_hat) * R30;
|
||||
//
|
||||
// Eigen::Matrix3d R3_1 = A_s *sin(psi) + B_s * cos(psi) + C_s;
|
||||
//
|
||||
// // std::cout <<" R3_1 "<< R3_1.transpose() * R3 << std::endl;
|
||||
|
||||
|
||||
// 计算肩部角度 (关节0, 1, 2)
|
||||
joints[0] = std::atan2(-R3(1, 1) * shoulder_config_, -R3(0, 1) * shoulder_config_);
|
||||
joints[1] = std::acos(std::clamp(double(-R3(2, 1)), -0.9999999999, 0.999999999)) * shoulder_config_;
|
||||
joints[2] = std::atan2(R3(2, 2) * shoulder_config_, -R3(2, 0) * shoulder_config_);
|
||||
|
||||
// 计算 R04
|
||||
Eigen::Matrix3d R04 = Eigen::Matrix3d::Identity();
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
// 从 DH 参数中获取参数
|
||||
Eigen::Vector4d dh = dh_params_.row(i);
|
||||
R04 = R04 * calc_dh(dh[0], dh[1], dh[2], dh[3] + joints[i]).block<3, 3>(0, 0);
|
||||
}
|
||||
|
||||
|
||||
Eigen::Matrix3d R47 = R04.transpose() * pose.block<3, 3>(0, 0);
|
||||
|
||||
|
||||
// //
|
||||
// // 计算变换矩阵
|
||||
// Eigen::MatrixXd T34 = calc_dh(dh_params_(3,0),dh_params_(3,1),dh_params_(3,2),dh_params_(3,3)+joints[3]);
|
||||
// Eigen::MatrixXd R34 = T34.block(0, 0, 3, 3);
|
||||
// Eigen::MatrixXd A_w = R34.transpose() * A_s.transpose() * pose.block(0, 0, 3, 3);
|
||||
// Eigen::MatrixXd B_w = R34.transpose() * B_s.transpose() * pose.block(0, 0, 3, 3);
|
||||
// Eigen::MatrixXd C_w = R34.transpose() * C_s.transpose() * pose.block(0, 0, 3, 3);
|
||||
//
|
||||
// Eigen::Matrix3d R47_2 = A_w *sin(psi) + B_w * cos(psi) + C_w;
|
||||
//
|
||||
// std::cout <<" R47_2 "<< R47.transpose() * R47_2 << std::endl;
|
||||
// 提取腕部欧拉角
|
||||
double phi_z = std::atan2(R47(1, 2), R47(0, 2));
|
||||
double theta_y = std::atan2(std::sqrt(R47(2, 0) * R47(2, 0) + R47(2, 1) * R47(2, 1)), R47(2, 2));
|
||||
double psi_z = std::atan2(R47(2, 1), -R47(2, 0));
|
||||
|
||||
// 处理奇异情况
|
||||
if (std::sin(theta_y) < 1e-12) {
|
||||
phi_z = std::atan2(R47(1, 0), R47(0, 0));
|
||||
psi_z = 0.0;
|
||||
}
|
||||
if (std::sin(M_PI - theta_y) < 1e-12) {
|
||||
phi_z = std::atan2(-R47(1, 0), -R47(0, 0));
|
||||
psi_z = 0.0;
|
||||
}
|
||||
|
||||
// 腕部分支调整
|
||||
if (wrist_config_ == INWARD) {
|
||||
phi_z += M_PI;
|
||||
theta_y = -theta_y;
|
||||
psi_z += M_PI;
|
||||
}
|
||||
|
||||
// 调整角度
|
||||
joints[4] = normalize_angle(phi_z - M_PI / 2);
|
||||
joints[5] = normalize_angle(theta_y - M_PI / 2);
|
||||
joints[6] = normalize_angle(psi_z);
|
||||
|
||||
// // 调整角度
|
||||
// joints[4] = (phi_z );
|
||||
// joints[5] = (theta_y);
|
||||
// joints[6] = (psi_z);
|
||||
|
||||
for (double q1: joints) {
|
||||
std::cout << q1 << " , ";
|
||||
}
|
||||
std::cout << std::endl;
|
||||
|
||||
|
||||
return joints;
|
||||
} catch (const std::exception &e) {
|
||||
throw std::runtime_error(e.what());
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Matrix3d SRSIkSlover::calc_rotation_matrix(const Eigen::Vector3d &rotation_axis, double rotation_angle) {
|
||||
// 归一化旋转轴
|
||||
Eigen::Vector3d normalized_axis = rotation_axis.normalized();
|
||||
|
||||
// 构造反对称矩阵
|
||||
double ux = normalized_axis[0], uy = normalized_axis[1], uz = normalized_axis[2];
|
||||
Eigen::Matrix3d u_hat;
|
||||
u_hat << 0, -uz, uy,
|
||||
uz, 0, -ux,
|
||||
-uy, ux, 0;
|
||||
|
||||
// 计算旋转矩阵
|
||||
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity() + std::sin(rotation_angle) * u_hat + (
|
||||
1 - std::cos(rotation_angle)) * (u_hat * u_hat);
|
||||
|
||||
return rotation_matrix;
|
||||
}
|
||||
|
||||
Eigen::Matrix4d SRSIkSlover::calc_dh(double d, double alpha, double a, double theta) {
|
||||
double ca = std::cos(alpha);
|
||||
double sa = std::sin(alpha);
|
||||
double ct = std::cos(theta);
|
||||
double st = std::sin(theta);
|
||||
|
||||
// 构建 DH 变换矩阵
|
||||
Eigen::Matrix4d T;
|
||||
T << ct, -st * ca, st * sa, a * ct,
|
||||
st, ct * ca, -ct * sa, a * st,
|
||||
0.0, sa, ca, d,
|
||||
0.0, 0.0, 0.0, 1.0;
|
||||
|
||||
return T;
|
||||
}
|
||||
|
||||
double SRSIkSlover::normalize_angle(const double angle) {
|
||||
// 将角度转换为 [0, 2π) 范围
|
||||
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
|
||||
|
||||
// 如果 a 为负数，则添加 2π，使其位于 [0, 2π) 范围
|
||||
if (a < 0.0) {
|
||||
a += 2.0 * M_PI;
|
||||
}
|
||||
|
||||
// 将角度范围调整到 [-π, π] 范围
|
||||
return a - M_PI;
|
||||
}
|
||||
|
||||
Eigen::Matrix4d SRSIkSlover::calc_total_transform(const std::vector<double> &joint_angles) {
|
||||
Eigen::Matrix4d T_total = Eigen::Matrix4d::Identity(); // 初始化为单位矩阵
|
||||
|
||||
// 遍历每一组 DH 参数
|
||||
for (size_t i = 0; i < dh_params_.rows(); ++i) {
|
||||
// 获取当前关节的 DH 参数
|
||||
double d = dh_params_(i, 0);
|
||||
double alpha = dh_params_(i, 1);
|
||||
double a = dh_params_(i, 2);
|
||||
double theta0 = dh_params_(i, 3);
|
||||
|
||||
// 当前关节的实际旋转角度 theta
|
||||
double theta = theta0 + joint_angles[i];
|
||||
|
||||
// 计算当前关节的变换矩阵
|
||||
Eigen::Matrix4d T = calc_dh(d, alpha, a, theta);
|
||||
|
||||
// 更新总变换矩阵
|
||||
T_total = T_total * T;
|
||||
}
|
||||
|
||||
return T_total;
|
||||
}
|
||||
|
||||
bool SRSIkSlover::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen::MatrixXd &s_mat, Eigen::MatrixXd &w_mat) {
|
||||
if (s_mat.rows() != 3 || s_mat.cols() != 9) s_mat.setZero(3, 9);
|
||||
if (w_mat.rows() != 3 || w_mat.cols() != 9) w_mat.setZero(3, 9);
|
||||
|
||||
try {
|
||||
// Eigen::VectorXd joints(7);
|
||||
std::vector<double> joints(7, 0);
|
||||
|
||||
// 目标位置
|
||||
Eigen::Vector3d P_target = pose.block<3, 1>(0, 3);
|
||||
|
||||
// 肩部 S 位置 ，相对于基坐标系
|
||||
Eigen::Vector3d S(0.0, 0.0, d_bs_);
|
||||
|
||||
// P67 ： tcp 相对于 {6} 坐标系（腕心 W）的位置
|
||||
Eigen::Vector3d P67(0.0, 0.0, d_wt_);
|
||||
|
||||
// 腕心 W 位置 ，相对于基坐标系
|
||||
Eigen::Vector3d W = P_target - pose.block<3, 3>(0, 0) * P67;
|
||||
|
||||
|
||||
double d_sw = (W - S).norm();
|
||||
if ((std::abs(d_se_ + d_ew_) < d_sw) || (d_sw < std::abs(d_se_ - d_ew_))) {
|
||||
throw std::runtime_error("Pose outside reachable workspace, IK solve failed");
|
||||
}
|
||||
|
||||
// 计算肘部角度 关节3
|
||||
double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2 * d_se_ * d_ew_);
|
||||
cos_elbow = std::clamp(cos_elbow, -1.0, 1.0);
|
||||
joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow));
|
||||
|
||||
|
||||
// 计算臂平面E处相对于 基坐标系 的旋转矩阵
|
||||
Eigen::Matrix3d R30 = reference_plane(S, W);
|
||||
Eigen::Vector3d normalized_axis = (W - S).normalized();
|
||||
|
||||
// 构造反对称矩阵
|
||||
double ux = normalized_axis[0], uy = normalized_axis[1], uz = normalized_axis[2];
|
||||
Eigen::Matrix3d u_hat;
|
||||
u_hat << 0, -uz, uy,
|
||||
uz, 0, -ux,
|
||||
-uy, ux, 0;
|
||||
|
||||
// 计算旋转矩阵 R03
|
||||
Eigen::MatrixXd A_s = u_hat * R30;
|
||||
Eigen::MatrixXd B_s = -u_hat * u_hat * R30;
|
||||
Eigen::MatrixXd C_s = (Eigen::MatrixXd::Identity(3, 3) + u_hat * u_hat) * R30;
|
||||
|
||||
|
||||
// 计算变换矩阵
|
||||
Eigen::MatrixXd T34 = calc_dh(dh_params_(3, 0), dh_params_(3, 1), dh_params_(3, 2),
|
||||
dh_params_(3, 3) + joints[3]);
|
||||
Eigen::MatrixXd R34 = T34.block(0, 0, 3, 3);
|
||||
Eigen::MatrixXd A_w = R34.transpose() * A_s.transpose() * pose.block(0, 0, 3, 3);
|
||||
Eigen::MatrixXd B_w = R34.transpose() * B_s.transpose() * pose.block(0, 0, 3, 3);
|
||||
Eigen::MatrixXd C_w = R34.transpose() * C_s.transpose() * pose.block(0, 0, 3, 3);
|
||||
|
||||
s_mat.block<3, 3>(0, 0) = A_s;
|
||||
s_mat.block<3, 3>(0, 3) = B_s;
|
||||
s_mat.block<3, 3>(0, 6) = C_s;
|
||||
|
||||
w_mat.block<3, 3>(0, 0) = A_w;
|
||||
w_mat.block<3, 3>(0, 3) = B_w;
|
||||
w_mat.block<3, 3>(0, 6) = C_w;
|
||||
|
||||
return true;
|
||||
} catch (const std::exception &e) {
|
||||
throw std::runtime_error(e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
std::vector<std::pair<double, double> > SRSIkSlover::calc_arm_angle_limits(
|
||||
const Eigen::MatrixXd &s_mat, const Eigen::MatrixXd &w_mat) {
|
||||
// 期望 s_mat / w_mat 都是 3x9： [A | B | C]
|
||||
if (s_mat.rows() != 3 || s_mat.cols() != 9 ||
|
||||
w_mat.rows() != 3 || w_mat.cols() != 9) {
|
||||
// 尺寸不对，直接返回空
|
||||
return {};
|
||||
}
|
||||
|
||||
|
||||
const Eigen::Matrix3d As = s_mat.block<3, 3>(0, 0);
|
||||
const Eigen::Matrix3d Bs = s_mat.block<3, 3>(0, 3);
|
||||
const Eigen::Matrix3d Cs = s_mat.block<3, 3>(0, 6);
|
||||
|
||||
const Eigen::Matrix3d Aw = w_mat.block<3, 3>(0, 0);
|
||||
const Eigen::Matrix3d Bw = w_mat.block<3, 3>(0, 3);
|
||||
const Eigen::Matrix3d Cw = w_mat.block<3, 3>(0, 6);
|
||||
|
||||
auto s = static_cast<int>(shoulder_config_);
|
||||
auto e = static_cast<int>(elbow_config_);
|
||||
auto w = static_cast<int>(wrist_config_);
|
||||
|
||||
auto limit_1 = ik_limit_analyzer_.calc_tan_limits(-s * As(1, 1), -s * As(0, 1), -s * Bs(1, 1), -s * Bs(0, 1),
|
||||
-s * Cs(1, 1), -s * Cs(0, 1), 0.5, M_PI / 2.0);
|
||||
|
||||
auto limit_2 = ik_limit_analyzer_.calc_cos_limits(-As(2, 1), -Bs(2, 1), -Cs(2, 1), s, -2.5, -2);
|
||||
|
||||
auto limit_3 = ik_limit_analyzer_.calc_tan_limits(s * As(2, 2),
|
||||
-s * As(2, 0), s * Bs(2, 2), -s * Bs(2, 0),
|
||||
s * Cs(2, 2), -s * Cs(2, 0), -3.14 / 2.0, 3.14 / 3.0);
|
||||
|
||||
auto limit_5 = ik_limit_analyzer_.calc_tan_limits(w * Aw(1, 2),
|
||||
w * Aw(0, 2), w * Bw(1, 2), w * Bw(0, 2),
|
||||
w * Cw(1, 2), w * Cw(0, 2), -0.2, 1.8,M_PI / 2.0);
|
||||
|
||||
auto limit_6 = ik_limit_analyzer_.calc_cos_limits(Aw(2, 2), Bw(2, 2), Cw(2, 2), w, 0.5, 2.0,M_PI / 2.0);
|
||||
auto limit_7 = ik_limit_analyzer_.calc_tan_limits(w * Aw(2, 1),
|
||||
-w * Aw(2, 0), w * Bw(2, 1), -w * Bw(2, 0),
|
||||
w * Cw(2, 1), -w * Cw(2, 0), -3.0, -2.0);
|
||||
|
||||
|
||||
ik_limit_analyzer_.print_intervals(limit_6);
|
||||
|
||||
|
||||
return limit_6;
|
||||
}
|
||||
89
src/utils/srs_ik/srs_ik_slover.h
Normal file
89
src/utils/srs_ik/srs_ik_slover.h
Normal file
@ -0,0 +1,89 @@
|
||||
//
|
||||
// Created by lgv on 2025/11/3.
|
||||
//
|
||||
|
||||
#ifndef CMVR_ES_SRS_IK_SLOVER_H
|
||||
#define CMVR_ES_SRS_IK_SLOVER_H
|
||||
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include "srs_ik/ik_limit_analyzer.h"
|
||||
namespace cmvr {
|
||||
namespace utils {
|
||||
|
||||
class SRSIkSlover {
|
||||
public:
|
||||
enum ConfigDirection {
|
||||
OUTWARD = 1, // 向外
|
||||
INWARD = -1 // 向内
|
||||
};
|
||||
|
||||
public:
|
||||
SRSIkSlover();
|
||||
~SRSIkSlover(){};
|
||||
|
||||
std::vector<double> inverse_kinematics(const Eigen::MatrixXd& pose, double psi);
|
||||
|
||||
Eigen::Matrix4d calc_total_transform(const std::vector<double>& joint_angles);
|
||||
|
||||
bool cal_coefficient_matrix(const Eigen::MatrixXd& pose,Eigen::MatrixXd& s_mat , Eigen::MatrixXd& w_mat);
|
||||
std::vector<std::pair<double, double>> calc_arm_angle_limits(const Eigen::MatrixXd& s_mat ,const Eigen::MatrixXd& w_mat);
|
||||
|
||||
void set_shoulder_config(ConfigDirection value) {
|
||||
shoulder_config_ = value;
|
||||
}
|
||||
|
||||
void set_elbow_config(ConfigDirection value) {
|
||||
elbow_config_ = value;
|
||||
}
|
||||
|
||||
void set_wrist_config(ConfigDirection value) {
|
||||
wrist_config_ = value;
|
||||
}
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
|
||||
ConfigDirection shoulder_config_{OUTWARD};
|
||||
ConfigDirection elbow_config_{OUTWARD};
|
||||
ConfigDirection wrist_config_{OUTWARD};
|
||||
// DH参数
|
||||
Eigen::VectorXd link_lengths_;
|
||||
Eigen::MatrixXd dh_params_;
|
||||
|
||||
double d_bs_, d_se_, d_ew_, d_wt_;
|
||||
|
||||
// 关节物理限位
|
||||
// first : min second : max
|
||||
std::vector<std::pair<double, double>> joints_limits_{};
|
||||
|
||||
IkLimitAnalyzer ik_limit_analyzer_;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 计算参考平面相对于基坐标系的旋转矩阵
|
||||
Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, const Eigen::Vector3d& W);
|
||||
|
||||
// 罗德里格斯公式
|
||||
Eigen::Matrix3d calc_rotation_matrix(const Eigen::Vector3d& rotation_axis, double rotation_angle);
|
||||
|
||||
|
||||
// 使用 DH 参数计算变换矩阵
|
||||
Eigen::Matrix4d calc_dh(double d, double alpha, double a, double theta);
|
||||
|
||||
// 将角度归一化到 [-π, π] 范围内
|
||||
double normalize_angle(const double angle);
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif //CMVR_ES_SRS_IK_SLOVER_H
|
||||
132
src/utils/srs_ik/srs_ik_test.cpp
Normal file
132
src/utils/srs_ik/srs_ik_test.cpp
Normal file
@ -0,0 +1,132 @@
|
||||
//
|
||||
// Created by lgv on 2025/11/3.
|
||||
//
|
||||
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "manif/SE3.h"
|
||||
#include "srs_ik/srs_ik_slover.h"
|
||||
#include "srs_ik/ik_limit_analyzer.h"
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <fstream>
|
||||
|
||||
using namespace manif;
|
||||
using namespace cmvr::utils;
|
||||
|
||||
struct IkSample {
|
||||
double psi;
|
||||
std::array<double,7> q; // q1..q7
|
||||
};
|
||||
|
||||
|
||||
|
||||
bool write_ik_samples_csv(const std::string& filepath,
|
||||
const std::vector<IkSample>& samples,
|
||||
bool write_header,
|
||||
int precision)
|
||||
{
|
||||
std::ofstream ofs(filepath, std::ios::out | std::ios::trunc);
|
||||
if (!ofs.is_open()) return false;
|
||||
|
||||
// 固定小数点（避免本地化成逗号）
|
||||
ofs.imbue(std::locale::classic());
|
||||
ofs << std::fixed << std::setprecision(precision);
|
||||
|
||||
if (write_header) {
|
||||
ofs << "psi,q1,q2,q3,q4,q5,q6,q7\n";
|
||||
}
|
||||
for (const auto& s : samples) {
|
||||
ofs << s.psi;
|
||||
for (int i = 0; i < 7; ++i) ofs << ',' << s.q[i];
|
||||
ofs << '\n';
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
TEST(SRS_IK_TEST,SRS_IK_SLOVER_TEST) {
|
||||
|
||||
std::cout << std::fixed << std::setprecision(7);
|
||||
|
||||
SRSIkSlover slover;
|
||||
std::vector<IkSample> samples;
|
||||
samples.reserve(4096);
|
||||
|
||||
std::vector<double> joint_angles(7,0);
|
||||
joint_angles[3] = 0.111;
|
||||
joint_angles = { 0.875, M_PI / 2, 0.2644, M_PI / 2, 1, M_PI/ 4, M_PI/ 5};
|
||||
|
||||
auto pose = slover.calc_total_transform(joint_angles);
|
||||
std::cout << "Pose: " << pose << std::endl;
|
||||
|
||||
// slover.set_elbow_config(SRSIkSlover::INWARD);
|
||||
// slover.set_wrist_config(SRSIkSlover::INWARD);
|
||||
// slover.set_shoulder_config(SRSIkSlover::INWARD);
|
||||
|
||||
|
||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||
slover.cal_coefficient_matrix(pose, s_mat, w_mat);
|
||||
auto limits = slover.calc_arm_angle_limits(s_mat,w_mat);
|
||||
for (const auto & limit: limits) {
|
||||
for (double psi = limit.first; psi < limit.second; psi+=0.001 ) {
|
||||
auto q = slover.inverse_kinematics(pose,psi);
|
||||
samples.push_back(IkSample{psi, {q[0],q[1],q[2],q[3],q[4],q[5],q[6]}});
|
||||
pose = slover.calc_total_transform(q);
|
||||
// std::cout << "Pose: " << pose << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Eigen::VectorXd nsparams = Eigen::VectorXd::LinSpaced(1000, -3.1415926, 3.1415926);
|
||||
// for (auto psi: nsparams) {
|
||||
// auto q = slover.inverse_kinematics(pose,psi);
|
||||
// samples.push_back(IkSample{psi, {q[0],q[1],q[2],q[3],q[4],q[5],q[6]}});
|
||||
// for (double q1: q) {
|
||||
// std::cout << q1 << " , " ;
|
||||
// }
|
||||
// std::cout << std::endl;
|
||||
// pose = slover.calc_total_transform(q);
|
||||
// // std::cout << "Pose: " << pose << std::endl;
|
||||
// }
|
||||
|
||||
|
||||
write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples,true, 9);
|
||||
}
|
||||
|
||||
TEST(SRS_IK_TEST,INTERSECT_TEST) {
|
||||
// 测试 1: 有交集的区间
|
||||
std::vector<std::pair<double, double>> A = {{-3.0, -1.0}, {1.0, 4.0}};
|
||||
std::vector<std::pair<double, double>> B = {{-2.0, 0.5}, {2.5, 5.0}};
|
||||
|
||||
std::cout << "Test 1: Intersecting intervals" << std::endl;
|
||||
auto result1 = IkLimitAnalyzer::intersect(A, B);
|
||||
IkLimitAnalyzer::print_intervals(result1);
|
||||
// 预期输出: [-2.0, -1.0] [2.5, 4.0]
|
||||
|
||||
// 测试 2: 相邻但不重叠的区间
|
||||
std::vector<std::pair<double, double>> C = {{-3.0, -1.0}, {2.0, 4.0}};
|
||||
std::vector<std::pair<double, double>> D = {{-1.0, 0.0}, {1.0, 3.0}};
|
||||
|
||||
std::cout << "Test 2: Adjacent intervals" << std::endl;
|
||||
auto result2 = IkLimitAnalyzer::intersect(C, D);
|
||||
IkLimitAnalyzer::print_intervals(result2);
|
||||
// 预期输出: [2.0, 3.0]
|
||||
|
||||
// 测试 3: 无交集的区间
|
||||
std::vector<std::pair<double, double>> E = {{-5.0, -3.0}, {2.0, 4.0}};
|
||||
std::vector<std::pair<double, double>> F = {{5.0, 6.0}, {7.0, 8.0}};
|
||||
|
||||
std::cout << "Test 3: Non-intersecting intervals" << std::endl;
|
||||
auto result3 = IkLimitAnalyzer::intersect(E, F);
|
||||
IkLimitAnalyzer::print_intervals(result3);
|
||||
// 预期输出: (无输出)
|
||||
|
||||
// 测试 4: 一个空的区间集
|
||||
std::vector<std::pair<double, double>> G = {};
|
||||
std::vector<std::pair<double, double>> H = {{1.0, 2.0}, {3.0, 4.0}};
|
||||
|
||||
std::cout << "Test 4: Empty intervals" << std::endl;
|
||||
auto result4 = IkLimitAnalyzer::intersect(G, H);
|
||||
IkLimitAnalyzer::print_intervals(result4);
|
||||
// 预期输出: (无输出)
|
||||
|
||||
}
|
||||
17
third_party/manif/0.0.5/include/manif/Bundle.h
vendored
Normal file
17
third_party/manif/0.0.5/include/manif/Bundle.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/Rn.h
vendored
Normal file
17
third_party/manif/0.0.5/include/manif/Rn.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/SE2.h
vendored
Normal file
16
third_party/manif/0.0.5/include/manif/SE2.h
vendored
Normal 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_ */
|
||||
15
third_party/manif/0.0.5/include/manif/SE3.h
vendored
Normal file
15
third_party/manif/0.0.5/include/manif/SE3.h
vendored
Normal file
@ -0,0 +1,15 @@
|
||||
#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
third_party/manif/0.0.5/include/manif/SE_2_3.h
vendored
Normal file
16
third_party/manif/0.0.5/include/manif/SE_2_3.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/SGal3.h
vendored
Normal file
16
third_party/manif/0.0.5/include/manif/SGal3.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/SO2.h
vendored
Normal file
17
third_party/manif/0.0.5/include/manif/SO2.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/SO3.h
vendored
Normal file
16
third_party/manif/0.0.5/include/manif/SO3.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/algorithms/average.h
vendored
Normal file
274
third_party/manif/0.0.5/include/manif/algorithms/average.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/algorithms/bezier.h
vendored
Normal file
91
third_party/manif/0.0.5/include/manif/algorithms/bezier.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/algorithms/decasteljau.h
vendored
Normal file
118
third_party/manif/0.0.5/include/manif/algorithms/decasteljau.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/algorithms/interpolation.h
vendored
Normal file
325
third_party/manif/0.0.5/include/manif/algorithms/interpolation.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/autodiff/autodiff.h
vendored
Normal file
54
third_party/manif/0.0.5/include/manif/autodiff/autodiff.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/autodiff/constants.h
vendored
Normal file
42
third_party/manif/0.0.5/include/manif/autodiff/constants.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/autodiff/local_parameterization.h
vendored
Normal file
37
third_party/manif/0.0.5/include/manif/autodiff/local_parameterization.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/ceres/ceres.h
vendored
Normal file
81
third_party/manif/0.0.5/include/manif/ceres/ceres.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/ceres/ceres_utils.h
vendored
Normal file
90
third_party/manif/0.0.5/include/manif/ceres/ceres_utils.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/ceres/constants.h
vendored
Normal file
26
third_party/manif/0.0.5/include/manif/ceres/constants.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/ceres/constraint.h
vendored
Normal file
147
third_party/manif/0.0.5/include/manif/ceres/constraint.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/ceres/local_parametrization.h
vendored
Normal file
46
third_party/manif/0.0.5/include/manif/ceres/local_parametrization.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/ceres/manifold.h
vendored
Normal file
61
third_party/manif/0.0.5/include/manif/ceres/manifold.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/ceres/objective.h
vendored
Normal file
96
third_party/manif/0.0.5/include/manif/ceres/objective.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/constants.h
vendored
Normal file
68
third_party/manif/0.0.5/include/manif/constants.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/functions.h
vendored
Normal file
230
third_party/manif/0.0.5/include/manif/functions.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/gtest/gtest_eigen_utils.h
vendored
Normal file
298
third_party/manif/0.0.5/include/manif/gtest/gtest_eigen_utils.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/gtest/gtest_manif_utils.h
vendored
Normal file
133
third_party/manif/0.0.5/include/manif/gtest/gtest_manif_utils.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/assignment_assert.h
vendored
Normal file
24
third_party/manif/0.0.5/include/manif/impl/assignment_assert.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/bracket.h
vendored
Normal file
35
third_party/manif/0.0.5/include/manif/impl/bracket.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/bundle/Bundle.h
vendored
Normal file
208
third_party/manif/0.0.5/include/manif/impl/bundle/Bundle.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/bundle/BundleTangent.h
vendored
Normal file
191
third_party/manif/0.0.5/include/manif/impl/bundle/BundleTangent.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/bundle/BundleTangent_base.h
vendored
Normal file
439
third_party/manif/0.0.5/include/manif/impl/bundle/BundleTangent_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/bundle/BundleTangent_map.h
vendored
Normal file
99
third_party/manif/0.0.5/include/manif/impl/bundle/BundleTangent_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/bundle/Bundle_base.h
vendored
Normal file
442
third_party/manif/0.0.5/include/manif/impl/bundle/Bundle_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/bundle/Bundle_map.h
vendored
Normal file
97
third_party/manif/0.0.5/include/manif/impl/bundle/Bundle_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/bundle/Bundle_properties.h
vendored
Normal file
33
third_party/manif/0.0.5/include/manif/impl/bundle/Bundle_properties.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/cast.h
vendored
Normal file
35
third_party/manif/0.0.5/include/manif/impl/cast.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/eigen.h
vendored
Normal file
254
third_party/manif/0.0.5/include/manif/impl/eigen.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/generator.h
vendored
Normal file
47
third_party/manif/0.0.5/include/manif/impl/generator.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/lie_group_base.h
vendored
Normal file
735
third_party/manif/0.0.5/include/manif/impl/lie_group_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/macro.h
vendored
Normal file
329
third_party/manif/0.0.5/include/manif/impl/macro.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/random.h
vendored
Normal file
39
third_party/manif/0.0.5/include/manif/impl/random.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/rn/Rn.h
vendored
Normal file
141
third_party/manif/0.0.5/include/manif/impl/rn/Rn.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/rn/RnTangent.h
vendored
Normal file
130
third_party/manif/0.0.5/include/manif/impl/rn/RnTangent.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/rn/RnTangent_base.h
vendored
Normal file
226
third_party/manif/0.0.5/include/manif/impl/rn/RnTangent_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/rn/RnTangent_map.h
vendored
Normal file
85
third_party/manif/0.0.5/include/manif/impl/rn/RnTangent_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/rn/Rn_base.h
vendored
Normal file
224
third_party/manif/0.0.5/include/manif/impl/rn/Rn_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/rn/Rn_map.h
vendored
Normal file
87
third_party/manif/0.0.5/include/manif/impl/rn/Rn_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/rn/Rn_properties.h
vendored
Normal file
33
third_party/manif/0.0.5/include/manif/impl/rn/Rn_properties.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se2/SE2.h
vendored
Normal file
226
third_party/manif/0.0.5/include/manif/impl/se2/SE2.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se2/SE2Tangent.h
vendored
Normal file
125
third_party/manif/0.0.5/include/manif/impl/se2/SE2Tangent.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se2/SE2Tangent_base.h
vendored
Normal file
495
third_party/manif/0.0.5/include/manif/impl/se2/SE2Tangent_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se2/SE2Tangent_map.h
vendored
Normal file
89
third_party/manif/0.0.5/include/manif/impl/se2/SE2Tangent_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se2/SE2_base.h
vendored
Normal file
441
third_party/manif/0.0.5/include/manif/impl/se2/SE2_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se2/SE2_map.h
vendored
Normal file
103
third_party/manif/0.0.5/include/manif/impl/se2/SE2_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se2/SE2_properties.h
vendored
Normal file
33
third_party/manif/0.0.5/include/manif/impl/se2/SE2_properties.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se3/SE3.h
vendored
Normal file
217
third_party/manif/0.0.5/include/manif/impl/se3/SE3.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se3/SE3Tangent.h
vendored
Normal file
113
third_party/manif/0.0.5/include/manif/impl/se3/SE3Tangent.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se3/SE3Tangent_base.h
vendored
Normal file
473
third_party/manif/0.0.5/include/manif/impl/se3/SE3Tangent_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se3/SE3Tangent_map.h
vendored
Normal file
89
third_party/manif/0.0.5/include/manif/impl/se3/SE3Tangent_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se3/SE3_base.h
vendored
Normal file
481
third_party/manif/0.0.5/include/manif/impl/se3/SE3_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se3/SE3_map.h
vendored
Normal file
91
third_party/manif/0.0.5/include/manif/impl/se3/SE3_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se3/SE3_properties.h
vendored
Normal file
33
third_party/manif/0.0.5/include/manif/impl/se3/SE3_properties.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3.h
vendored
Normal file
230
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent.h
vendored
Normal file
112
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent_base.h
vendored
Normal file
495
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent_map.h
vendored
Normal file
89
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3Tangent_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3_base.h
vendored
Normal file
491
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3_map.h
vendored
Normal file
89
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3_properties.h
vendored
Normal file
33
third_party/manif/0.0.5/include/manif/impl/se_2_3/SE_2_3_properties.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3.h
vendored
Normal file
254
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3Tangent.h
vendored
Normal file
106
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3Tangent.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3Tangent_base.h
vendored
Normal file
635
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3Tangent_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3Tangent_map.h
vendored
Normal file
85
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3Tangent_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3_base.h
vendored
Normal file
484
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3_map.h
vendored
Normal file
84
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3_properties.h
vendored
Normal file
31
third_party/manif/0.0.5/include/manif/impl/sgal3/SGal3_properties.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so2/SO2.h
vendored
Normal file
152
third_party/manif/0.0.5/include/manif/impl/so2/SO2.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so2/SO2Tangent.h
vendored
Normal file
122
third_party/manif/0.0.5/include/manif/impl/so2/SO2Tangent.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so2/SO2Tangent_base.h
vendored
Normal file
240
third_party/manif/0.0.5/include/manif/impl/so2/SO2Tangent_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so2/SO2Tangent_map.h
vendored
Normal file
85
third_party/manif/0.0.5/include/manif/impl/so2/SO2Tangent_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so2/SO2_base.h
vendored
Normal file
355
third_party/manif/0.0.5/include/manif/impl/so2/SO2_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so2/SO2_map.h
vendored
Normal file
91
third_party/manif/0.0.5/include/manif/impl/so2/SO2_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so2/SO2_properties.h
vendored
Normal file
33
third_party/manif/0.0.5/include/manif/impl/so2/SO2_properties.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so3/SO3.h
vendored
Normal file
183
third_party/manif/0.0.5/include/manif/impl/so3/SO3.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so3/SO3Tangent.h
vendored
Normal file
112
third_party/manif/0.0.5/include/manif/impl/so3/SO3Tangent.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so3/SO3Tangent_base.h
vendored
Normal file
342
third_party/manif/0.0.5/include/manif/impl/so3/SO3Tangent_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so3/SO3Tangent_map.h
vendored
Normal file
89
third_party/manif/0.0.5/include/manif/impl/so3/SO3Tangent_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so3/SO3_base.h
vendored
Normal file
440
third_party/manif/0.0.5/include/manif/impl/so3/SO3_base.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so3/SO3_map.h
vendored
Normal file
95
third_party/manif/0.0.5/include/manif/impl/so3/SO3_map.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/so3/SO3_properties.h
vendored
Normal file
33
third_party/manif/0.0.5/include/manif/impl/so3/SO3_properties.h
vendored
Normal 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
third_party/manif/0.0.5/include/manif/impl/tangent_base.h
vendored
Normal file
978
third_party/manif/0.0.5/include/manif/impl/tangent_base.h
vendored
Normal 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_ */
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user