feat(slam): add lidar SLAM and pointcloud processing packages
This commit is contained in:
X-lanni
@@ -0,0 +1,472 @@
|
||||
#include "FOV_Checker.h"
|
||||
|
||||
FOV_Checker::FOV_Checker(){
|
||||
// fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","w");
|
||||
// fprintf(fp,"cur_pose_x,cur_pose_y,cur_pose_z,axis_x,axis_y,axis_z,theta,depth\n");
|
||||
// fclose(fp);
|
||||
}
|
||||
|
||||
FOV_Checker::~FOV_Checker(){
|
||||
|
||||
}
|
||||
|
||||
void FOV_Checker::Set_Env(BoxPointType env_param){
|
||||
env = env_param;
|
||||
}
|
||||
|
||||
void FOV_Checker::Set_BoxLength(double box_len_param){
|
||||
box_length = box_len_param;
|
||||
}
|
||||
|
||||
void round_v3d(Eigen::Vector3d &vec, int decimal){
|
||||
double tmp;
|
||||
int t;
|
||||
for (int i = 0; i < 3; i++){
|
||||
t = pow(10,decimal);
|
||||
tmp = round(vec(i)*t);
|
||||
vec(i) = tmp/t;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void FOV_Checker::check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector<BoxPointType> &boxes){
|
||||
round_v3d(cur_pose,4);
|
||||
round_v3d(axis,3);
|
||||
axis = axis/axis.norm();
|
||||
// fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","a");
|
||||
// fprintf(fp,"%f,%f,%f,%f,%f,%f,%0.4f,%0.1f,",cur_pose(0),cur_pose(1),cur_pose(2),axis(0),axis(1),axis(2),theta,depth);
|
||||
// fclose(fp);
|
||||
// cout << "cur_pose: " << cur_pose.transpose() << endl;
|
||||
// cout<< "axis: " << axis.transpose() << endl;
|
||||
// cout<< "theta: " << theta << " depth: " << depth << endl;
|
||||
// cout<< "env: " << env.vertex_min[0] << " " << env.vertex_max[0] << endl;
|
||||
double axis_angle[6], min_angle, gap, plane_u_min, plane_u_max;
|
||||
Eigen::Vector3d plane_w, plane_u, plane_v, center_point, start_point, box_p;
|
||||
Eigen::Vector3d box_p_min, box_p_max;
|
||||
int i, j, k, index, maxn, start_i, max_uN, max_vN, max_ulogN, u_min, u_max;
|
||||
bool flag = false, box_found = false;
|
||||
boxes.clear();
|
||||
BoxPointType box;
|
||||
axis_angle[0] = acos(axis(0));
|
||||
axis_angle[1] = acos(axis(1));
|
||||
axis_angle[2] = acos(axis(2));
|
||||
axis_angle[3] = acos(-axis(0));
|
||||
axis_angle[4] = acos(-axis(1));
|
||||
axis_angle[5] = acos(-axis(2));
|
||||
index = 1;
|
||||
min_angle = axis_angle[0];
|
||||
for (i=1;i<6;i++){
|
||||
if (axis_angle[i]<min_angle){
|
||||
min_angle = axis_angle[i];
|
||||
index = i+1;
|
||||
}
|
||||
}
|
||||
switch (index){
|
||||
case 1:
|
||||
// YZ plane
|
||||
plane_w = Eigen::Vector3d(1,0,0);
|
||||
plane_u = Eigen::Vector3d(0,1,0);
|
||||
plane_v = Eigen::Vector3d(0,0,1);
|
||||
gap = floor(cur_pose(0)/box_length + 0.5 + eps_value) * box_length + 0.5 * box_length - cur_pose(0);
|
||||
maxn = ceil((env.vertex_max[0]-cur_pose(0))/box_length) +1;
|
||||
start_i = 0;
|
||||
break;
|
||||
case 2:
|
||||
// XZ plane
|
||||
plane_w = Eigen::Vector3d(0,1,0);
|
||||
plane_u = Eigen::Vector3d(1,0,0);
|
||||
plane_v = Eigen::Vector3d(0,0,1);
|
||||
gap = floor(cur_pose(1)/box_length + 0.5 + eps_value) * box_length + 0.5 * box_length - cur_pose(1);
|
||||
maxn = ceil((env.vertex_max[1]-cur_pose(1))/box_length) +1;
|
||||
start_i = 0;
|
||||
break;
|
||||
case 3:
|
||||
// XY plane
|
||||
plane_w = Eigen::Vector3d(0,0,1);
|
||||
plane_u = Eigen::Vector3d(1,0,0);
|
||||
plane_v = Eigen::Vector3d(0,1,0);
|
||||
gap = floor(cur_pose(2)/box_length + 0.5 + eps_value) * box_length + 0.5 * box_length - cur_pose(2);
|
||||
maxn = ceil((env.vertex_max[2]-cur_pose(2))/box_length) +1;
|
||||
start_i = 0;
|
||||
break;
|
||||
case 4:
|
||||
// YZ plane
|
||||
plane_w = Eigen::Vector3d(1,0,0);
|
||||
plane_u = Eigen::Vector3d(0,1,0);
|
||||
plane_v = Eigen::Vector3d(0,0,1);
|
||||
gap = ceil(cur_pose(0)/box_length - 0.5 - eps_value) * box_length - 0.5 * box_length - cur_pose(0);
|
||||
maxn = ceil((cur_pose(0)-env.vertex_min[0])/box_length) +1;
|
||||
start_i = 1;
|
||||
break;
|
||||
case 5:
|
||||
// XZ plane
|
||||
plane_w = Eigen::Vector3d(0,1,0);
|
||||
plane_u = Eigen::Vector3d(1,0,0);
|
||||
plane_v = Eigen::Vector3d(0,0,1);
|
||||
gap = ceil(cur_pose(1)/box_length - 0.5 - eps_value) * box_length - 0.5 * box_length - cur_pose(1);
|
||||
maxn = ceil((cur_pose(1)-env.vertex_min[1])/box_length) +1;
|
||||
start_i = 1;
|
||||
break;
|
||||
case 6:
|
||||
// XY plane
|
||||
plane_w = Eigen::Vector3d(1,0,0);
|
||||
plane_u = Eigen::Vector3d(0,1,0);
|
||||
plane_v = Eigen::Vector3d(0,0,1);
|
||||
gap = ceil(cur_pose(2)/box_length - 0.5 - eps_value) * box_length - 0.5 * box_length - cur_pose(2);
|
||||
maxn = ceil((cur_pose(2)-env.vertex_min[2])/box_length) +1;
|
||||
start_i = 1;
|
||||
break;
|
||||
default:
|
||||
// YZ plane
|
||||
plane_w = Eigen::Vector3d(1,0,0);
|
||||
plane_u = Eigen::Vector3d(0,1,0);
|
||||
plane_v = Eigen::Vector3d(0,0,1);
|
||||
gap = ceil(cur_pose(0)/box_length + 0.5 + eps_value) * box_length + 0.5 * box_length - cur_pose(0);
|
||||
maxn = ceil((env.vertex_max[0]-cur_pose(0))/box_length) +1;
|
||||
start_i = 0;
|
||||
break;
|
||||
}
|
||||
for (i=start_i; i<=maxn; i++){
|
||||
center_point = cur_pose + (abs(gap) + (i-1) * box_length)/cos(min_angle) * axis;
|
||||
if (index == 1 || index == 4){
|
||||
start_point = Eigen::Vector3d(center_point(0),floor(center_point(1)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length,floor(center_point(2)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length);
|
||||
max_uN = ceil((env.vertex_max[1]-env.vertex_min[1])/box_length);
|
||||
max_ulogN = floor(log2(max_uN));
|
||||
max_vN = ceil((env.vertex_max[2]-env.vertex_min[2])/box_length);
|
||||
plane_u_min = env.vertex_min[1];
|
||||
plane_u_max = env.vertex_max[1];
|
||||
} else {
|
||||
if (index == 2 || index == 5){
|
||||
start_point = Eigen::Vector3d(floor(center_point(0)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length, center_point(1), floor(center_point(2)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length);
|
||||
max_uN = ceil((env.vertex_max[0]-env.vertex_min[0])/box_length);
|
||||
max_ulogN = floor(log2(max_uN));
|
||||
max_vN = ceil((env.vertex_max[2]-env.vertex_min[2])/box_length);
|
||||
plane_u_min = env.vertex_min[0];
|
||||
plane_u_max = env.vertex_max[0];
|
||||
} else {
|
||||
start_point = Eigen::Vector3d(floor(center_point(0)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length, floor(center_point(1)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length, center_point(2));
|
||||
max_uN = ceil((env.vertex_max[1]-env.vertex_min[1])/box_length);
|
||||
max_ulogN = floor(log2(max_uN));
|
||||
max_vN = ceil((env.vertex_max[2]-env.vertex_min[2])/box_length);
|
||||
plane_u_min = env.vertex_min[1];
|
||||
plane_u_max = env.vertex_max[1];
|
||||
}
|
||||
}
|
||||
flag = false;
|
||||
for (j = 1; j <= max_vN; j++){
|
||||
k = max_ulogN;
|
||||
u_min = 0;
|
||||
box_p_min = start_point.cwiseProduct(plane_w + plane_v) + plane_u * plane_u_min + plane_v * box_length * (j-1);
|
||||
box_p_max = plane_u * plane_u_max + start_point.cwiseProduct(plane_w + plane_v) + plane_v * box_length * j + plane_w * box_length;
|
||||
//printf("---- UPSIDE (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box_p_min[0],box_p_min[1],box_p_min[2],box_p_max[0],box_p_max[1],box_p_max[2]);
|
||||
|
||||
while (k>=0){
|
||||
box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length;
|
||||
box.vertex_min[0] = box_p_min(0);
|
||||
box.vertex_min[1] = box_p_min(1);
|
||||
box.vertex_min[2] = box_p_min(2);
|
||||
box.vertex_max[0] = box_p(0);
|
||||
box.vertex_max[1] = box_p(1);
|
||||
box.vertex_max[2] = box_p(2);
|
||||
if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k);
|
||||
k = k-1;
|
||||
}
|
||||
k = max_ulogN;
|
||||
u_max = 0;
|
||||
while (k>=0){
|
||||
box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length;
|
||||
box.vertex_min[0] = box_p(0);
|
||||
box.vertex_min[1] = box_p(1);
|
||||
box.vertex_min[2] = box_p(2);
|
||||
box.vertex_max[0] = box_p_max(0);
|
||||
box.vertex_max[1] = box_p_max(1);
|
||||
box.vertex_max[2] = box_p_max(2);
|
||||
if (!check_box(cur_pose, axis, theta, depth, box)) u_max = u_max + pow(2,k);
|
||||
|
||||
k = k-1;
|
||||
}
|
||||
u_max = max(0, max_uN - u_max - 1);
|
||||
box_found = false;
|
||||
//printf("---- u_min -> u_max: %d->%d\n",u_min,u_max);
|
||||
for (k = u_min; k <= u_max; k++){
|
||||
box_p = box_p_min + plane_u * box_length * k;
|
||||
box.vertex_min[0] = box_p(0);
|
||||
box.vertex_min[1] = box_p(1);
|
||||
box.vertex_min[2] = box_p(2);
|
||||
box.vertex_max[0] = box_p(0) + box_length;
|
||||
box.vertex_max[1] = box_p(1) + box_length;
|
||||
box.vertex_max[2] = box_p(2) + box_length;
|
||||
if (check_box_in_env(box)){
|
||||
//printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]);
|
||||
box_found = true;
|
||||
boxes.push_back(box);
|
||||
}
|
||||
}
|
||||
if (box_found) {
|
||||
flag = true;
|
||||
} else {
|
||||
if (j>1) break;
|
||||
}
|
||||
}
|
||||
for (j = 1; j <= max_vN; j++){
|
||||
k = max_ulogN;
|
||||
u_min = 0;
|
||||
box_p_min = start_point.cwiseProduct(plane_w + plane_v) + plane_u * plane_u_min - plane_v * box_length * j;
|
||||
box_p_max = plane_u * plane_u_max + start_point.cwiseProduct(plane_w + plane_v) - plane_v * box_length * (j-1) + plane_w * box_length;
|
||||
//printf("---- DOWNSIDE (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box_p_min[0],box_p_min[1],box_p_min[2],box_p_max[0],box_p_max[1],box_p_max[2]);
|
||||
|
||||
while (k>=0){
|
||||
box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length;
|
||||
box.vertex_min[0] = box_p_min(0);
|
||||
box.vertex_min[1] = box_p_min(1);
|
||||
box.vertex_min[2] = box_p_min(2);
|
||||
box.vertex_max[0] = box_p(0);
|
||||
box.vertex_max[1] = box_p(1);
|
||||
box.vertex_max[2] = box_p(2);
|
||||
if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k);
|
||||
k = k-1;
|
||||
}
|
||||
k = max_ulogN;
|
||||
u_max = 0;
|
||||
while (k>=0){
|
||||
box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length;
|
||||
box.vertex_min[0] = box_p(0);
|
||||
box.vertex_min[1] = box_p(1);
|
||||
box.vertex_min[2] = box_p(2);
|
||||
box.vertex_max[0] = box_p_max(0);
|
||||
box.vertex_max[1] = box_p_max(1);
|
||||
box.vertex_max[2] = box_p_max(2);
|
||||
if (!check_box(cur_pose, axis, theta, depth, box)) {
|
||||
u_max = u_max + pow(2,k);
|
||||
// printf("-------- Not Included: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]);
|
||||
}
|
||||
|
||||
k = k-1;
|
||||
}
|
||||
u_max = max(0, max_uN - u_max - 1);
|
||||
//printf("---- u_min -> u_max: %d->%d\n",u_min,u_max);
|
||||
box_found = 0;
|
||||
for (k = u_min; k <= u_max; k++){
|
||||
box_p = box_p_min + plane_u * box_length * k;
|
||||
box.vertex_min[0] = box_p(0);
|
||||
box.vertex_min[1] = box_p(1);
|
||||
box.vertex_min[2] = box_p(2);
|
||||
box.vertex_max[0] = box_p(0) + box_length;
|
||||
box.vertex_max[1] = box_p(1) + box_length;
|
||||
box.vertex_max[2] = box_p(2) + box_length;
|
||||
if (check_box_in_env(box)){
|
||||
//printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]);
|
||||
box_found = 1;
|
||||
boxes.push_back(box);
|
||||
}
|
||||
}
|
||||
if (box_found) {
|
||||
flag = true;
|
||||
} else {
|
||||
if (j>1) break;
|
||||
}
|
||||
}
|
||||
if (!flag && i>0) break;
|
||||
}
|
||||
}
|
||||
|
||||
bool FOV_Checker::check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box){
|
||||
Eigen::Vector3d vertex[8];
|
||||
bool s;
|
||||
vertex[0] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_min[2]);
|
||||
vertex[1] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_max[2]);
|
||||
vertex[2] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_min[2]);
|
||||
vertex[3] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_max[2]);
|
||||
vertex[4] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_min[2]);
|
||||
vertex[5] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_max[2]);
|
||||
vertex[6] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_min[2]);
|
||||
vertex[7] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_max[2]);
|
||||
for (int i = 0; i < 8; i++){
|
||||
if (check_point(cur_pose, axis, theta, depth, vertex[i])){
|
||||
return true;
|
||||
}
|
||||
}
|
||||
Eigen::Vector3d center_point = (vertex[7]+vertex[0])/2.0;
|
||||
if (check_point(cur_pose, axis, theta, depth, center_point)){
|
||||
return true;
|
||||
}
|
||||
PlaneType plane[6];
|
||||
plane[0].p[0] = vertex[0];
|
||||
plane[0].p[1] = vertex[2];
|
||||
plane[0].p[2] = vertex[1];
|
||||
plane[0].p[3] = vertex[3];
|
||||
|
||||
plane[1].p[0] = vertex[0];
|
||||
plane[1].p[1] = vertex[4];
|
||||
plane[1].p[2] = vertex[2];
|
||||
plane[1].p[3] = vertex[6];
|
||||
|
||||
plane[2].p[0] = vertex[0];
|
||||
plane[2].p[1] = vertex[4];
|
||||
plane[2].p[2] = vertex[1];
|
||||
plane[2].p[3] = vertex[5];
|
||||
|
||||
plane[3].p[0] = vertex[4];
|
||||
plane[3].p[1] = vertex[6];
|
||||
plane[3].p[2] = vertex[5];
|
||||
plane[3].p[3] = vertex[7];
|
||||
|
||||
plane[4].p[0] = vertex[2];
|
||||
plane[4].p[1] = vertex[6];
|
||||
plane[4].p[2] = vertex[3];
|
||||
plane[4].p[3] = vertex[7];
|
||||
|
||||
plane[5].p[0] = vertex[1];
|
||||
plane[5].p[1] = vertex[5];
|
||||
plane[5].p[2] = vertex[3];
|
||||
plane[5].p[3] = vertex[7];
|
||||
if (check_surface(cur_pose, axis, theta, depth, plane[0]) || check_surface(cur_pose, axis, theta, depth, plane[1]) || check_surface(cur_pose, axis, theta, depth, plane[2]) || check_surface(cur_pose, axis, theta, depth, plane[3]) || check_surface(cur_pose, axis, theta, depth, plane[4]) || check_surface(cur_pose, axis, theta, depth, plane[5]))
|
||||
s = 1;
|
||||
else
|
||||
s = 0;
|
||||
return s;
|
||||
}
|
||||
|
||||
bool FOV_Checker::check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane){
|
||||
Eigen::Vector3d plane_p, plane_u, plane_v, plane_w, pc, p, vec;
|
||||
bool s;
|
||||
double t, vec_dot_u, vec_dot_v;
|
||||
plane_p = plane.p[0];
|
||||
plane_u = plane.p[1] - plane_p;
|
||||
plane_v = plane.p[2] - plane_p;
|
||||
if (check_line(cur_pose, axis, theta, depth, plane_p, plane_u) || check_line(cur_pose, axis, theta, depth, plane_p, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_u, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_v, plane_u)){
|
||||
s = 1;
|
||||
return s;
|
||||
}
|
||||
pc = plane_p + (plane.p[3]-plane.p[0])/2;
|
||||
if (check_point(cur_pose, axis, theta, depth, pc)){
|
||||
s = 1;
|
||||
return s;
|
||||
}
|
||||
plane_w = plane_u.cross(plane_v);
|
||||
p = plane_p - cur_pose;
|
||||
t = (p.dot(plane_w))/(axis.dot(plane_w));
|
||||
vec = cur_pose + t * axis - plane_p;
|
||||
vec_dot_u = vec.dot(plane_u)/plane_u.norm();
|
||||
vec_dot_v = vec.dot(plane_v)/plane_v.norm();
|
||||
if (t>=-eps_value && t<=depth && vec_dot_u>=-eps_value && vec_dot_u<=plane_u.norm() && vec_dot_v>=-eps_value && vec_dot_v <= plane_v.norm())
|
||||
s = 1;
|
||||
else
|
||||
s = 0;
|
||||
return s;
|
||||
}
|
||||
|
||||
bool FOV_Checker::check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec){
|
||||
Eigen::Vector3d p, vec_1, vec_2;
|
||||
double xl, yl, zl, xn, yn, zn, dot_1, dot_2, ln, pn, pl, l2, p2;
|
||||
double A, B, C, delta, t1, t2;
|
||||
bool s;
|
||||
p = line_p - cur_pose;
|
||||
xl = line_vec(0); yl = line_vec(1); zl = line_vec(2);
|
||||
xn = axis(0); yn = axis(1); zn = axis(2);
|
||||
vec_1 = line_p - cur_pose;
|
||||
vec_2 = line_p + line_vec - cur_pose;
|
||||
dot_1 = vec_1.dot(axis);
|
||||
dot_2 = vec_2.dot(axis);
|
||||
//printf("xl yl zl: %0.4f, %0.4f, %0.4f\n", xl, yl, zl);
|
||||
//printf("xn yn zn: %0.4f, %0.4f, %0.4f\n", xn, yn, zn);
|
||||
//printf("dot_1, dot_2, %0.4f, %0.4f\n",dot_1, dot_2);
|
||||
if ((dot_1<0 && dot_2<0) || (dot_1>depth && dot_2>depth)){
|
||||
s = false;
|
||||
return s;
|
||||
}
|
||||
ln = xl*xn+yl*yn+zl*zn;
|
||||
pn = p(0)*xn+p(1)*yn+p(2)*zn;
|
||||
pl = p(0)*xl+p(1)*yl+p(2)*zl;
|
||||
l2 = xl*xl+yl*yl+zl*zl;
|
||||
p2 = p.norm()*p.norm();
|
||||
//printf("ln: %0.4f\n",ln);
|
||||
//printf("pn:%0.4f\n",pn);
|
||||
//printf("pl:%0.4f\n",pl);
|
||||
//printf("l2:%0.4f\n",l2);
|
||||
//printf("p2:%0.4f\n",p2);
|
||||
//printf("theta, cos(theta):%0.4f %0.4f\n",theta,cos(theta));
|
||||
A = ln * ln - l2 * cos(theta) * cos(theta);
|
||||
B = 2 * pn * ln - 2 * cos(theta) * cos(theta)*pl;
|
||||
C = pn * pn - p2 * cos(theta) * cos(theta);
|
||||
//printf("A:%0.4f, B:%0.4f, C:%0.4f\n", A,B,C);
|
||||
if (!(fabs(A)<=eps_value)){
|
||||
delta = B*B - 4*A*C;
|
||||
//printf("delta: %0.4f\n",delta);
|
||||
if (delta <= eps_value){
|
||||
if (A < -eps_value){
|
||||
s = false;
|
||||
return s;
|
||||
}
|
||||
else{
|
||||
s = true;
|
||||
return s;
|
||||
}
|
||||
} else {
|
||||
double sqrt_delta = sqrt(delta);
|
||||
t1 = (-B - sqrt_delta)/(2*A);
|
||||
t2 = (-B + sqrt_delta)/(2*A);
|
||||
if (t1>t2) swap(t1,t2);
|
||||
//printf("t1,t2: %0.4f,%0.4f\n",t1,t2);
|
||||
// printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1));
|
||||
if ((t1>=-eps_value && t1<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1)){
|
||||
s = true;
|
||||
return s;
|
||||
}
|
||||
// printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2));
|
||||
if ((t2>=-eps_value && t2<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2)){
|
||||
s = true;
|
||||
return s;
|
||||
}
|
||||
if (A>-eps_value && (t2<eps_value || t1>1-eps_value)){
|
||||
s = true;
|
||||
return s;
|
||||
}
|
||||
if (A<eps_value && t1<eps_value && t2>1-eps_value){
|
||||
s = true;
|
||||
return s;
|
||||
}
|
||||
s = false;
|
||||
}
|
||||
} else{
|
||||
if (!(fabs(B)<=eps_value)){
|
||||
s = (B>-eps_value && -C/B<=1+eps_value) || (B<eps_value && -C/B>=-eps_value);
|
||||
return s;
|
||||
}
|
||||
else {
|
||||
s = C>=-eps_value;
|
||||
return s;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool FOV_Checker::check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point){
|
||||
Eigen::Vector3d vec;
|
||||
double proj_len;
|
||||
bool s;
|
||||
vec = point-cur_pose;
|
||||
if (vec.transpose()*vec < 0.4 * box_length * box_length){
|
||||
return true;
|
||||
}
|
||||
proj_len = vec.dot(axis);
|
||||
if (proj_len > depth){
|
||||
s = false;
|
||||
return s;
|
||||
}
|
||||
//printf("acos: %0.4f\n",acos(proj_len/vec.norm()));
|
||||
if (fabs(vec.norm()) <= 1e-4 || acos(proj_len/vec.norm()) <= theta + 0.0175)
|
||||
s = true;
|
||||
else
|
||||
s = false;
|
||||
return s;
|
||||
}
|
||||
|
||||
bool FOV_Checker::check_box_in_env(BoxPointType box){
|
||||
if (box.vertex_min[0] >= env.vertex_min[0]-eps_value && box.vertex_min[1] >= env.vertex_min[1]-eps_value && box.vertex_min[2] >= env.vertex_min[2]-eps_value && box.vertex_max[0]<= env.vertex_max[0]+eps_value && box.vertex_max[1]<= env.vertex_max[1]+eps_value && box.vertex_max[2]<= env.vertex_max[2]+eps_value){
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,33 @@
|
||||
// Include Files
|
||||
#pragma once
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
#include "ikd-Tree/ikd_Tree.h"
|
||||
#include <Eigen/Core>
|
||||
#include <algorithm>
|
||||
|
||||
#define eps_value 1e-6
|
||||
|
||||
struct PlaneType{
|
||||
Eigen::Vector3d p[4];
|
||||
};
|
||||
|
||||
class FOV_Checker{
|
||||
public:
|
||||
FOV_Checker();
|
||||
~FOV_Checker();
|
||||
void Set_Env(BoxPointType env_param);
|
||||
void Set_BoxLength(double box_len_param);
|
||||
void check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector<BoxPointType> &boxes);
|
||||
bool check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box);
|
||||
bool check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec);
|
||||
bool check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane);
|
||||
bool check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point);
|
||||
bool check_box_in_env(BoxPointType box);
|
||||
private:
|
||||
BoxPointType env;
|
||||
double box_length;
|
||||
FILE *fp;
|
||||
|
||||
};
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
# Prerequisites
|
||||
*.d
|
||||
|
||||
# Compiled Object files
|
||||
*.slo
|
||||
*.lo
|
||||
*.o
|
||||
*.obj
|
||||
|
||||
# Precompiled Headers
|
||||
*.gch
|
||||
*.pch
|
||||
|
||||
# Compiled Dynamic libraries
|
||||
*.so
|
||||
*.dylib
|
||||
*.dll
|
||||
|
||||
# Fortran module files
|
||||
*.mod
|
||||
*.smod
|
||||
|
||||
# Compiled Static libraries
|
||||
*.lai
|
||||
*.la
|
||||
*.a
|
||||
*.lib
|
||||
|
||||
# Executables
|
||||
*.exe
|
||||
*.out
|
||||
*.app
|
||||
@@ -0,0 +1,390 @@
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef ESEKFOM_EKF_HPP
|
||||
#define ESEKFOM_EKF_HPP
|
||||
|
||||
|
||||
#include <vector>
|
||||
#include <cstdlib>
|
||||
|
||||
#include <boost/bind/bind.hpp>
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Eigen>
|
||||
#include <Eigen/Sparse>
|
||||
|
||||
#include "../mtk/types/vect.hpp"
|
||||
#include "../mtk/types/SOn.hpp"
|
||||
#include "../mtk/types/S2.hpp"
|
||||
#include "../mtk/types/SEn.hpp"
|
||||
#include "../mtk/startIdx.hpp"
|
||||
#include "../mtk/build_manifold.hpp"
|
||||
#include "util.hpp"
|
||||
|
||||
namespace esekfom {
|
||||
|
||||
using namespace Eigen;
|
||||
|
||||
template<typename T>
|
||||
struct dyn_share_modified
|
||||
{
|
||||
bool valid;
|
||||
bool converge;
|
||||
T M_Noise;
|
||||
Eigen::Matrix<T, Eigen::Dynamic, 1> z;
|
||||
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_x;
|
||||
Eigen::Matrix<T, 6, 1> z_IMU;
|
||||
Eigen::Matrix<T, 6, 1> R_IMU;
|
||||
bool satu_check[6];
|
||||
};
|
||||
|
||||
template<typename state, int process_noise_dof, typename input = state, typename measurement=state, int measurement_noise_dof=0>
|
||||
class esekf{
|
||||
|
||||
typedef esekf self;
|
||||
enum{
|
||||
n = state::DOF, m = state::DIM, l = measurement::DOF
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
typedef typename state::scalar scalar_type;
|
||||
typedef Matrix<scalar_type, n, n> cov;
|
||||
typedef Matrix<scalar_type, m, n> cov_;
|
||||
typedef SparseMatrix<scalar_type> spMt;
|
||||
typedef Matrix<scalar_type, n, 1> vectorized_state;
|
||||
typedef Matrix<scalar_type, m, 1> flatted_state;
|
||||
typedef flatted_state processModel(state &, const input &);
|
||||
typedef Eigen::Matrix<scalar_type, m, n> processMatrix1(state &, const input &);
|
||||
typedef Eigen::Matrix<scalar_type, m, process_noise_dof> processMatrix2(state &, const input &);
|
||||
typedef Eigen::Matrix<scalar_type, process_noise_dof, process_noise_dof> processnoisecovariance;
|
||||
|
||||
typedef void measurementModel_dyn_share_modified(state &, dyn_share_modified<scalar_type> &);
|
||||
typedef Eigen::Matrix<scalar_type ,l, n> measurementMatrix1(state &);
|
||||
typedef Eigen::Matrix<scalar_type , Eigen::Dynamic, n> measurementMatrix1_dyn(state &);
|
||||
typedef Eigen::Matrix<scalar_type ,l, measurement_noise_dof> measurementMatrix2(state &);
|
||||
typedef Eigen::Matrix<scalar_type ,Eigen::Dynamic, Eigen::Dynamic> measurementMatrix2_dyn(state &);
|
||||
typedef Eigen::Matrix<scalar_type, measurement_noise_dof, measurement_noise_dof> measurementnoisecovariance;
|
||||
typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> measurementnoisecovariance_dyn;
|
||||
|
||||
esekf(const state &x = state(),
|
||||
const cov &P = cov::Identity()): x_(x), P_(P){};
|
||||
|
||||
void init_dyn_share_modified(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in)
|
||||
{
|
||||
f = f_in;
|
||||
f_x = f_x_in;
|
||||
// f_w = f_w_in;
|
||||
h_dyn_share_modified_1 = h_dyn_share_in;
|
||||
maximum_iter = 1;
|
||||
x_.build_S2_state();
|
||||
x_.build_SO3_state();
|
||||
x_.build_vect_state();
|
||||
x_.build_SEN_state();
|
||||
}
|
||||
|
||||
void init_dyn_share_modified_2h(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in1, measurementModel_dyn_share_modified h_dyn_share_in2)
|
||||
{
|
||||
f = f_in;
|
||||
f_x = f_x_in;
|
||||
// f_w = f_w_in;
|
||||
h_dyn_share_modified_1 = h_dyn_share_in1;
|
||||
h_dyn_share_modified_2 = h_dyn_share_in2;
|
||||
maximum_iter = 1;
|
||||
x_.build_S2_state();
|
||||
x_.build_SO3_state();
|
||||
x_.build_vect_state();
|
||||
x_.build_SEN_state();
|
||||
}
|
||||
|
||||
// iterated error state EKF propogation
|
||||
void predict(double &dt, processnoisecovariance &Q, const input &i_in, bool predict_state, bool prop_cov){
|
||||
if (predict_state)
|
||||
{
|
||||
flatted_state f_ = f(x_, i_in);
|
||||
x_.oplus(f_, dt);
|
||||
}
|
||||
|
||||
if (prop_cov)
|
||||
{
|
||||
flatted_state f_ = f(x_, i_in);
|
||||
// state x_before = x_;
|
||||
|
||||
cov_ f_x_ = f_x(x_, i_in);
|
||||
cov f_x_final;
|
||||
F_x1 = cov::Identity();
|
||||
for (std::vector<std::pair<std::pair<int, int>, int> >::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++) {
|
||||
int idx = (*it).first.first;
|
||||
int dim = (*it).first.second;
|
||||
int dof = (*it).second;
|
||||
for(int i = 0; i < n; i++){
|
||||
for(int j=0; j<dof; j++)
|
||||
{f_x_final(idx+j, i) = f_x_(dim+j, i);}
|
||||
}
|
||||
}
|
||||
|
||||
Matrix<scalar_type, 3, 3> res_temp_SO3;
|
||||
MTK::vect<3, scalar_type> seg_SO3;
|
||||
for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
|
||||
int idx = (*it).first;
|
||||
int dim = (*it).second;
|
||||
for(int i = 0; i < 3; i++){
|
||||
seg_SO3(i) = -1 * f_(dim + i) * dt;
|
||||
}
|
||||
MTK::SO3<scalar_type> res;
|
||||
res.w() = MTK::exp<scalar_type, 3>(res.vec(), seg_SO3, scalar_type(1/2));
|
||||
F_x1.template block<3, 3>(idx, idx) = res.normalized().toRotationMatrix();
|
||||
res_temp_SO3 = MTK::A_matrix(seg_SO3);
|
||||
for(int i = 0; i < n; i++){
|
||||
f_x_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_. template block<3, 1>(dim, i));
|
||||
}
|
||||
}
|
||||
|
||||
F_x1 += f_x_final * dt;
|
||||
P_ = F_x1 * P_ * (F_x1).transpose() + Q * (dt * dt);
|
||||
}
|
||||
}
|
||||
|
||||
bool update_iterated_dyn_share_modified() {
|
||||
dyn_share_modified<scalar_type> dyn_share;
|
||||
state x_propagated = x_;
|
||||
int dof_Measurement;
|
||||
double m_noise;
|
||||
for(int i=0; i<maximum_iter; i++)
|
||||
{
|
||||
dyn_share.valid = true;
|
||||
h_dyn_share_modified_1(x_, dyn_share);
|
||||
if(! dyn_share.valid)
|
||||
{
|
||||
return false;
|
||||
// continue;
|
||||
}
|
||||
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> z = dyn_share.z;
|
||||
// Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R = dyn_share.R;
|
||||
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x = dyn_share.h_x;
|
||||
// Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v = dyn_share.h_v;
|
||||
dof_Measurement = h_x.rows();
|
||||
m_noise = dyn_share.M_Noise;
|
||||
// dof_Measurement_noise = dyn_share.R.rows();
|
||||
// vectorized_state dx, dx_new;
|
||||
// x_.boxminus(dx, x_propagated);
|
||||
// dx_new = dx;
|
||||
// P_ = P_propagated;
|
||||
|
||||
Matrix<scalar_type, n, Eigen::Dynamic> PHT;
|
||||
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> HPHT;
|
||||
Matrix<scalar_type, n, Eigen::Dynamic> K_;
|
||||
// if(n > dof_Measurement)
|
||||
{
|
||||
PHT = P_. template block<n, 12>(0, 0) * h_x.transpose();
|
||||
HPHT = h_x * PHT.topRows(12);
|
||||
for (int m = 0; m < dof_Measurement; m++)
|
||||
{
|
||||
HPHT(m, m) += m_noise;
|
||||
}
|
||||
K_= PHT*HPHT.inverse();
|
||||
}
|
||||
Matrix<scalar_type, n, 1> dx_ = K_ * z; // - h) + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
|
||||
// state x_before = x_;
|
||||
|
||||
x_.boxplus(dx_);
|
||||
dyn_share.converge = true;
|
||||
|
||||
// L_ = P_;
|
||||
// Matrix<scalar_type, 3, 3> res_temp_SO3;
|
||||
// MTK::vect<3, scalar_type> seg_SO3;
|
||||
// for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
|
||||
// int idx = (*it).first;
|
||||
// for(int i = 0; i < 3; i++){
|
||||
// seg_SO3(i) = dx_(i + idx);
|
||||
// }
|
||||
// res_temp_SO3 = A_matrix(seg_SO3).transpose();
|
||||
// for(int i = 0; i < n; i++){
|
||||
// L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));
|
||||
// }
|
||||
// {
|
||||
// for(int i = 0; i < dof_Measurement; i++){
|
||||
// K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
|
||||
// }
|
||||
// }
|
||||
// for(int i = 0; i < n; i++){
|
||||
// L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
|
||||
// // P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
|
||||
// }
|
||||
// for(int i = 0; i < n; i++){
|
||||
// P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
|
||||
// }
|
||||
// }
|
||||
// Matrix<scalar_type, 2, 2> res_temp_S2;
|
||||
// MTK::vect<2, scalar_type> seg_S2;
|
||||
// for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
|
||||
// int idx = (*it).first;
|
||||
|
||||
// for(int i = 0; i < 2; i++){
|
||||
// seg_S2(i) = dx_(i + idx);
|
||||
// }
|
||||
|
||||
// Eigen::Matrix<scalar_type, 2, 3> Nx;
|
||||
// Eigen::Matrix<scalar_type, 3, 2> Mx;
|
||||
// x_.S2_Nx_yy(Nx, idx);
|
||||
// x_propagated.S2_Mx(Mx, seg_S2, idx);
|
||||
// res_temp_S2 = Nx * Mx;
|
||||
|
||||
// for(int i = 0; i < n; i++){
|
||||
// L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));
|
||||
// }
|
||||
|
||||
// {
|
||||
// for(int i = 0; i < dof_Measurement; i++){
|
||||
// K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
|
||||
// }
|
||||
// }
|
||||
// for(int i = 0; i < n; i++){
|
||||
// L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
|
||||
// }
|
||||
// for(int i = 0; i < n; i++){
|
||||
// P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
|
||||
// }
|
||||
// }
|
||||
// if(n > dof_Measurement)
|
||||
{
|
||||
P_ = P_ - K_*h_x*P_. template block<12, n>(0, 0);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void update_iterated_dyn_share_IMU() {
|
||||
|
||||
dyn_share_modified<scalar_type> dyn_share;
|
||||
for(int i=0; i<maximum_iter; i++)
|
||||
{
|
||||
dyn_share.valid = true;
|
||||
h_dyn_share_modified_2(x_, dyn_share);
|
||||
|
||||
Matrix<scalar_type, 6, 1> z = dyn_share.z_IMU;
|
||||
|
||||
Matrix<double, 30, 6> PHT;
|
||||
Matrix<double, 6, 30> HP;
|
||||
Matrix<double, 6, 6> HPHT;
|
||||
PHT.setZero();
|
||||
HP.setZero();
|
||||
HPHT.setZero();
|
||||
for (int l_ = 0; l_ < 6; l_++)
|
||||
{
|
||||
if (!dyn_share.satu_check[l_])
|
||||
{
|
||||
PHT.col(l_) = P_.col(15+l_) + P_.col(24+l_);
|
||||
HP.row(l_) = P_.row(15+l_) + P_.row(24+l_);
|
||||
}
|
||||
}
|
||||
for (int l_ = 0; l_ < 6; l_++)
|
||||
{
|
||||
if (!dyn_share.satu_check[l_])
|
||||
{
|
||||
HPHT.col(l_) = HP.col(15+l_) + HP.col(24+l_);
|
||||
}
|
||||
HPHT(l_, l_) += dyn_share.R_IMU(l_); //, l);
|
||||
}
|
||||
Eigen::Matrix<double, 30, 6> K = PHT * HPHT.inverse();
|
||||
|
||||
Matrix<scalar_type, n, 1> dx_ = K * z;
|
||||
|
||||
P_ -= K * HP;
|
||||
x_.boxplus(dx_);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void change_x(state &input_state)
|
||||
{
|
||||
x_ = input_state;
|
||||
|
||||
if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size())&&(!x_.SEN_state.size()))
|
||||
{
|
||||
x_.build_S2_state();
|
||||
x_.build_SO3_state();
|
||||
x_.build_vect_state();
|
||||
x_.build_SEN_state();
|
||||
}
|
||||
}
|
||||
|
||||
void change_P(cov &input_cov)
|
||||
{
|
||||
P_ = input_cov;
|
||||
}
|
||||
|
||||
const state& get_x() const {
|
||||
return x_;
|
||||
}
|
||||
const cov& get_P() const {
|
||||
return P_;
|
||||
}
|
||||
state x_;
|
||||
private:
|
||||
measurement m_;
|
||||
cov P_;
|
||||
spMt l_;
|
||||
spMt f_x_1;
|
||||
spMt f_x_2;
|
||||
cov F_x1 = cov::Identity();
|
||||
cov F_x2 = cov::Identity();
|
||||
cov L_ = cov::Identity();
|
||||
|
||||
processModel *f;
|
||||
processMatrix1 *f_x;
|
||||
processMatrix2 *f_w;
|
||||
|
||||
measurementMatrix1 *h_x;
|
||||
measurementMatrix2 *h_v;
|
||||
|
||||
measurementMatrix1_dyn *h_x_dyn;
|
||||
measurementMatrix2_dyn *h_v_dyn;
|
||||
|
||||
measurementModel_dyn_share_modified *h_dyn_share_modified_1;
|
||||
|
||||
measurementModel_dyn_share_modified *h_dyn_share_modified_2;
|
||||
|
||||
int maximum_iter = 0;
|
||||
scalar_type limit[n];
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
} // namespace esekfom
|
||||
|
||||
#endif // ESEKFOM_EKF_HPP
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef __MEKFOM_UTIL_HPP__
|
||||
#define __MEKFOM_UTIL_HPP__
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include "../mtk/src/mtkmath.hpp"
|
||||
namespace esekfom {
|
||||
|
||||
template <typename T1, typename T2>
|
||||
class is_same {
|
||||
public:
|
||||
operator bool() {
|
||||
return false;
|
||||
}
|
||||
};
|
||||
template<typename T1>
|
||||
class is_same<T1, T1> {
|
||||
public:
|
||||
operator bool() {
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class is_double {
|
||||
public:
|
||||
operator bool() {
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
template<>
|
||||
class is_double<double> {
|
||||
public:
|
||||
operator bool() {
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
static T
|
||||
id(const T &x)
|
||||
{
|
||||
return x;
|
||||
}
|
||||
|
||||
} // namespace esekfom
|
||||
|
||||
#endif // __MEKFOM_UTIL_HPP__
|
||||
@@ -0,0 +1,248 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/build_manifold.hpp
|
||||
* @brief Macro to automatically construct compound manifolds.
|
||||
*
|
||||
*/
|
||||
#ifndef MTK_AUTOCONSTRUCT_HPP_
|
||||
#define MTK_AUTOCONSTRUCT_HPP_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <boost/preprocessor/seq.hpp>
|
||||
#include <boost/preprocessor/cat.hpp>
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "src/SubManifold.hpp"
|
||||
#include "startIdx.hpp"
|
||||
|
||||
#ifndef PARSED_BY_DOXYGEN
|
||||
//////// internals //////
|
||||
|
||||
#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple
|
||||
|
||||
#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries))
|
||||
|
||||
#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)
|
||||
|
||||
#define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type()
|
||||
#define MTK_CONSTRUCTOR_COPY( type, id) id(id)
|
||||
#define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale);
|
||||
#define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale);
|
||||
#define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id);
|
||||
#define MTK_HAT( type, id) if(id.IDX == idx){id.hat(vec, res);}
|
||||
#define MTK_JACOB_RIGHT_INV( type, id) if(id.IDX == idx){id.Jacob_right_inv(vec, res);}
|
||||
#define MTK_JACOB_RIGHT( type, id) if(id.IDX == idx){id.Jacob_right(vec, res);}
|
||||
#define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);}
|
||||
#define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);}
|
||||
#define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);}
|
||||
#define MTK_OSTREAM( type, id) << __var.id << " "
|
||||
#define MTK_ISTREAM( type, id) >> __var.id
|
||||
#define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));}
|
||||
#define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));}
|
||||
#define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));}
|
||||
#define MTK_SEN_state( type, id) if(id.TYP == 4){(SEN_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));}
|
||||
|
||||
#define MTK_SUBVARLIST(seq, S2state, SO3state, SENstate) \
|
||||
BOOST_PP_FOR_1( \
|
||||
( \
|
||||
BOOST_PP_SEQ_SIZE(seq), \
|
||||
BOOST_PP_SEQ_HEAD(seq), \
|
||||
BOOST_PP_SEQ_TAIL(seq) (~), \
|
||||
0,\
|
||||
0,\
|
||||
S2state,\
|
||||
SO3state,\
|
||||
SENstate ),\
|
||||
MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT)
|
||||
|
||||
#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
|
||||
MTK::SubManifold<type, dof, dim> id;
|
||||
#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state, SENstate) \
|
||||
MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
|
||||
enum {DOF = type::DOF + dof}; \
|
||||
enum {DIM = type::DIM+dim}; \
|
||||
typedef type::scalar scalar;
|
||||
|
||||
#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state
|
||||
#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state, SENstate) \
|
||||
MTK_APPLY_MACRO_ON_TUPLE(~, \
|
||||
BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \
|
||||
( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate))
|
||||
|
||||
#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state
|
||||
|
||||
//! this used to be BOOST_PP_TUPLE_ELEM_4_0:
|
||||
#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g, h) a
|
||||
|
||||
#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state
|
||||
#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate) ( \
|
||||
BOOST_PP_DEC(len), \
|
||||
BOOST_PP_SEQ_HEAD(seq), \
|
||||
BOOST_PP_SEQ_TAIL(seq), \
|
||||
dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\
|
||||
dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\
|
||||
S2state,\
|
||||
SO3state,\
|
||||
SENstate )
|
||||
|
||||
#endif /* not PARSED_BY_DOXYGEN */
|
||||
|
||||
|
||||
/**
|
||||
* Construct a manifold.
|
||||
* @param name is the class-name of the manifold,
|
||||
* @param entries is the list of sub manifolds
|
||||
*
|
||||
* Entries must be given in a list like this:
|
||||
* @code
|
||||
* typedef MTK::trafo<MTK::SO3<double> > Pose;
|
||||
* typedef MTK::vect<double, 3> Vec3;
|
||||
* MTK_BUILD_MANIFOLD(imu_state,
|
||||
* ((Pose, pose))
|
||||
* ((Vec3, vel))
|
||||
* ((Vec3, acc_bias))
|
||||
* )
|
||||
* @endcode
|
||||
* Whitespace is optional, but the double parentheses are necessary.
|
||||
* Construction is done entirely in preprocessor.
|
||||
* After construction @a name is also a manifold. Its members can be
|
||||
* accessed by names given in @a entries.
|
||||
*
|
||||
* @note Variable types are not allowed to have commas, thus types like
|
||||
* @c vect<double, 3> need to be typedef'ed ahead.
|
||||
*/
|
||||
#define MTK_BUILD_MANIFOLD(name, entries) \
|
||||
struct name { \
|
||||
typedef name self; \
|
||||
std::vector<std::pair<int, int> > S2_state;\
|
||||
std::vector<std::pair<int, int> > SO3_state;\
|
||||
std::vector<std::pair<std::pair<int, int>, int> > vect_state;\
|
||||
std::vector<std::pair<std::pair<int, int>, int> > SEN_state;\
|
||||
MTK_SUBVARLIST(entries, S2_state, SO3_state, SEN_state) \
|
||||
name ( \
|
||||
MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \
|
||||
) : \
|
||||
MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\
|
||||
int getDOF() const { return DOF; } \
|
||||
void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { \
|
||||
MTK_TRANSFORM(MTK_BOXPLUS, entries)\
|
||||
} \
|
||||
void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) { \
|
||||
MTK_TRANSFORM(MTK_OPLUS, entries)\
|
||||
} \
|
||||
void boxminus(MTK::vectview<scalar,DOF> __res, const name& __oth) const { \
|
||||
MTK_TRANSFORM(MTK_BOXMINUS, entries)\
|
||||
} \
|
||||
friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \
|
||||
return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \
|
||||
} \
|
||||
void build_S2_state(){\
|
||||
MTK_TRANSFORM(MTK_S2_state, entries)\
|
||||
}\
|
||||
void build_vect_state(){\
|
||||
MTK_TRANSFORM(MTK_vect_state, entries)\
|
||||
}\
|
||||
void build_SO3_state(){\
|
||||
MTK_TRANSFORM(MTK_SO3_state, entries)\
|
||||
}\
|
||||
void build_SEN_state(){\
|
||||
MTK_TRANSFORM(MTK_SEN_state, entries)\
|
||||
}\
|
||||
void Lie_hat(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
|
||||
MTK_TRANSFORM(MTK_HAT, entries)\
|
||||
}\
|
||||
void Lie_Jacob_Right_Inv(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
|
||||
MTK_TRANSFORM(MTK_JACOB_RIGHT_INV, entries)\
|
||||
}\
|
||||
void Lie_Jacob_Right(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
|
||||
MTK_TRANSFORM(MTK_JACOB_RIGHT, entries)\
|
||||
}\
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) {\
|
||||
MTK_TRANSFORM(MTK_S2_hat, entries)\
|
||||
}\
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res, int idx) {\
|
||||
MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\
|
||||
}\
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, Eigen::Matrix<scalar, 2, 1> dx, int idx) {\
|
||||
MTK_TRANSFORM(MTK_S2_Mx, entries)\
|
||||
}\
|
||||
friend std::istream& operator>>(std::istream& __is, name& __var){ \
|
||||
return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \
|
||||
} \
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /*MTK_AUTOCONSTRUCT_HPP_*/
|
||||
@@ -0,0 +1,123 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/SubManifold.hpp
|
||||
* @brief Defines the SubManifold class
|
||||
*/
|
||||
|
||||
|
||||
#ifndef SUBMANIFOLD_HPP_
|
||||
#define SUBMANIFOLD_HPP_
|
||||
|
||||
|
||||
#include "vectview.hpp"
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
/**
|
||||
* @ingroup SubManifolds
|
||||
* Helper class for compound manifolds.
|
||||
* This class wraps a manifold T and provides an enum IDX refering to the
|
||||
* index of the SubManifold within the compound manifold.
|
||||
*
|
||||
* Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds".
|
||||
*
|
||||
* @tparam T The manifold type of the sub-type
|
||||
* @tparam idx The index of the sub-type within the compound manifold
|
||||
*/
|
||||
template<class T, int idx, int dim>
|
||||
struct SubManifold : public T
|
||||
{
|
||||
enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ };
|
||||
//! manifold type
|
||||
typedef T type;
|
||||
|
||||
//! Construct from derived type
|
||||
template<class X>
|
||||
explicit
|
||||
SubManifold(const X& t) : T(t) {};
|
||||
|
||||
//! Construct from internal type
|
||||
//explicit
|
||||
SubManifold(const T& t) : T(t) {};
|
||||
|
||||
//! inherit assignment operator
|
||||
using T::operator=;
|
||||
|
||||
};
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
#endif /* SUBMANIFOLD_HPP_ */
|
||||
@@ -0,0 +1,294 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/mtkmath.hpp
|
||||
* @brief several math utility functions.
|
||||
*/
|
||||
|
||||
#ifndef MTKMATH_H_
|
||||
#define MTKMATH_H_
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <boost/math/tools/precision.hpp>
|
||||
|
||||
#include "../types/vect.hpp"
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.1415926535897932384626433832795
|
||||
#endif
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<class Manifold>
|
||||
struct traits {
|
||||
typedef typename Manifold::scalar scalar;
|
||||
enum {DOF = Manifold::DOF};
|
||||
typedef vect<DOF, scalar> vectorized_type;
|
||||
typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<float> : traits<Scalar<float> > {};
|
||||
template<>
|
||||
struct traits<double> : traits<Scalar<double> > {};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
/**
|
||||
* \defgroup MTKMath Mathematical helper functions
|
||||
*/
|
||||
//@{
|
||||
|
||||
//! constant @f$ \pi @f$
|
||||
const double pi = M_PI;
|
||||
|
||||
template<class scalar> inline scalar tolerance();
|
||||
|
||||
template<> inline float tolerance<float >() { return 1e-5f; }
|
||||
template<> inline double tolerance<double>() { return 1e-11; }
|
||||
|
||||
|
||||
/**
|
||||
* normalize @a x to @f$[-bound, bound] @f$.
|
||||
*
|
||||
* result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
|
||||
*/
|
||||
template<class scalar>
|
||||
inline scalar normalize(scalar x, scalar bound){ //not used
|
||||
if(std::fabs(x) <= bound) return x;
|
||||
int r = (int)(x *(scalar(1.0)/ bound));
|
||||
return x - ((r + (r>>31) + 1) & ~1)*bound;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate cosine and sinc of sqrt(x2).
|
||||
* @param x2 the squared angle must be non-negative
|
||||
* @return a pair containing cos and sinc of sqrt(x2)
|
||||
*/
|
||||
template<class scalar>
|
||||
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
|
||||
using std::sqrt;
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
|
||||
static scalar const taylor_2_bound = sqrt(taylor_0_bound);
|
||||
static scalar const taylor_n_bound = sqrt(taylor_2_bound);
|
||||
|
||||
assert(x2>=0 && "argument must be non-negative and must not be nan/-nan");
|
||||
|
||||
// FIXME check if bigger bounds are possible
|
||||
if(x2>=taylor_n_bound) {
|
||||
// slow fall-back solution
|
||||
scalar x = sqrt(x2);
|
||||
return std::make_pair(cos(x), sin(x)/x); // x is greater than 0.
|
||||
}
|
||||
|
||||
// FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd)
|
||||
// TODO Find optimal coefficients using Remez algorithm
|
||||
static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
|
||||
scalar cosi = 1., sinc=1;
|
||||
scalar term = -1/2. * x2;
|
||||
for(int i=0; i<3; ++i) {
|
||||
cosi += term;
|
||||
term *= inv[2*i];
|
||||
sinc += term;
|
||||
term *= -inv[2*i+1] * x2;
|
||||
}
|
||||
|
||||
return std::make_pair(cosi, sinc);
|
||||
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
res << 0, -v[2], v[1],
|
||||
v[2], 0, -v[0],
|
||||
-v[1], v[0], 0;
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv_trans(const Base& v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
if(v.norm() > MTK::tolerance<typename Base::scalar>())
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
if(v.norm() > MTK::tolerance<typename Base::scalar>())
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() - 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template<typename scalar>
|
||||
Eigen::Matrix<scalar, 2, 3> S2_w_expw_( Eigen::Matrix<scalar, 2, 1> v, scalar length)
|
||||
{
|
||||
Eigen::Matrix<scalar, 2, 3> res;
|
||||
scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]);
|
||||
if(norm < MTK::tolerance<scalar>()){
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
res(0, 1) = 1;
|
||||
res(1, 2) = 1;
|
||||
res /= length;
|
||||
}
|
||||
else{
|
||||
res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0,
|
||||
-v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm);
|
||||
res /= length;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v){
|
||||
Eigen::Matrix<typename Base::scalar, 3, 3> res;
|
||||
double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
|
||||
double norm = std::sqrt(squaredNorm);
|
||||
if(norm < MTK::tolerance<typename Base::scalar>()){
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
|
||||
}
|
||||
else{
|
||||
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
template<class scalar, int n>
|
||||
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
|
||||
scalar norm2 = vec.squaredNorm();
|
||||
std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
|
||||
scalar mult = cos_sinc.second * scale;
|
||||
result = mult * vec;
|
||||
return cos_sinc.first;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Inverse function to @c exp.
|
||||
*
|
||||
* @param result @c vectview to the result
|
||||
* @param w scalar part of input
|
||||
* @param vec vector part of input
|
||||
* @param scale scale result by this value
|
||||
* @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result
|
||||
*/
|
||||
template<class scalar, int n>
|
||||
void log(vectview<scalar, n> result,
|
||||
const scalar &w, const vectview<const scalar, n> vec,
|
||||
const scalar &scale, bool plus_minus_periodicity)
|
||||
{
|
||||
// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division
|
||||
scalar nv = vec.norm();
|
||||
if(nv < tolerance<scalar>()) {
|
||||
if(!plus_minus_periodicity && w < 0) {
|
||||
// find the maximal entry:
|
||||
int i;
|
||||
nv = vec.cwiseAbs().maxCoeff(&i);
|
||||
result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
|
||||
return;
|
||||
}
|
||||
nv = tolerance<scalar>();
|
||||
}
|
||||
scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );
|
||||
|
||||
result = s * vec;
|
||||
}
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
#endif /* MTKMATH_H_ */
|
||||
@@ -0,0 +1,168 @@
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/src/vectview.hpp
|
||||
* @brief Wrapper class around a pointer used as interface for plain vectors.
|
||||
*/
|
||||
|
||||
#ifndef VECTVIEW_HPP_
|
||||
#define VECTVIEW_HPP_
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
namespace MTK {
|
||||
|
||||
/**
|
||||
* A view to a vector.
|
||||
* Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions.
|
||||
* The dimension of the vector is given as template parameter and type-checked when used in expressions.
|
||||
* Data has to be modifiable.
|
||||
*
|
||||
* @tparam scalar Scalar type of the vector.
|
||||
* @tparam dim Dimension of the vector.
|
||||
*
|
||||
* @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct
|
||||
*/
|
||||
namespace internal {
|
||||
template<class Base, class T1, class T2>
|
||||
struct CovBlock {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> ConstType;
|
||||
};
|
||||
|
||||
template<class Base, class T1, class T2>
|
||||
struct CovBlock_ {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> ConstType;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2>
|
||||
struct CrossCovBlock {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> ConstType;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2>
|
||||
struct CrossCovBlock_ {
|
||||
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> Type;
|
||||
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> ConstType;
|
||||
};
|
||||
|
||||
template<class scalar, int dim>
|
||||
struct VectviewBase {
|
||||
typedef Eigen::Matrix<scalar, dim, 1> matrix_type;
|
||||
typedef typename matrix_type::MapType Type;
|
||||
typedef typename matrix_type::ConstMapType ConstType;
|
||||
};
|
||||
|
||||
template<class T>
|
||||
struct UnalignedType {
|
||||
typedef T type;
|
||||
};
|
||||
}
|
||||
|
||||
template<class scalar, int dim>
|
||||
class vectview : public internal::VectviewBase<scalar, dim>::Type {
|
||||
typedef internal::VectviewBase<scalar, dim> VectviewBase;
|
||||
public:
|
||||
//! plain matrix type
|
||||
typedef typename VectviewBase::matrix_type matrix_type;
|
||||
//! base type
|
||||
typedef typename VectviewBase::Type base;
|
||||
//! construct from pointer
|
||||
explicit
|
||||
vectview(scalar* data, int dim_=dim) : base(data, dim_) {}
|
||||
//! construct from plain matrix
|
||||
vectview(matrix_type& m) : base(m.data(), m.size()) {}
|
||||
//! construct from another @c vectview
|
||||
vectview(const vectview &v) : base(v) {}
|
||||
//! construct from Eigen::Block:
|
||||
template<class Base>
|
||||
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0), block.size()) {}
|
||||
template<class Base, bool PacketAccess>
|
||||
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0), block.size()) {}
|
||||
|
||||
//! inherit assignment operator
|
||||
using base::operator=;
|
||||
//! data pointer
|
||||
scalar* data() {return const_cast<scalar*>(base::data());}
|
||||
};
|
||||
|
||||
/**
|
||||
* @c const version of @c vectview.
|
||||
* Compared to @c Eigen::Map this implementation is const correct, i.e.,
|
||||
* data will not be modifiable using this view.
|
||||
*
|
||||
* @tparam scalar Scalar type of the vector.
|
||||
* @tparam dim Dimension of the vector.
|
||||
*
|
||||
* @sa vectview
|
||||
*/
|
||||
template<class scalar, int dim>
|
||||
class vectview<const scalar, dim> : public internal::VectviewBase<scalar, dim>::ConstType {
|
||||
typedef internal::VectviewBase<scalar, dim> VectviewBase;
|
||||
public:
|
||||
//! plain matrix type
|
||||
typedef typename VectviewBase::matrix_type matrix_type;
|
||||
//! base type
|
||||
typedef typename VectviewBase::ConstType base;
|
||||
//! construct from const pointer
|
||||
explicit
|
||||
vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {}
|
||||
//! construct from column vector
|
||||
template<int options>
|
||||
vectview(const Eigen::Matrix<scalar, dim, 1, options>& m) : base(m.data()) {}
|
||||
//! construct from row vector
|
||||
template<int options, int phony>
|
||||
vectview(const Eigen::Matrix<scalar, 1, dim, options, phony>& m) : base(m.data()) {}
|
||||
//! construct from another @c vectview
|
||||
vectview(vectview<scalar, dim> x) : base(x.data()) {}
|
||||
//! construct from base
|
||||
vectview(const base &x) : base(x) {}
|
||||
/**
|
||||
* Construct from Block
|
||||
* @todo adapt this, when Block gets const-correct
|
||||
*/
|
||||
template<class Base>
|
||||
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0)) {}
|
||||
template<class Base, bool PacketAccess>
|
||||
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0)) {}
|
||||
|
||||
};
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif /* VECTVIEW_HPP_ */
|
||||
@@ -0,0 +1,328 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/startIdx.hpp
|
||||
* @brief Tools to access sub-elements of compound manifolds.
|
||||
*/
|
||||
#ifndef GET_START_INDEX_H_
|
||||
#define GET_START_INDEX_H_
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "src/SubManifold.hpp"
|
||||
#include "src/vectview.hpp"
|
||||
|
||||
namespace MTK {
|
||||
|
||||
|
||||
/**
|
||||
* \defgroup SubManifolds Accessing Submanifolds
|
||||
* For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers
|
||||
* can be used to get sub-vectors or matrix-blocks of a corresponding big matrix.
|
||||
* E.g. for a type @a pose consisting of @a orient and @a trans the member pointers
|
||||
* @c &pose::orient and @c &pose::trans give all required information and are still
|
||||
* valid if the base type gets extended or the actual types of @a orient and @a trans
|
||||
* change (e.g. from 2D to 3D).
|
||||
*
|
||||
* @todo Maybe require manifolds to typedef MatrixType and VectorType, etc.
|
||||
*/
|
||||
//@{
|
||||
|
||||
/**
|
||||
* Determine the index of a sub-variable within a compound variable.
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
int getStartIdx( MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return idx;
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
int getStartIdx_( MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return dim;
|
||||
}
|
||||
|
||||
/**
|
||||
* Determine the degrees of freedom of a sub-variable within a compound variable.
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
int getDof( MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return T::DOF;
|
||||
}
|
||||
template<class Base, class T, int idx, int dim>
|
||||
int getDim( MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return T::DIM;
|
||||
}
|
||||
|
||||
/**
|
||||
* set the diagonal elements of a covariance matrix corresponding to a sub-variable
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
void setDiagonal(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
|
||||
MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
|
||||
{
|
||||
cov.diagonal().template segment<T::DOF>(idx).setConstant(val);
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
void setDiagonal_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
|
||||
MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
|
||||
{
|
||||
cov.diagonal().template segment<T::DIM>(dim).setConstant(val);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the subblock of corresponding to two members, i.e.
|
||||
* \code
|
||||
* Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
|
||||
* MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression;
|
||||
* MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans();
|
||||
* \endcode
|
||||
* lets you modify mixed covariance entries in a bigger covariance matrix.
|
||||
*/
|
||||
template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
|
||||
typename MTK::internal::CovBlock<Base, T1, T2>::Type
|
||||
subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
|
||||
MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
|
||||
{
|
||||
return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
|
||||
}
|
||||
|
||||
template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
|
||||
typename MTK::internal::CovBlock_<Base, T1, T2>::Type
|
||||
subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
|
||||
MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
|
||||
{
|
||||
return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
|
||||
}
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
|
||||
typename MTK::internal::CrossCovBlock<Base1, Base2, T1, T2>::Type
|
||||
subblock(Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
|
||||
{
|
||||
return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
|
||||
}
|
||||
|
||||
template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
|
||||
typename MTK::internal::CrossCovBlock_<Base1, Base2, T1, T2>::Type
|
||||
subblock_(Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
|
||||
{
|
||||
return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
|
||||
}
|
||||
/**
|
||||
* Get the subblock of corresponding to a member, i.e.
|
||||
* \code
|
||||
* Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
|
||||
* MTK::subblock(m, &Pose::orient) = some_expression;
|
||||
* \endcode
|
||||
* lets you modify covariance entries in a bigger covariance matrix.
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
typename MTK::internal::CovBlock_<Base, T, T>::Type
|
||||
subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
|
||||
MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return cov.template block<T::DIM, T::DIM>(dim, dim);
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
typename MTK::internal::CovBlock<Base, T, T>::Type
|
||||
subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
|
||||
MTK::SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return cov.template block<T::DOF, T::DOF>(idx, idx);
|
||||
}
|
||||
|
||||
template<typename Base>
|
||||
class get_cov {
|
||||
public:
|
||||
typedef Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> type;
|
||||
typedef const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> const_type;
|
||||
};
|
||||
|
||||
template<typename Base>
|
||||
class get_cov_ {
|
||||
public:
|
||||
typedef Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> type;
|
||||
typedef const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> const_type;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2>
|
||||
class get_cross_cov {
|
||||
public:
|
||||
typedef Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> type;
|
||||
typedef const type const_type;
|
||||
};
|
||||
|
||||
template<typename Base1, typename Base2>
|
||||
class get_cross_cov_ {
|
||||
public:
|
||||
typedef Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> type;
|
||||
typedef const type const_type;
|
||||
};
|
||||
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
vectview<typename Base::scalar, T::DIM>
|
||||
subvector_impl_(vectview<typename Base::scalar, Base::DIM> vec, SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return vec.template segment<T::DIM>(dim);
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
vectview<typename Base::scalar, T::DOF>
|
||||
subvector_impl(vectview<typename Base::scalar, Base::DOF> vec, SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return vec.template segment<T::DOF>(idx);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the subvector corresponding to a sub-manifold from a bigger vector.
|
||||
*/
|
||||
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
|
||||
vectview<Scalar, T::DIM>
|
||||
subvector_(vectview<Scalar, BaseDIM> vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl_(vec, ptr);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
|
||||
vectview<Scalar, T::DOF>
|
||||
subvector(vectview<Scalar, BaseDOF> vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl(vec, ptr);
|
||||
}
|
||||
|
||||
/**
|
||||
* @todo This should be covered already by subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
|
||||
*/
|
||||
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
|
||||
vectview<Scalar, T::DOF>
|
||||
subvector(Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl(vectview<Scalar, BaseDOF>(vec), ptr);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
|
||||
vectview<Scalar, T::DIM>
|
||||
subvector_(Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl_(vectview<Scalar, BaseDIM>(vec), ptr);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
|
||||
vectview<const Scalar, T::DIM>
|
||||
subvector_(const Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl_(vectview<const Scalar, BaseDIM>(vec), ptr);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
|
||||
vectview<const Scalar, T::DOF>
|
||||
subvector(const Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl(vectview<const Scalar, BaseDOF>(vec), ptr);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* const version of subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
|
||||
*/
|
||||
template<class Base, class T, int idx, int dim>
|
||||
vectview<const typename Base::scalar, T::DOF>
|
||||
subvector_impl(const vectview<const typename Base::scalar, Base::DOF> cvec, SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return cvec.template segment<T::DOF>(idx);
|
||||
}
|
||||
|
||||
template<class Base, class T, int idx, int dim>
|
||||
vectview<const typename Base::scalar, T::DIM>
|
||||
subvector_impl_(const vectview<const typename Base::scalar, Base::DIM> cvec, SubManifold<T, idx, dim> Base::*)
|
||||
{
|
||||
return cvec.template segment<T::DIM>(dim);
|
||||
}
|
||||
|
||||
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
|
||||
vectview<const Scalar, T::DOF>
|
||||
subvector(const vectview<const Scalar, BaseDOF> cvec, SubManifold<T, idx, dim> Base::* ptr)
|
||||
{
|
||||
return subvector_impl(cvec, ptr);
|
||||
}
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif // GET_START_INDEX_H_
|
||||
@@ -0,0 +1,326 @@
|
||||
// This is a NEW implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/types/S2.hpp
|
||||
* @brief Unit vectors on the sphere, or directions in 3D.
|
||||
*/
|
||||
#ifndef S2_H_
|
||||
#define S2_H_
|
||||
|
||||
|
||||
#include "vect.hpp"
|
||||
|
||||
#include "SOn.hpp"
|
||||
#include "../src/mtkmath.hpp"
|
||||
|
||||
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
/**
|
||||
* Manifold representation of @f$ S^2 @f$.
|
||||
* Used for unit vectors on the sphere or directions in 3D.
|
||||
*
|
||||
* @todo add conversions from/to polar angles?
|
||||
*/
|
||||
template<class _scalar = double, int den = 1, int num = 1, int S2_typ = 3>
|
||||
struct S2 {
|
||||
|
||||
typedef _scalar scalar;
|
||||
typedef vect<3, scalar> vect_type;
|
||||
typedef SO3<scalar> SO3_type;
|
||||
typedef typename vect_type::base vec3;
|
||||
scalar length = scalar(den)/scalar(num);
|
||||
enum {DOF=2, TYP = 1, DIM = 3};
|
||||
|
||||
//private:
|
||||
/**
|
||||
* Unit vector on the sphere, or vector pointing in a direction
|
||||
*/
|
||||
vect_type vec;
|
||||
|
||||
public:
|
||||
S2() {
|
||||
if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1));
|
||||
if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0);
|
||||
if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0);
|
||||
}
|
||||
S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) {
|
||||
vec.normalize();
|
||||
vec = vec * length;
|
||||
}
|
||||
|
||||
S2(const vect_type &_vec) : vec(_vec) {
|
||||
vec.normalize();
|
||||
vec = vec * length;
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, 3> delta, scalar scale = 1)
|
||||
{
|
||||
SO3_type res;
|
||||
res.w() = MTK::exp<scalar, 3>(res.vec(), delta, scalar(scale/2));
|
||||
vec = res.normalized().toRotationMatrix() * vec;
|
||||
}
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, 2> delta, scalar scale=1) {
|
||||
Eigen::Matrix<scalar, 3, 2> Bx;
|
||||
S2_Bx(Bx);
|
||||
vect_type Bu = Bx*delta;SO3_type res;
|
||||
res.w() = MTK::exp<scalar, 3>(res.vec(), Bu, scalar(scale/2));
|
||||
vec = res.normalized().toRotationMatrix() * vec;
|
||||
}
|
||||
|
||||
void boxminus(MTK::vectview<scalar, 2> res, const S2<scalar, den, num, S2_typ>& other) const {
|
||||
scalar v_sin = (MTK::hat(vec)*other.vec).norm();
|
||||
scalar v_cos = vec.transpose() * other.vec;
|
||||
scalar theta = std::atan2(v_sin, v_cos);
|
||||
if(v_sin < MTK::tolerance<scalar>())
|
||||
{
|
||||
if(std::fabs(theta) > MTK::tolerance<scalar>() )
|
||||
{
|
||||
res[0] = 3.1415926;
|
||||
res[1] = 0;
|
||||
}
|
||||
else{
|
||||
res[0] = 0;
|
||||
res[1] = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
S2<scalar, den, num, S2_typ> other_copy = other;
|
||||
Eigen::Matrix<scalar, 3, 2>Bx;
|
||||
other_copy.S2_Bx(Bx);
|
||||
res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec;
|
||||
}
|
||||
}
|
||||
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
Eigen::Matrix<scalar, 3, 3> skew_vec;
|
||||
skew_vec << scalar(0), -vec[2], vec[1],
|
||||
vec[2], scalar(0), -vec[0],
|
||||
-vec[1], vec[0], scalar(0);
|
||||
res = skew_vec;
|
||||
}
|
||||
|
||||
|
||||
void S2_Bx(Eigen::Matrix<scalar, 3, 2> &res)
|
||||
{
|
||||
if(S2_typ == 3)
|
||||
{
|
||||
if(vec[2] + length > tolerance<scalar>())
|
||||
{
|
||||
|
||||
res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]),
|
||||
-vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]),
|
||||
-vec[0], -vec[1];
|
||||
res /= length;
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
res(1, 1) = -1;
|
||||
res(2, 0) = 1;
|
||||
}
|
||||
}
|
||||
else if(S2_typ == 2)
|
||||
{
|
||||
if(vec[1] + length > tolerance<scalar>())
|
||||
{
|
||||
|
||||
res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]),
|
||||
-vec[0], -vec[2],
|
||||
-vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]);
|
||||
res /= length;
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
res(1, 1) = -1;
|
||||
res(2, 0) = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(vec[0] + length > tolerance<scalar>())
|
||||
{
|
||||
|
||||
res << -vec[1], -vec[2],
|
||||
length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]),
|
||||
-vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]);
|
||||
res /= length;
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
res(1, 1) = -1;
|
||||
res(2, 0) = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void S2_Nx(Eigen::Matrix<scalar, 2, 3> &res, S2<scalar, den, num, S2_typ>& subtrahend)
|
||||
{
|
||||
if((vec+subtrahend.vec).norm() > tolerance<scalar>())
|
||||
{
|
||||
Eigen::Matrix<scalar, 3, 2> Bx;
|
||||
S2_Bx(Bx);
|
||||
if((vec-subtrahend.vec).norm() > tolerance<scalar>())
|
||||
{
|
||||
scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm();
|
||||
scalar v_cos = vec.transpose() * subtrahend.vec;
|
||||
|
||||
res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length));
|
||||
}
|
||||
else
|
||||
{
|
||||
res = 1/length/length*Bx.transpose()*MTK::hat(vec);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "No N(x, y) for x=-y" << std::endl;
|
||||
std::exit(100);
|
||||
}
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
Eigen::Matrix<scalar, 3, 2> Bx;
|
||||
S2_Bx(Bx);
|
||||
res = 1/length/length*Bx.transpose()*MTK::hat(vec);
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
Eigen::Matrix<scalar, 3, 2> Bx;
|
||||
S2_Bx(Bx);
|
||||
if(delta.norm() < tolerance<scalar>())
|
||||
{
|
||||
res = -MTK::hat(vec)*Bx;
|
||||
}
|
||||
else{
|
||||
vect_type Bu = Bx*delta;
|
||||
SO3_type exp_delta;
|
||||
exp_delta.w() = MTK::exp<scalar, 3>(exp_delta.vec(), Bu, scalar(1/2));
|
||||
res = -exp_delta.normalized().toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx;
|
||||
}
|
||||
}
|
||||
|
||||
operator const vect_type&() const{
|
||||
return vec;
|
||||
}
|
||||
|
||||
const vect_type& get_vect() const {
|
||||
return vec;
|
||||
}
|
||||
|
||||
friend S2<scalar, den, num, S2_typ> operator*(const SO3<scalar>& rot, const S2<scalar, den, num, S2_typ>& dir)
|
||||
{
|
||||
S2<scalar, den, num, S2_typ> ret;
|
||||
ret.vec = rot.normalized() * dir.vec;
|
||||
return ret;
|
||||
}
|
||||
|
||||
scalar operator[](int idx) const {return vec[idx]; }
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const S2<scalar, den, num, S2_typ>& vec){
|
||||
return os << vec.vec.transpose() << " ";
|
||||
}
|
||||
friend std::istream& operator>>(std::istream &is, S2<scalar, den, num, S2_typ>& vec){
|
||||
for(int i=0; i<3; ++i)
|
||||
is >> vec.vec[i];
|
||||
vec.vec.normalize();
|
||||
vec.vec = vec.vec * vec.length;
|
||||
return is;
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
#endif /*S2_H_*/
|
||||
@@ -0,0 +1,334 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/types/SEn.hpp
|
||||
* @brief Standard Orthogonal Groups i.e.\ rotatation groups.
|
||||
*/
|
||||
#ifndef SEN_H_
|
||||
#define SEN_H_
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include "SOn.hpp"
|
||||
#include "vect.hpp"
|
||||
#include "../src/mtkmath.hpp"
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
|
||||
/**
|
||||
* Three-dimensional orientations represented as Quaternion.
|
||||
* It is assumed that the internal Quaternion always stays normalized,
|
||||
* should this not be the case, call inherited member function @c normalize().
|
||||
*/
|
||||
template<class _scalar = double, int num_of_vec_plus1 = 6, int dim_of_mat = 4, int Options = Eigen::AutoAlign>
|
||||
struct SEN {
|
||||
enum {DOF = num_of_vec_plus1, DIM = num_of_vec_plus1, TYP = 4};
|
||||
typedef _scalar scalar;
|
||||
typedef Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> base;
|
||||
typedef SO3<scalar> SO3_type;
|
||||
// typedef Eigen::Quaternion<scalar, Options> base;
|
||||
// typedef Eigen::Quaternion<scalar> Quaternion;
|
||||
typedef vect<DIM, scalar, Options> vect_type;
|
||||
SO3_type SO3_data;
|
||||
base mat;
|
||||
|
||||
/**
|
||||
* Construct from real part and three imaginary parts.
|
||||
* Quaternion is normalized after construction.
|
||||
*/
|
||||
// SEN(const base& src) : mat(src) {
|
||||
// // base::normalize();
|
||||
// }
|
||||
|
||||
/**
|
||||
* Construct from Eigen::Quaternion.
|
||||
* @note Non-normalized input may result result in spurious behavior.
|
||||
*/
|
||||
SEN(const base& src = base::Identity()) : mat(src) {}
|
||||
|
||||
/**
|
||||
* Construct from rotation matrix.
|
||||
* @note Invalid rotation matrices may lead to spurious behavior.
|
||||
*/
|
||||
// template<class Derived>
|
||||
// SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}
|
||||
|
||||
/**
|
||||
* Construct from arbitrary rotation type.
|
||||
* @note Invalid rotation matrices may lead to spurious behavior.
|
||||
*/
|
||||
// template<class Derived>
|
||||
// SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}
|
||||
|
||||
//! @name Manifold requirements
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
SEN delta = exp(vec, scale); // ?
|
||||
mat = mat * delta.mat;
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const SEN<scalar,num_of_vec_plus1,dim_of_mat, Options>& other) const {
|
||||
base error_mat = other.mat.inverse() * mat;
|
||||
res = log(error_mat);
|
||||
}
|
||||
//}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
SEN delta = exp(vec, scale);
|
||||
mat = mat * delta.mat;
|
||||
}
|
||||
|
||||
// void hat(MTK::vectview<const scalar, DOF>& v, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> &res) {
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
|
||||
Eigen::Matrix<scalar, 3, 3> psi;
|
||||
psi << 0, -v[2], v[1],
|
||||
v[2], 0, -v[0],
|
||||
-v[1], v[0], 0;
|
||||
res.block<3, 3>(0, 0) = psi;
|
||||
for(int i = 3; i < v.size() / 3 + 2; i++)
|
||||
{
|
||||
res.block<3, 1>(0, i) = v.segment<3>(i + (i-3)*3);
|
||||
}
|
||||
// return res;
|
||||
}
|
||||
|
||||
// void Jacob_right_inv(MTK::vectview<const scalar, DOF> vec, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> & res){
|
||||
void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
|
||||
res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
|
||||
Eigen::Matrix<scalar, 3, 3> M_v;
|
||||
Eigen::VectorXd vec_psi, vec_ro;
|
||||
Eigen::MatrixXd jac_v;
|
||||
Eigen::MatrixXd hat_v, hat_ro;
|
||||
vec_psi = vec.segment<3>(0);
|
||||
// Eigen::Matrix<scalar, 3, 1> ;
|
||||
SO3_data.hat(vec_psi, hat_v);
|
||||
SO3_data.Jacob_right_inv(vec_psi, jac_v);
|
||||
double norm = vec_psi.norm();
|
||||
for(int i = 0; i < vec.size() / 3; i++)
|
||||
{
|
||||
res.block<3, 3>(i*3, i*3) = jac_v;
|
||||
}
|
||||
for(int i = 1; i < vec.size() / 3; i++)
|
||||
{
|
||||
vec_ro = vec.segment<3>(i * 3);
|
||||
SO3_data.hat(vec_ro, hat_ro);
|
||||
if(norm > MTK::tolerance<scalar>())
|
||||
{
|
||||
res.block<3,3>(i*3, 0) = 0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v;
|
||||
}
|
||||
else
|
||||
{
|
||||
res.block<3,3>(i*3, 0) = 0.5 * hat_ro;
|
||||
}
|
||||
|
||||
}
|
||||
// return res;
|
||||
}
|
||||
|
||||
// void Jacob_right(MTK::vectview<const scalar, DOF> & vec, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> &res){
|
||||
void Jacob_right(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
|
||||
res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
|
||||
Eigen::MatrixXd hat_v, hat_ro;
|
||||
Eigen::VectorXd vec_psi, vec_ro;
|
||||
Eigen::MatrixXd jac_v;
|
||||
vec_psi = vec.segment<3>(0);
|
||||
// Eigen::Matrix<scalar, 3, 1> ;
|
||||
SO3_data.hat(vec_psi, hat_v);
|
||||
SO3_data.Jacob_right(vec_psi, jac_v);
|
||||
// double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
|
||||
// double norm = std::sqrt(squaredNorm);
|
||||
double norm = vec_psi.norm();
|
||||
for(int i = 0; i < vec.size() / 3; i++)
|
||||
{
|
||||
res.block<3, 3>(i*3, i*3) = jac_v;
|
||||
}
|
||||
for(int i = 1; i < vec.size() / 3; i++)
|
||||
{
|
||||
vec_ro = vec.segment<3>(i * 3);
|
||||
SO3_data.hat(vec_ro, hat_ro);
|
||||
if(norm > MTK::tolerance<scalar>())
|
||||
{
|
||||
res.block<3,3>(i*3, 0) = -1 * jac_v * (0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v) * jac_v;
|
||||
}
|
||||
else
|
||||
{
|
||||
res.block<3,3>(i*3, 0) = -0.5 * jac_v * hat_ro * jac_v;
|
||||
}
|
||||
|
||||
}
|
||||
// return res;
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const SEN<scalar, DOF, dim_of_mat, Options>& q){
|
||||
for(int i=0; i<dim_of_mat; i++)
|
||||
{
|
||||
for(int j = 0; j < dim_of_mat; j++)
|
||||
{
|
||||
os << q.mat(i, j) << " ";
|
||||
}
|
||||
}
|
||||
return os;
|
||||
}
|
||||
|
||||
friend std::istream& operator>>(std::istream &is, SEN<scalar, DOF, dim_of_mat, Options>& q){
|
||||
// vect<dim_of_mat * dim_of_mat,scalar> coeffs;
|
||||
for(int i=0; i<dim_of_mat; i++)
|
||||
{
|
||||
for(int j = 0; j < dim_of_mat; j++)
|
||||
{
|
||||
is >> q.mat(i, j);
|
||||
}
|
||||
}
|
||||
// is >> q.mat;
|
||||
// coeffs;
|
||||
// q.coeffs() = coeffs.normalized();
|
||||
return is;
|
||||
}
|
||||
|
||||
//! @name Helper functions
|
||||
//{
|
||||
/**
|
||||
* Calculate the exponential map. In matrix terms this would correspond
|
||||
* to the Rodrigues formula.
|
||||
*/
|
||||
// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround
|
||||
// static SO3 exp(MTK::vectview<const scalar, 3> dvec, scalar scale = 1){
|
||||
static SEN exp(const Eigen::Matrix<scalar, DOF, 1>& dvec, scalar scale = 1){
|
||||
SEN res;
|
||||
res.mat = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Identity();
|
||||
Eigen::Matrix<scalar, 3, 3> exp_; //, jac;
|
||||
Eigen::MatrixXd jac;
|
||||
Eigen::Matrix<scalar, 3, 1> psi;
|
||||
Eigen::VectorXd minus_psi;
|
||||
psi = dvec.template block<3,1>(0, 0);
|
||||
minus_psi = -psi;
|
||||
SO3_type SO3_temp;
|
||||
exp_ = SO3_type::exp(psi);
|
||||
SO3_temp.Jacob_right(minus_psi, jac);
|
||||
res.mat.template block<3,3>(0, 0) = exp_;
|
||||
for(int i = 3; i < DOF / 3 + 2; i++)
|
||||
{
|
||||
res.mat.template block<3, 1>(0, i) = jac * dvec.template block<3,1>(i + (i-3)*3,0);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
/**
|
||||
* Calculate the inverse of @c exp.
|
||||
* Only guarantees that <code>exp(log(x)) == x </code>
|
||||
*/
|
||||
static Eigen::Matrix<scalar, DOF, 1> log(base &orient){
|
||||
Eigen::Matrix<scalar, DOF, 1> res;
|
||||
Eigen::Matrix<scalar, 3, 1> psi;
|
||||
Eigen::VectorXd minus_psi;
|
||||
Eigen::Matrix<scalar, 3, 3> mat_psi;
|
||||
Eigen::MatrixXd jac;
|
||||
mat_psi = orient.template block<3, 3>(0, 0);
|
||||
SO3_type SO3_temp;
|
||||
SO3_type exp_psi(mat_psi);
|
||||
psi = SO3_type::log(exp_psi);
|
||||
minus_psi = -psi;
|
||||
SO3_temp.Jacob_right_inv(minus_psi, jac);
|
||||
for(int i = 3; i < dim_of_mat; i++)
|
||||
{
|
||||
res.template block<3,1>(i + (i-3)*3,0) = jac * orient.template block<3, 1>(0, i);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif /*SON_H_*/
|
||||
|
||||
@@ -0,0 +1,365 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/types/SOn.hpp
|
||||
* @brief Standard Orthogonal Groups i.e.\ rotatation groups.
|
||||
*/
|
||||
#ifndef SON_H_
|
||||
#define SON_H_
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include "vect.hpp"
|
||||
#include "../src/mtkmath.hpp"
|
||||
|
||||
|
||||
namespace MTK {
|
||||
|
||||
|
||||
/**
|
||||
* Two-dimensional orientations represented as scalar.
|
||||
* There is no guarantee that the representing scalar is within any interval,
|
||||
* but the result of boxminus will always have magnitude @f$\le\pi @f$.
|
||||
*/
|
||||
template<class _scalar = double, int Options = Eigen::AutoAlign>
|
||||
struct SO2 : public Eigen::Rotation2D<_scalar> {
|
||||
enum {DOF = 1, DIM = 2, TYP = 3};
|
||||
|
||||
typedef _scalar scalar;
|
||||
typedef Eigen::Rotation2D<scalar> base;
|
||||
typedef vect<DIM, scalar, Options> vect_type;
|
||||
|
||||
//! Construct from angle
|
||||
SO2(const scalar& angle = 0) : base(angle) { }
|
||||
|
||||
//! Construct from Eigen::Rotation2D
|
||||
SO2(const base& src) : base(src) {}
|
||||
|
||||
/**
|
||||
* Construct from 2D vector.
|
||||
* Resulting orientation will rotate the first unit vector to point to vec.
|
||||
*/
|
||||
SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {};
|
||||
|
||||
|
||||
//! Calculate @c this->inverse() * @c r
|
||||
SO2 operator%(const base &r) const {
|
||||
return base::inverse() * r;
|
||||
}
|
||||
|
||||
//! Calculate @c this->inverse() * @c r
|
||||
template<class Derived>
|
||||
vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
|
||||
return base::inverse() * vec;
|
||||
}
|
||||
|
||||
//! Calculate @c *this * @c r.inverse()
|
||||
SO2 operator/(const SO2 &r) const {
|
||||
return *this * r.inverse();
|
||||
}
|
||||
|
||||
//! Gets the angle as scalar.
|
||||
operator scalar() const {
|
||||
return base::angle();
|
||||
}
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
//! @name Manifold requirements
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
base::angle() += scale * vec[0];
|
||||
}
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
base::angle() += scale * vec[0];
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const SO2<scalar>& other) const {
|
||||
res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi));
|
||||
}
|
||||
|
||||
friend std::istream& operator>>(std::istream &is, SO2<scalar>& ang){
|
||||
return is >> ang.angle();
|
||||
}
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Three-dimensional orientations represented as Quaternion.
|
||||
* It is assumed that the internal Quaternion always stays normalized,
|
||||
* should this not be the case, call inherited member function @c normalize().
|
||||
*/
|
||||
template<class _scalar = double, int Options = Eigen::AutoAlign>
|
||||
struct SO3 : public Eigen::Quaternion<_scalar, Options> {
|
||||
enum {DOF = 3, DIM = 3, TYP = 2};
|
||||
typedef _scalar scalar;
|
||||
typedef Eigen::Quaternion<scalar, Options> base;
|
||||
typedef Eigen::Quaternion<scalar> Quaternion;
|
||||
typedef vect<DIM, scalar, Options> vect_type;
|
||||
|
||||
//! Calculate @c this->inverse() * @c r
|
||||
template<class OtherDerived> EIGEN_STRONG_INLINE
|
||||
Quaternion operator%(const Eigen::QuaternionBase<OtherDerived> &r) const {
|
||||
return base::conjugate() * r;
|
||||
}
|
||||
|
||||
//! Calculate @c this->inverse() * @c r
|
||||
template<class Derived>
|
||||
vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
|
||||
return base::conjugate() * vec;
|
||||
}
|
||||
|
||||
//! Calculate @c this * @c r.conjugate()
|
||||
template<class OtherDerived> EIGEN_STRONG_INLINE
|
||||
Quaternion operator/(const Eigen::QuaternionBase<OtherDerived> &r) const {
|
||||
return *this * r.conjugate();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from real part and three imaginary parts.
|
||||
* Quaternion is normalized after construction.
|
||||
*/
|
||||
SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) {
|
||||
base::normalize();
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from Eigen::Quaternion.
|
||||
* @note Non-normalized input may result result in spurious behavior.
|
||||
*/
|
||||
SO3(const base& src = base::Identity()) : base(src) {}
|
||||
|
||||
/**
|
||||
* Construct from rotation matrix.
|
||||
* @note Invalid rotation matrices may lead to spurious behavior.
|
||||
*/
|
||||
template<class Derived>
|
||||
SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}
|
||||
|
||||
/**
|
||||
* Construct from arbitrary rotation type.
|
||||
* @note Invalid rotation matrices may lead to spurious behavior.
|
||||
*/
|
||||
template<class Derived>
|
||||
SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}
|
||||
|
||||
//! @name Manifold requirements
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
SO3 delta = exp(vec, scale);
|
||||
*this = *this * delta;
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const SO3<scalar>& other) const {
|
||||
res = SO3::log(other.conjugate() * *this);
|
||||
}
|
||||
//}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
SO3 delta = exp(vec, scale);
|
||||
*this = *this * delta;
|
||||
}
|
||||
|
||||
// void hat(MTK::vectview<const scalar, DOF>& v, Eigen::Matrix<scalar, 3, 3> &res) {
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
// Eigen::Matrix<scalar, 3, 3> res;
|
||||
res << 0, -v[2], v[1],
|
||||
v[2], 0, -v[0],
|
||||
-v[1], v[0], 0;
|
||||
// return res;
|
||||
}
|
||||
|
||||
// void Jacob_right_inv(MTK::vectview<const scalar, DOF> vec, Eigen::Matrix<scalar, 3, 3> & res){
|
||||
void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
|
||||
Eigen::MatrixXd hat_v;
|
||||
hat(vec, hat_v);
|
||||
if(vec.norm() > MTK::tolerance<scalar>())
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Identity() + 0.5 * hat_v + (1 - vec.norm() * std::cos(vec.norm() / 2) / 2 / std::sin(vec.norm() / 2)) * hat_v * hat_v / vec.squaredNorm();
|
||||
}
|
||||
else
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Identity();
|
||||
}
|
||||
// return res;
|
||||
}
|
||||
|
||||
// void Jacob_right(MTK::vectview<const scalar, DOF> & v, Eigen::Matrix<scalar, 3, 3> &res){
|
||||
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res){
|
||||
Eigen::MatrixXd hat_v;
|
||||
hat(v, hat_v);
|
||||
double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
|
||||
double norm = std::sqrt(squaredNorm);
|
||||
if(norm < MTK::tolerance<scalar>()){
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Identity();
|
||||
}
|
||||
else{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Identity() - (1 - std::cos(norm)) / squaredNorm * hat_v + (1 - std::sin(norm) / norm) / squaredNorm * hat_v * hat_v;
|
||||
}
|
||||
// return res;
|
||||
}
|
||||
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const SO3<scalar, Options>& q){
|
||||
return os << q.coeffs().transpose() << " ";
|
||||
}
|
||||
|
||||
friend std::istream& operator>>(std::istream &is, SO3<scalar, Options>& q){
|
||||
vect<4,scalar> coeffs;
|
||||
is >> coeffs;
|
||||
q.coeffs() = coeffs.normalized();
|
||||
return is;
|
||||
}
|
||||
|
||||
//! @name Helper functions
|
||||
//{
|
||||
/**
|
||||
* Calculate the exponential map. In matrix terms this would correspond
|
||||
* to the Rodrigues formula.
|
||||
*/
|
||||
// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround
|
||||
// static SO3 exp(MTK::vectview<const scalar, 3> dvec, scalar scale = 1){
|
||||
static SO3 exp(const Eigen::Matrix<scalar, 3, 1>& dvec, scalar scale = 1){
|
||||
SO3 res;
|
||||
res.w() = MTK::exp<scalar, 3>(res.vec(), dvec, scalar(scale/2));
|
||||
return res;
|
||||
}
|
||||
/**
|
||||
* Calculate the inverse of @c exp.
|
||||
* Only guarantees that <code>exp(log(x)) == x </code>
|
||||
*/
|
||||
static typename base::Vector3 log(const SO3 &orient){
|
||||
typename base::Vector3 res;
|
||||
MTK::log<scalar, 3>(res, orient.w(), orient.vec(), scalar(2), true);
|
||||
return res;
|
||||
}
|
||||
};
|
||||
|
||||
namespace internal {
|
||||
template<class Scalar, int Options>
|
||||
struct UnalignedType<SO2<Scalar, Options > >{
|
||||
typedef SO2<Scalar, Options | Eigen::DontAlign> type;
|
||||
};
|
||||
|
||||
template<class Scalar, int Options>
|
||||
struct UnalignedType<SO3<Scalar, Options > >{
|
||||
typedef SO3<Scalar, Options | Eigen::DontAlign> type;
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif /*SON_H_*/
|
||||
|
||||
@@ -0,0 +1,511 @@
|
||||
// This is an advanced implementation of the algorithm described in the
|
||||
// following paper:
|
||||
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
|
||||
|
||||
/*
|
||||
* Copyright (c) 2019--2023, The University of Hong Kong
|
||||
* All rights reserved.
|
||||
*
|
||||
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2008--2011, Universitaet Bremen
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
/**
|
||||
* @file mtk/types/vect.hpp
|
||||
* @brief Basic vectors interpreted as manifolds.
|
||||
*
|
||||
* This file also implements a simple wrapper for matrices, for arbitrary scalars
|
||||
* and for positive scalars.
|
||||
*/
|
||||
#ifndef VECT_H_
|
||||
#define VECT_H_
|
||||
|
||||
#include <iosfwd>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
#include "../src/vectview.hpp"
|
||||
|
||||
namespace MTK {
|
||||
|
||||
static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]");
|
||||
|
||||
|
||||
/**
|
||||
* A simple vector class.
|
||||
* Implementation is basically a wrapper around Eigen::Matrix with manifold
|
||||
* requirements added.
|
||||
*/
|
||||
template<int D = 3, class _scalar = double, int _Options=Eigen::AutoAlign>
|
||||
struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> {
|
||||
typedef Eigen::Matrix<_scalar, D, 1, _Options> base;
|
||||
enum {DOF = D, DIM = D, TYP = 0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
//using base::operator=;
|
||||
|
||||
/** Standard constructor. Sets all values to zero. */
|
||||
vect(const base &src = base::Zero()) : base(src) {}
|
||||
|
||||
/** Constructor copying the value of the expression \a other */
|
||||
template<typename OtherDerived>
|
||||
EIGEN_STRONG_INLINE vect(const Eigen::DenseBase<OtherDerived>& other) : base(other) {}
|
||||
|
||||
/** Construct from memory. */
|
||||
vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { }
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
|
||||
*this += scale * vec;
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, D> res, const vect<D, scalar>& other) const {
|
||||
res = *this - other;
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
|
||||
*this += scale * vec;
|
||||
}
|
||||
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const vect<D, scalar, _Options>& v){
|
||||
// Eigen sometimes messes with the streams flags, so output manually:
|
||||
for(int i=0; i<DOF; ++i)
|
||||
os << v(i) << " ";
|
||||
return os;
|
||||
}
|
||||
friend std::istream& operator>>(std::istream &is, vect<D, scalar, _Options>& v){
|
||||
char term=0;
|
||||
is >> std::ws; // skip whitespace
|
||||
switch(is.peek()) {
|
||||
case '(': term=')'; is.ignore(1); break;
|
||||
case '[': term=']'; is.ignore(1); break;
|
||||
case '{': term='}'; is.ignore(1); break;
|
||||
default: break;
|
||||
}
|
||||
if(D==Eigen::Dynamic) {
|
||||
assert(term !=0 && "Dynamic vectors must be embraced");
|
||||
std::vector<scalar> temp;
|
||||
while(is.good() && is.peek() != term) {
|
||||
scalar x;
|
||||
is >> x;
|
||||
temp.push_back(x);
|
||||
if(is.peek()==',') is.ignore(1);
|
||||
}
|
||||
v = vect::Map(temp.data(), temp.size());
|
||||
} else
|
||||
for(int i=0; i<v.size(); ++i){
|
||||
is >> v[i];
|
||||
if(is.peek()==',') { // ignore commas between values
|
||||
is.ignore(1);
|
||||
}
|
||||
}
|
||||
if(term!=0) {
|
||||
char x;
|
||||
is >> x;
|
||||
if(x!=term) {
|
||||
is.setstate(is.badbit);
|
||||
// assert(x==term && "start and end bracket do not match!");
|
||||
}
|
||||
}
|
||||
return is;
|
||||
}
|
||||
|
||||
template<int dim>
|
||||
vectview<scalar, dim> tail(){
|
||||
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
|
||||
return base::template tail<dim>();
|
||||
}
|
||||
template<int dim>
|
||||
vectview<const scalar, dim> tail() const{
|
||||
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
|
||||
return base::template tail<dim>();
|
||||
}
|
||||
template<int dim>
|
||||
vectview<scalar, dim> head(){
|
||||
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
|
||||
return base::template head<dim>();
|
||||
}
|
||||
template<int dim>
|
||||
vectview<const scalar, dim> head() const{
|
||||
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
|
||||
return base::template head<dim>();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* A simple matrix class.
|
||||
* Implementation is basically a wrapper around Eigen::Matrix with manifold
|
||||
* requirements added, i.e., matrix is viewed as a plain vector for that.
|
||||
*/
|
||||
template<int M, int N, class _scalar = double, int _Options = Eigen::Matrix<_scalar, M, N>::Options>
|
||||
struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> {
|
||||
typedef Eigen::Matrix<_scalar, M, N, _Options> base;
|
||||
enum {DOF = M * N, TYP = 4, DIM=0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
using base::operator=;
|
||||
|
||||
/** Standard constructor. Sets all values to zero. */
|
||||
matrix() {
|
||||
base::setZero();
|
||||
}
|
||||
|
||||
/** Constructor copying the value of the expression \a other */
|
||||
template<typename OtherDerived>
|
||||
EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase<OtherDerived>& other) : base(other) {}
|
||||
|
||||
/** Construct from memory. */
|
||||
matrix(const scalar* src) : base(src) { }
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
*this += scale * base::Map(vec.data());
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const matrix& other) const {
|
||||
base::Map(res.data()) = *this - other;
|
||||
}
|
||||
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
*this += scale * base::Map(vec.data());
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, const matrix<M, N, scalar, _Options>& mat){
|
||||
for(int i=0; i<DOF; ++i){
|
||||
os << mat.data()[i] << " ";
|
||||
}
|
||||
return os;
|
||||
}
|
||||
friend std::istream& operator>>(std::istream &is, matrix<M, N, scalar, _Options>& mat){
|
||||
for(int i=0; i<DOF; ++i){
|
||||
is >> mat.data()[i];
|
||||
}
|
||||
return is;
|
||||
}
|
||||
};// @todo What if M / N = Eigen::Dynamic?
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* A simple scalar type.
|
||||
*/
|
||||
template<class _scalar = double>
|
||||
struct Scalar {
|
||||
enum {DOF = 1, TYP = 5, DIM=0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
scalar value;
|
||||
|
||||
Scalar(const scalar& value = scalar(0)) : value(value) {}
|
||||
operator const scalar&() const { return value; }
|
||||
operator scalar&() { return value; }
|
||||
Scalar& operator=(const scalar& val) { value = val; return *this; }
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
value += scale * vec[0];
|
||||
}
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
|
||||
value += scale * vec[0];
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const Scalar& other) const {
|
||||
res[0] = *this - other;
|
||||
}
|
||||
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Positive scalars.
|
||||
* Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$.
|
||||
*/
|
||||
template<class _scalar = double>
|
||||
struct PositiveScalar {
|
||||
enum {DOF = 1, TYP = 6, DIM=0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
scalar value;
|
||||
|
||||
PositiveScalar(const scalar& value = scalar(1)) : value(value) {
|
||||
assert(value > scalar(0));
|
||||
}
|
||||
operator const scalar&() const { return value; }
|
||||
PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; }
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
value *= std::exp(scale * vec[0]);
|
||||
}
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const PositiveScalar& other) const {
|
||||
res[0] = std::log(*this / other);
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
value *= std::exp(scale * vec[0]);
|
||||
}
|
||||
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
|
||||
friend std::istream& operator>>(std::istream &is, PositiveScalar<scalar>& s){
|
||||
is >> s.value;
|
||||
assert(s.value > 0);
|
||||
return is;
|
||||
}
|
||||
};
|
||||
|
||||
template<class _scalar = double>
|
||||
struct Complex : public std::complex<_scalar>{
|
||||
enum {DOF = 2, TYP = 7, DIM=0};
|
||||
typedef _scalar scalar;
|
||||
|
||||
typedef std::complex<scalar> Base;
|
||||
|
||||
Complex(const Base& value) : Base(value) {}
|
||||
Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {}
|
||||
Complex(const MTK::vectview<const scalar, 2> &in) : Base(in[0], in[1]) {}
|
||||
template<class Derived>
|
||||
Complex(const Eigen::DenseBase<Derived> &in) : Base(in[0], in[1]) {}
|
||||
|
||||
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
Base::real() += scale * vec[0];
|
||||
Base::imag() += scale * vec[1];
|
||||
};
|
||||
void boxminus(MTK::vectview<scalar, DOF> res, const Complex& other) const {
|
||||
Complex diff = *this - other;
|
||||
res << diff.real(), diff.imag();
|
||||
}
|
||||
|
||||
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
|
||||
{
|
||||
res = Eigen::Matrix<scalar, 3, 3>::Zero();
|
||||
}
|
||||
|
||||
void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
|
||||
std::cout << "wrong idx" << std::endl;
|
||||
}
|
||||
|
||||
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
|
||||
Base::real() += scale * vec[0];
|
||||
Base::imag() += scale * vec[1];
|
||||
};
|
||||
|
||||
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 2, 3>::Zero();
|
||||
}
|
||||
|
||||
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
|
||||
{
|
||||
std::cerr << "wrong idx for S2" << std::endl;
|
||||
std::exit(100);
|
||||
res = Eigen::Matrix<scalar, 3, 2>::Zero();
|
||||
}
|
||||
|
||||
scalar squaredNorm() const {
|
||||
return std::pow(Base::real(),2) + std::pow(Base::imag(),2);
|
||||
}
|
||||
|
||||
const scalar& operator()(int i) const {
|
||||
assert(0<=i && i<2 && "Index out of range");
|
||||
return i==0 ? Base::real() : Base::imag();
|
||||
}
|
||||
scalar& operator()(int i){
|
||||
assert(0<=i && i<2 && "Index out of range");
|
||||
return i==0 ? Base::real() : Base::imag();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<int dim, class Scalar, int Options>
|
||||
struct UnalignedType<vect<dim, Scalar, Options > >{
|
||||
typedef vect<dim, Scalar, Options | Eigen::DontAlign> type;
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /*VECT_H_*/
|
||||
@@ -0,0 +1,113 @@
|
||||
/*
|
||||
* Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH
|
||||
* All rights reserved.
|
||||
*
|
||||
* Author: Rene Wagner <rene.wagner@dfki.de>
|
||||
* Christoph Hertzberg <chtz@informatik.uni-bremen.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Universitaet Bremen nor the DFKI GmbH
|
||||
* nor the names of its contributors may be used to endorse or
|
||||
* promote products derived from this software without specific
|
||||
* prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef WRAPPED_CV_MAT_HPP_
|
||||
#define WRAPPED_CV_MAT_HPP_
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <opencv/cv.h>
|
||||
|
||||
namespace MTK {
|
||||
|
||||
template<class f_type>
|
||||
struct cv_f_type;
|
||||
|
||||
template<>
|
||||
struct cv_f_type<double>
|
||||
{
|
||||
enum {value = CV_64F};
|
||||
};
|
||||
|
||||
template<>
|
||||
struct cv_f_type<float>
|
||||
{
|
||||
enum {value = CV_32F};
|
||||
};
|
||||
|
||||
/**
|
||||
* cv_mat wraps a CvMat around an Eigen Matrix
|
||||
*/
|
||||
template<int rows, int cols, class f_type = double>
|
||||
class cv_mat : public matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor>
|
||||
{
|
||||
typedef matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor> base_type;
|
||||
enum {type_ = cv_f_type<f_type>::value};
|
||||
CvMat cv_mat_;
|
||||
|
||||
public:
|
||||
cv_mat()
|
||||
{
|
||||
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
|
||||
}
|
||||
|
||||
cv_mat(const cv_mat& oth) : base_type(oth)
|
||||
{
|
||||
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
|
||||
}
|
||||
|
||||
template<class Derived>
|
||||
cv_mat(const Eigen::MatrixBase<Derived> &value) : base_type(value)
|
||||
{
|
||||
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
|
||||
}
|
||||
|
||||
template<class Derived>
|
||||
cv_mat& operator=(const Eigen::MatrixBase<Derived> &value)
|
||||
{
|
||||
base_type::operator=(value);
|
||||
return *this;
|
||||
}
|
||||
|
||||
cv_mat& operator=(const cv_mat& value)
|
||||
{
|
||||
base_type::operator=(value);
|
||||
return *this;
|
||||
}
|
||||
|
||||
// FIXME: Maybe overloading operator& is not a good idea ...
|
||||
CvMat* operator&()
|
||||
{
|
||||
return &cv_mat_;
|
||||
}
|
||||
const CvMat* operator&() const
|
||||
{
|
||||
return &cv_mat_;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace MTK
|
||||
|
||||
#endif /* WRAPPED_CV_MAT_HPP_ */
|
||||
@@ -0,0 +1,339 @@
|
||||
GNU GENERAL PUBLIC LICENSE
|
||||
Version 2, June 1991
|
||||
|
||||
Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
Everyone is permitted to copy and distribute verbatim copies
|
||||
of this license document, but changing it is not allowed.
|
||||
|
||||
Preamble
|
||||
|
||||
The licenses for most software are designed to take away your
|
||||
freedom to share and change it. By contrast, the GNU General Public
|
||||
License is intended to guarantee your freedom to share and change free
|
||||
software--to make sure the software is free for all its users. This
|
||||
General Public License applies to most of the Free Software
|
||||
Foundation's software and to any other program whose authors commit to
|
||||
using it. (Some other Free Software Foundation software is covered by
|
||||
the GNU Lesser General Public License instead.) You can apply it to
|
||||
your programs, too.
|
||||
|
||||
When we speak of free software, we are referring to freedom, not
|
||||
price. Our General Public Licenses are designed to make sure that you
|
||||
have the freedom to distribute copies of free software (and charge for
|
||||
this service if you wish), that you receive source code or can get it
|
||||
if you want it, that you can change the software or use pieces of it
|
||||
in new free programs; and that you know you can do these things.
|
||||
|
||||
To protect your rights, we need to make restrictions that forbid
|
||||
anyone to deny you these rights or to ask you to surrender the rights.
|
||||
These restrictions translate to certain responsibilities for you if you
|
||||
distribute copies of the software, or if you modify it.
|
||||
|
||||
For example, if you distribute copies of such a program, whether
|
||||
gratis or for a fee, you must give the recipients all the rights that
|
||||
you have. You must make sure that they, too, receive or can get the
|
||||
source code. And you must show them these terms so they know their
|
||||
rights.
|
||||
|
||||
We protect your rights with two steps: (1) copyright the software, and
|
||||
(2) offer you this license which gives you legal permission to copy,
|
||||
distribute and/or modify the software.
|
||||
|
||||
Also, for each author's protection and ours, we want to make certain
|
||||
that everyone understands that there is no warranty for this free
|
||||
software. If the software is modified by someone else and passed on, we
|
||||
want its recipients to know that what they have is not the original, so
|
||||
that any problems introduced by others will not reflect on the original
|
||||
authors' reputations.
|
||||
|
||||
Finally, any free program is threatened constantly by software
|
||||
patents. We wish to avoid the danger that redistributors of a free
|
||||
program will individually obtain patent licenses, in effect making the
|
||||
program proprietary. To prevent this, we have made it clear that any
|
||||
patent must be licensed for everyone's free use or not licensed at all.
|
||||
|
||||
The precise terms and conditions for copying, distribution and
|
||||
modification follow.
|
||||
|
||||
GNU GENERAL PUBLIC LICENSE
|
||||
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
|
||||
|
||||
0. This License applies to any program or other work which contains
|
||||
a notice placed by the copyright holder saying it may be distributed
|
||||
under the terms of this General Public License. The "Program", below,
|
||||
refers to any such program or work, and a "work based on the Program"
|
||||
means either the Program or any derivative work under copyright law:
|
||||
that is to say, a work containing the Program or a portion of it,
|
||||
either verbatim or with modifications and/or translated into another
|
||||
language. (Hereinafter, translation is included without limitation in
|
||||
the term "modification".) Each licensee is addressed as "you".
|
||||
|
||||
Activities other than copying, distribution and modification are not
|
||||
covered by this License; they are outside its scope. The act of
|
||||
running the Program is not restricted, and the output from the Program
|
||||
is covered only if its contents constitute a work based on the
|
||||
Program (independent of having been made by running the Program).
|
||||
Whether that is true depends on what the Program does.
|
||||
|
||||
1. You may copy and distribute verbatim copies of the Program's
|
||||
source code as you receive it, in any medium, provided that you
|
||||
conspicuously and appropriately publish on each copy an appropriate
|
||||
copyright notice and disclaimer of warranty; keep intact all the
|
||||
notices that refer to this License and to the absence of any warranty;
|
||||
and give any other recipients of the Program a copy of this License
|
||||
along with the Program.
|
||||
|
||||
You may charge a fee for the physical act of transferring a copy, and
|
||||
you may at your option offer warranty protection in exchange for a fee.
|
||||
|
||||
2. You may modify your copy or copies of the Program or any portion
|
||||
of it, thus forming a work based on the Program, and copy and
|
||||
distribute such modifications or work under the terms of Section 1
|
||||
above, provided that you also meet all of these conditions:
|
||||
|
||||
a) You must cause the modified files to carry prominent notices
|
||||
stating that you changed the files and the date of any change.
|
||||
|
||||
b) You must cause any work that you distribute or publish, that in
|
||||
whole or in part contains or is derived from the Program or any
|
||||
part thereof, to be licensed as a whole at no charge to all third
|
||||
parties under the terms of this License.
|
||||
|
||||
c) If the modified program normally reads commands interactively
|
||||
when run, you must cause it, when started running for such
|
||||
interactive use in the most ordinary way, to print or display an
|
||||
announcement including an appropriate copyright notice and a
|
||||
notice that there is no warranty (or else, saying that you provide
|
||||
a warranty) and that users may redistribute the program under
|
||||
these conditions, and telling the user how to view a copy of this
|
||||
License. (Exception: if the Program itself is interactive but
|
||||
does not normally print such an announcement, your work based on
|
||||
the Program is not required to print an announcement.)
|
||||
|
||||
These requirements apply to the modified work as a whole. If
|
||||
identifiable sections of that work are not derived from the Program,
|
||||
and can be reasonably considered independent and separate works in
|
||||
themselves, then this License, and its terms, do not apply to those
|
||||
sections when you distribute them as separate works. But when you
|
||||
distribute the same sections as part of a whole which is a work based
|
||||
on the Program, the distribution of the whole must be on the terms of
|
||||
this License, whose permissions for other licensees extend to the
|
||||
entire whole, and thus to each and every part regardless of who wrote it.
|
||||
|
||||
Thus, it is not the intent of this section to claim rights or contest
|
||||
your rights to work written entirely by you; rather, the intent is to
|
||||
exercise the right to control the distribution of derivative or
|
||||
collective works based on the Program.
|
||||
|
||||
In addition, mere aggregation of another work not based on the Program
|
||||
with the Program (or with a work based on the Program) on a volume of
|
||||
a storage or distribution medium does not bring the other work under
|
||||
the scope of this License.
|
||||
|
||||
3. You may copy and distribute the Program (or a work based on it,
|
||||
under Section 2) in object code or executable form under the terms of
|
||||
Sections 1 and 2 above provided that you also do one of the following:
|
||||
|
||||
a) Accompany it with the complete corresponding machine-readable
|
||||
source code, which must be distributed under the terms of Sections
|
||||
1 and 2 above on a medium customarily used for software interchange; or,
|
||||
|
||||
b) Accompany it with a written offer, valid for at least three
|
||||
years, to give any third party, for a charge no more than your
|
||||
cost of physically performing source distribution, a complete
|
||||
machine-readable copy of the corresponding source code, to be
|
||||
distributed under the terms of Sections 1 and 2 above on a medium
|
||||
customarily used for software interchange; or,
|
||||
|
||||
c) Accompany it with the information you received as to the offer
|
||||
to distribute corresponding source code. (This alternative is
|
||||
allowed only for noncommercial distribution and only if you
|
||||
received the program in object code or executable form with such
|
||||
an offer, in accord with Subsection b above.)
|
||||
|
||||
The source code for a work means the preferred form of the work for
|
||||
making modifications to it. For an executable work, complete source
|
||||
code means all the source code for all modules it contains, plus any
|
||||
associated interface definition files, plus the scripts used to
|
||||
control compilation and installation of the executable. However, as a
|
||||
special exception, the source code distributed need not include
|
||||
anything that is normally distributed (in either source or binary
|
||||
form) with the major components (compiler, kernel, and so on) of the
|
||||
operating system on which the executable runs, unless that component
|
||||
itself accompanies the executable.
|
||||
|
||||
If distribution of executable or object code is made by offering
|
||||
access to copy from a designated place, then offering equivalent
|
||||
access to copy the source code from the same place counts as
|
||||
distribution of the source code, even though third parties are not
|
||||
compelled to copy the source along with the object code.
|
||||
|
||||
4. You may not copy, modify, sublicense, or distribute the Program
|
||||
except as expressly provided under this License. Any attempt
|
||||
otherwise to copy, modify, sublicense or distribute the Program is
|
||||
void, and will automatically terminate your rights under this License.
|
||||
However, parties who have received copies, or rights, from you under
|
||||
this License will not have their licenses terminated so long as such
|
||||
parties remain in full compliance.
|
||||
|
||||
5. You are not required to accept this License, since you have not
|
||||
signed it. However, nothing else grants you permission to modify or
|
||||
distribute the Program or its derivative works. These actions are
|
||||
prohibited by law if you do not accept this License. Therefore, by
|
||||
modifying or distributing the Program (or any work based on the
|
||||
Program), you indicate your acceptance of this License to do so, and
|
||||
all its terms and conditions for copying, distributing or modifying
|
||||
the Program or works based on it.
|
||||
|
||||
6. Each time you redistribute the Program (or any work based on the
|
||||
Program), the recipient automatically receives a license from the
|
||||
original licensor to copy, distribute or modify the Program subject to
|
||||
these terms and conditions. You may not impose any further
|
||||
restrictions on the recipients' exercise of the rights granted herein.
|
||||
You are not responsible for enforcing compliance by third parties to
|
||||
this License.
|
||||
|
||||
7. If, as a consequence of a court judgment or allegation of patent
|
||||
infringement or for any other reason (not limited to patent issues),
|
||||
conditions are imposed on you (whether by court order, agreement or
|
||||
otherwise) that contradict the conditions of this License, they do not
|
||||
excuse you from the conditions of this License. If you cannot
|
||||
distribute so as to satisfy simultaneously your obligations under this
|
||||
License and any other pertinent obligations, then as a consequence you
|
||||
may not distribute the Program at all. For example, if a patent
|
||||
license would not permit royalty-free redistribution of the Program by
|
||||
all those who receive copies directly or indirectly through you, then
|
||||
the only way you could satisfy both it and this License would be to
|
||||
refrain entirely from distribution of the Program.
|
||||
|
||||
If any portion of this section is held invalid or unenforceable under
|
||||
any particular circumstance, the balance of the section is intended to
|
||||
apply and the section as a whole is intended to apply in other
|
||||
circumstances.
|
||||
|
||||
It is not the purpose of this section to induce you to infringe any
|
||||
patents or other property right claims or to contest validity of any
|
||||
such claims; this section has the sole purpose of protecting the
|
||||
integrity of the free software distribution system, which is
|
||||
implemented by public license practices. Many people have made
|
||||
generous contributions to the wide range of software distributed
|
||||
through that system in reliance on consistent application of that
|
||||
system; it is up to the author/donor to decide if he or she is willing
|
||||
to distribute software through any other system and a licensee cannot
|
||||
impose that choice.
|
||||
|
||||
This section is intended to make thoroughly clear what is believed to
|
||||
be a consequence of the rest of this License.
|
||||
|
||||
8. If the distribution and/or use of the Program is restricted in
|
||||
certain countries either by patents or by copyrighted interfaces, the
|
||||
original copyright holder who places the Program under this License
|
||||
may add an explicit geographical distribution limitation excluding
|
||||
those countries, so that distribution is permitted only in or among
|
||||
countries not thus excluded. In such case, this License incorporates
|
||||
the limitation as if written in the body of this License.
|
||||
|
||||
9. The Free Software Foundation may publish revised and/or new versions
|
||||
of the General Public License from time to time. Such new versions will
|
||||
be similar in spirit to the present version, but may differ in detail to
|
||||
address new problems or concerns.
|
||||
|
||||
Each version is given a distinguishing version number. If the Program
|
||||
specifies a version number of this License which applies to it and "any
|
||||
later version", you have the option of following the terms and conditions
|
||||
either of that version or of any later version published by the Free
|
||||
Software Foundation. If the Program does not specify a version number of
|
||||
this License, you may choose any version ever published by the Free Software
|
||||
Foundation.
|
||||
|
||||
10. If you wish to incorporate parts of the Program into other free
|
||||
programs whose distribution conditions are different, write to the author
|
||||
to ask for permission. For software which is copyrighted by the Free
|
||||
Software Foundation, write to the Free Software Foundation; we sometimes
|
||||
make exceptions for this. Our decision will be guided by the two goals
|
||||
of preserving the free status of all derivatives of our free software and
|
||||
of promoting the sharing and reuse of software generally.
|
||||
|
||||
NO WARRANTY
|
||||
|
||||
11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
|
||||
FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
|
||||
OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
|
||||
PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
|
||||
OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
||||
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
|
||||
TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
|
||||
PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
|
||||
REPAIR OR CORRECTION.
|
||||
|
||||
12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
|
||||
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
|
||||
REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
|
||||
INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
|
||||
OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
|
||||
TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
|
||||
YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
|
||||
PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGES.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
How to Apply These Terms to Your New Programs
|
||||
|
||||
If you develop a new program, and you want it to be of the greatest
|
||||
possible use to the public, the best way to achieve this is to make it
|
||||
free software which everyone can redistribute and change under these terms.
|
||||
|
||||
To do so, attach the following notices to the program. It is safest
|
||||
to attach them to the start of each source file to most effectively
|
||||
convey the exclusion of warranty; and each file should have at least
|
||||
the "copyright" line and a pointer to where the full notice is found.
|
||||
|
||||
<one line to give the program's name and a brief idea of what it does.>
|
||||
Copyright (C) <year> <name of author>
|
||||
|
||||
This program is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation; either version 2 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License along
|
||||
with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
|
||||
Also add information on how to contact you by electronic and paper mail.
|
||||
|
||||
If the program is interactive, make it output a short notice like this
|
||||
when it starts in an interactive mode:
|
||||
|
||||
Gnomovision version 69, Copyright (C) year name of author
|
||||
Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
|
||||
This is free software, and you are welcome to redistribute it
|
||||
under certain conditions; type `show c' for details.
|
||||
|
||||
The hypothetical commands `show w' and `show c' should show the appropriate
|
||||
parts of the General Public License. Of course, the commands you use may
|
||||
be called something other than `show w' and `show c'; they could even be
|
||||
mouse-clicks or menu items--whatever suits your program.
|
||||
|
||||
You should also get your employer (if you work as a programmer) or your
|
||||
school, if any, to sign a "copyright disclaimer" for the program, if
|
||||
necessary. Here is a sample; alter the names:
|
||||
|
||||
Yoyodyne, Inc., hereby disclaims all copyright interest in the program
|
||||
`Gnomovision' (which makes passes at compilers) written by James Hacker.
|
||||
|
||||
<signature of Ty Coon>, 1 April 1989
|
||||
Ty Coon, President of Vice
|
||||
|
||||
This General Public License does not permit incorporating your program into
|
||||
proprietary programs. If your program is a subroutine library, you may
|
||||
consider it more useful to permit linking proprietary applications with the
|
||||
library. If this is what you want to do, use the GNU Lesser General
|
||||
Public License instead of this License.
|
||||
@@ -0,0 +1,489 @@
|
||||
## IKFoM
|
||||
**IKFoM** (Iterated Kalman Filters on Manifolds) is a computationally efficient and convenient toolkit for deploying iterated Kalman filters on various robotic systems, especially systems operating on high-dimension manifold. It implements a manifold-embedding Kalman filter which separates the manifold structures from system descriptions and is able to be used by only defining the system in a canonical form and calling the respective steps accordingly. The current implementation supports the full iterated Kalman filtering for systems on manifold <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbb{R}^m\times&space;SO(3)\times\cdots\times&space;SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbb{R}^m\times&space;SO(3)\times\cdots\times&space;SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" title="\mathbb{R}^m\times SO(3)\times\cdots\times SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" /></a> and any of its sub-manifolds, and it is extendable to other types of manifold when necessary.
|
||||
|
||||
|
||||
**Developers**
|
||||
|
||||
[Dongjiao He](https://github.com/Joanna-HE)
|
||||
|
||||
**Our related video**: https://youtu.be/sz_ZlDkl6fA
|
||||
|
||||
## 1. Prerequisites
|
||||
|
||||
### 1.1. **Eigen && Boost**
|
||||
Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).
|
||||
|
||||
Boost >= 1.65.
|
||||
|
||||
## 2. Usage when the measurement is of constant dimension and type.
|
||||
Clone the repository:
|
||||
|
||||
```
|
||||
git clone https://github.com/hku-mars/IKFoM.git
|
||||
```
|
||||
|
||||
1. include the necessary head file:
|
||||
```
|
||||
#include<esekfom/esekfom.hpp>
|
||||
```
|
||||
2. Select and instantiate the primitive manifolds:
|
||||
```
|
||||
typedef MTK::SO3<double> SO3; // scalar type of variable: double
|
||||
typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
|
||||
typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
|
||||
```
|
||||
3. Build system state, input and measurement as compound manifolds which are composed of the primitive manifolds:
|
||||
```
|
||||
MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
|
||||
((vect3, pos)) // ((primitive manifold type, name of variable))
|
||||
((vect3, vel))
|
||||
((SO3, rot))
|
||||
((vect3, bg))
|
||||
((vect3, ba))
|
||||
((S2, grav))
|
||||
((SO3, offset_R_L_I))
|
||||
((vect3, offset_T_L_I))
|
||||
);
|
||||
```
|
||||
4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k+1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k+1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"><see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
|
||||
```
|
||||
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
|
||||
Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
|
||||
res(0) = s.vel[0];
|
||||
res(1) = s.vel[1];
|
||||
res(2) = s.vel[2];
|
||||
return res;
|
||||
}
|
||||
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
|
||||
Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
|
||||
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
|
||||
return cov;
|
||||
}
|
||||
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
|
||||
Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
|
||||
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
|
||||
return cov;
|
||||
}
|
||||
```
|
||||
Those functions would be called during the ekf state predict
|
||||
|
||||
5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
|
||||
```
|
||||
measurement h(state &s, bool &valid) // the iteration stops before convergence whenever the user set valid as false
|
||||
{
|
||||
if (condition){ valid = false;
|
||||
} // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied.
|
||||
measurement h_;
|
||||
h_.position = s.pos;
|
||||
return h_;
|
||||
}
|
||||
Eigen::Matrix<double, measurement_dof, state_dof> dh_dx(state &s) {}
|
||||
Eigen::Matrix<double, measurement_dof, measurement_noise_dof> dh_dv(state &s) {}
|
||||
```
|
||||
Those functions would be called during the ekf state update
|
||||
|
||||
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
|
||||
|
||||
(1) initial state and covariance:
|
||||
```
|
||||
state init_state;
|
||||
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov init_P;
|
||||
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf(init_state,init_P);
|
||||
```
|
||||
(2) default state and covariance:
|
||||
```
|
||||
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf;
|
||||
```
|
||||
where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**.
|
||||
|
||||
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
|
||||
```
|
||||
double epsi[state_dof] = {0.001};
|
||||
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
|
||||
kf.init(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi);
|
||||
```
|
||||
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
|
||||
```
|
||||
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
|
||||
```
|
||||
9. Once a measurement **z** is received, an iterated update is executed:
|
||||
```
|
||||
kf.update_iterated(z, R); // measurement noise covariance: R, an Eigen matrix
|
||||
```
|
||||
*Remarks(1):*
|
||||
- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
|
||||
|
||||
5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
|
||||
```
|
||||
measurement h_share(state &s, esekfom::share_datastruct<state, measurement, measurement_noise_dof> &share_data)
|
||||
{
|
||||
if(share_data.converge) {} // this value is true means iteration is converged
|
||||
if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified
|
||||
share_data.h_x = H_x; // H_x is the result matrix of the first differentiation
|
||||
share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
|
||||
share_data.R = R; // R is the measurement noise covariance
|
||||
share_data.z = z; // z is the obtained measurement
|
||||
|
||||
measurement h_;
|
||||
h_.position = s.pos;
|
||||
return h_;
|
||||
}
|
||||
```
|
||||
This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
|
||||
|
||||
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
|
||||
|
||||
(1) initial state and covariance:
|
||||
```
|
||||
state init_state;
|
||||
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov init_P;
|
||||
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf(init_state,init_P);
|
||||
```
|
||||
(2) default state and covariance:
|
||||
```
|
||||
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf;
|
||||
```
|
||||
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
|
||||
```
|
||||
double epsi[state_dof] = {0.001};
|
||||
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
|
||||
kf.init_share(f, df_dx, df_dw, h_share, Maximum_iter, epsi);
|
||||
```
|
||||
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
|
||||
```
|
||||
kf.predict(dt, Q, in); // process noise covariance: Q
|
||||
```
|
||||
9. Once a measurement **z** is received, an iterated update is executed:
|
||||
```
|
||||
kf.update_iterated_share();
|
||||
```
|
||||
|
||||
*Remarks(2):*
|
||||
- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
|
||||
```
|
||||
state set_x;
|
||||
kf.change_x(set_x);
|
||||
esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov set_P;
|
||||
kf.change_P(set_P);
|
||||
```
|
||||
|
||||
## 3. Usage when the measurement is an Eigen vector of changing dimension.
|
||||
|
||||
Clone the repository:
|
||||
|
||||
```
|
||||
git clone https://github.com/hku-mars/IKFoM.git
|
||||
```
|
||||
|
||||
1. include the necessary head file:
|
||||
```
|
||||
#include<esekfom/esekfom.hpp>
|
||||
```
|
||||
2. Select and instantiate the primitive manifolds:
|
||||
```
|
||||
typedef MTK::SO3<double> SO3; // scalar type of variable: double
|
||||
typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
|
||||
typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
|
||||
```
|
||||
3. Build system state and input as compound manifolds which are composed of the primitive manifolds:
|
||||
```
|
||||
MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
|
||||
((vect3, pos)) // ((primitive manifold type, name of variable))
|
||||
((vect3, vel))
|
||||
((SO3, rot))
|
||||
((vect3, bg))
|
||||
((vect3, ba))
|
||||
((S2, grav))
|
||||
((SO3, offset_R_L_I))
|
||||
((vect3, offset_T_L_I))
|
||||
);
|
||||
```
|
||||
4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k+1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k+1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"> <see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
|
||||
```
|
||||
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
|
||||
Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
|
||||
res(0) = s.vel[0];
|
||||
res(1) = s.vel[1];
|
||||
res(2) = s.vel[2];
|
||||
return res;
|
||||
}
|
||||
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
|
||||
Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
|
||||
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
|
||||
return cov;
|
||||
}
|
||||
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
|
||||
Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
|
||||
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
|
||||
return cov;
|
||||
}
|
||||
```
|
||||
Those functions would be called during ekf state predict
|
||||
|
||||
5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
|
||||
```
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> h(state &s, bool &valid) //the iteration stops before convergence when valid is false {
|
||||
if (condition){ valid = false;
|
||||
} // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied.
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> h_;
|
||||
h_(0) = s.pos[0];
|
||||
return h_;
|
||||
}
|
||||
Eigen::Matrix<double, Eigen::Dynamic, state_dof> dh_dx(state &s) {}
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dh_dv(state &s) {}
|
||||
```
|
||||
Those functions would be called during ekf state update
|
||||
|
||||
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
|
||||
|
||||
(1) initial state and covariance:
|
||||
```
|
||||
state init_state;
|
||||
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
|
||||
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
|
||||
```
|
||||
(2) default state and covariance:
|
||||
```
|
||||
esekfom::esekf<state, process_noise_dof, input> kf;
|
||||
```
|
||||
where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**
|
||||
|
||||
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
|
||||
```
|
||||
double epsi[state_dof] = {0.001};
|
||||
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
|
||||
kf.init_dyn(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi);
|
||||
```
|
||||
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
|
||||
```
|
||||
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
|
||||
```
|
||||
9. Once a measurement **z** is received, an iterated update is executed:
|
||||
```
|
||||
kf.update_iterated_dyn(z, R); // measurement noise covariance: R, an Eigen matrix
|
||||
```
|
||||
*Remarks(1):*
|
||||
- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
|
||||
5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
|
||||
```
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> h_dyn_share(state &s, esekfom::dyn_share_datastruct<double> &dyn_share_data)
|
||||
{
|
||||
if(dyn_share_data.converge) {} // this value is true means iteration is converged
|
||||
if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified
|
||||
dyn_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation
|
||||
dyn_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
|
||||
dyn_share_data.R = R; // R is the measurement noise covariance
|
||||
dyn_share_data.z = z; // z is the obtained measurement
|
||||
|
||||
Eigen::Matrix<double, Eigen::Dynamic, 1> h_;
|
||||
h_(0) = s.pos[0];
|
||||
return h_;
|
||||
}
|
||||
This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
|
||||
```
|
||||
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
|
||||
(1) initial state and covariance:
|
||||
```
|
||||
state init_state;
|
||||
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
|
||||
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
|
||||
```
|
||||
(2) default state and covariance:
|
||||
```
|
||||
esekfom::esekf<state, process_noise_dof, input> kf;
|
||||
```
|
||||
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
|
||||
```
|
||||
double epsi[state_dof] = {0.001};
|
||||
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
|
||||
kf.init_dyn_share(f, df_dx, df_dw, h_dyn_share, Maximum_iter, epsi);
|
||||
```
|
||||
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
|
||||
```
|
||||
kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
|
||||
```
|
||||
9. Once a measurement **z** is received, an iterated update is executed:
|
||||
```
|
||||
kf.update_iterated_dyn_share();
|
||||
```
|
||||
|
||||
*Remarks(2):*
|
||||
- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
|
||||
```
|
||||
state set_x;
|
||||
kf.change_x(set_x);
|
||||
esekfom::esekf<state, process_noise_dof, input>::cov set_P;
|
||||
kf.change_P(set_P);
|
||||
```
|
||||
|
||||
## 4. Usage when the measurement is a changing manifold during the run time.
|
||||
|
||||
Clone the repository:
|
||||
|
||||
```
|
||||
git clone https://github.com/hku-mars/IKFoM.git
|
||||
```
|
||||
|
||||
1. include the necessary head file:
|
||||
```
|
||||
#include<esekfom/esekfom.hpp>
|
||||
```
|
||||
2. Select and instantiate the primitive manifolds:
|
||||
```
|
||||
typedef MTK::SO3<double> SO3; // scalar type of variable: double
|
||||
typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
|
||||
typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
|
||||
```
|
||||
3. Build system state and input as compound manifolds which are composed of the primitive manifolds:
|
||||
```
|
||||
MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
|
||||
((vect3, pos)) // ((primitive manifold type, name of variable))
|
||||
((vect3, vel))
|
||||
((SO3, rot))
|
||||
((vect3, bg))
|
||||
((vect3, ba))
|
||||
((S2, grav))
|
||||
((SO3, offset_R_L_I))
|
||||
((vect3, offset_T_L_I))
|
||||
);
|
||||
```
|
||||
4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k+1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k+1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"> <see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
|
||||
```
|
||||
Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
|
||||
Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
|
||||
res(0) = s.vel[0];
|
||||
res(1) = s.vel[1];
|
||||
res(2) = s.vel[2];
|
||||
return res;
|
||||
}
|
||||
Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
|
||||
Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
|
||||
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
|
||||
return cov;
|
||||
}
|
||||
Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
|
||||
Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
|
||||
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
|
||||
return cov;
|
||||
}
|
||||
```
|
||||
Those functions would be called during ekf state predict
|
||||
|
||||
5. Implement the differentiation of the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
|
||||
```
|
||||
Eigen::Matrix<double, Eigen::Dynamic, state_dof> dh_dx(state &s, bool &valid) {} //the iteration stops before convergence when valid is false
|
||||
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dh_dv(state &s, bool &valid) {}
|
||||
```
|
||||
Those functions would be called during ekf state update
|
||||
|
||||
6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
|
||||
|
||||
(1) initial state and covariance:
|
||||
```
|
||||
state init_state;
|
||||
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
|
||||
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
|
||||
```
|
||||
(2)
|
||||
```
|
||||
esekfom::esekf<state, process_noise_dof, input> kf;
|
||||
```
|
||||
Where **process_noise_dof** is the dimension of process noise, of type of std int
|
||||
|
||||
7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
|
||||
```
|
||||
double epsi[state_dof] = {0.001};
|
||||
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
|
||||
kf.init_dyn_runtime(f, df_dx, df_dw, dh_dx, dh_dv, Maximum_iter, epsi);
|
||||
```
|
||||
8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
|
||||
```
|
||||
kf.predict(dt, Q, in); // process noise covariance: Q
|
||||
```
|
||||
9. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> :
|
||||
```
|
||||
measurement h(state &s, bool &valid) //the iteration stops before convergence when valid is false
|
||||
{
|
||||
if (condition) valid = false; // the update iteration could be stopped when the condition other than convergence is satisfied
|
||||
measurement h_;
|
||||
h_.pos = s.pos;
|
||||
return h_;
|
||||
}
|
||||
```
|
||||
then an iterated update is executed:
|
||||
```
|
||||
kf.update_iterated_dyn_runtime(z, R, h); // measurement noise covariance: R, an Eigen matrix
|
||||
```
|
||||
*Remarks(1):*
|
||||
- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
|
||||
5. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
|
||||
|
||||
(1) initial state and covariance:
|
||||
```
|
||||
state init_state;
|
||||
esekfom::esekf<state, process_noise_dof, input>::cov init_P;
|
||||
esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
|
||||
```
|
||||
(2) default state and covariance:
|
||||
```
|
||||
esekfom::esekf<state, process_noise_dof, input> kf;
|
||||
```
|
||||
6. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
|
||||
```
|
||||
double epsi[state_dof] = {0.001};
|
||||
fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
|
||||
kf.init_dyn_runtime_share(f, df_dx, df_dw, Maximum_iter, epsi);
|
||||
```
|
||||
7. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
|
||||
```
|
||||
kf.predict(dt, Q, in); // process noise covariance: Q. an Eigen matrix
|
||||
```
|
||||
8. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
|
||||
```
|
||||
measurement h_dyn_runtime_share(state &s, esekfom::dyn_runtime_share_datastruct<double> &dyn_runtime_share_data)
|
||||
{
|
||||
if(dyn_runtime_share_data.converge) {} // this value is true means iteration is converged
|
||||
if(condition) dyn_runtime_share_data.valid = false; // the iteration stops before convergence when this value is false, if conditions other than convergence is satisfied
|
||||
dyn_runtime_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation
|
||||
dyn_runtime_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
|
||||
dyn_runtime_share_data.R = R; // R is the measurement noise covariance
|
||||
|
||||
measurement h_;
|
||||
h_.pos = s.pos;
|
||||
return h_;
|
||||
}
|
||||
```
|
||||
This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
|
||||
|
||||
then an iterated update is executed:
|
||||
```
|
||||
kf.update_iterated_dyn_runtime_share(z, h_dyn_runtime_share);
|
||||
```
|
||||
|
||||
*Remarks(2):*
|
||||
- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
|
||||
```
|
||||
state set_x;
|
||||
kf.change_x(set_x);
|
||||
esekfom::esekf<state, process_noise_dof, input>::cov set_P;
|
||||
kf.change_P(set_P);
|
||||
```
|
||||
|
||||
## 5. Run the sample
|
||||
Clone the repository:
|
||||
|
||||
```
|
||||
git clone https://github.com/hku-mars/IKFoM.git
|
||||
```
|
||||
In the **Samples** file folder, there is the scource code that applys the **IKFoM** on the original source code from [FAST LIO](https://github.com/hku-mars/FAST_LIO). Please follow the README.md shown in that repository excepting the step **2. Build**, which is modified as:
|
||||
```
|
||||
cd ~/catkin_ws/src
|
||||
cp -r ~/IKFoM/Samples/FAST_LIO-stable FAST_LIO-stable
|
||||
cd ..
|
||||
catkin_make
|
||||
source devel/setup.bash
|
||||
```
|
||||
|
||||
## 6.Acknowledgments
|
||||
Thanks for C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
|
||||
|
||||
@@ -0,0 +1,189 @@
|
||||
#ifndef COMMON_LIB_H
|
||||
#define COMMON_LIB_H
|
||||
|
||||
#include <so3_math.h>
|
||||
#include <Eigen/Eigen>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <deque>
|
||||
using namespace std;
|
||||
using namespace Eigen;
|
||||
|
||||
#define PI_M (3.14159265358)
|
||||
#define G_m_s2 (9.81) // Gravity const in GuangDong/China
|
||||
#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3)
|
||||
#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3)
|
||||
#define CUBE_LEN (6.0)
|
||||
#define LIDAR_SP_LEN (2)
|
||||
#define INIT_COV (0.0001)
|
||||
#define NUM_MATCH_POINTS (5)
|
||||
#define MAX_MEAS_DIM (10000)
|
||||
|
||||
#define VEC_FROM_ARRAY(v) v[0],v[1],v[2]
|
||||
#define VEC_FROM_ARRAY_SIX(v) v[0],v[1],v[2],v[3],v[4],v[5]
|
||||
#define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8]
|
||||
#define CONSTRAIN(v,min,max) ((v>min)?((v<max)?v:max):min)
|
||||
#define ARRAY_FROM_EIGEN(mat) mat.data(), mat.data() + mat.rows() * mat.cols()
|
||||
#define STD_VEC_FROM_EIGEN(mat) vector<decltype(mat)::Scalar> (mat.data(), mat.data() + mat.rows() * mat.cols())
|
||||
#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name))
|
||||
|
||||
typedef pcl::PointXYZINormal PointType;
|
||||
typedef pcl::PointXYZRGB PointTypeRGB;
|
||||
typedef pcl::PointCloud<PointType> PointCloudXYZI;
|
||||
typedef pcl::PointCloud<PointTypeRGB> PointCloudXYZRGB;
|
||||
typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
|
||||
typedef Vector3d V3D;
|
||||
typedef Matrix3d M3D;
|
||||
typedef Vector3f V3F;
|
||||
typedef Matrix3f M3F;
|
||||
|
||||
#define MD(a,b) Matrix<double, (a), (b)>
|
||||
#define VD(a) Matrix<double, (a), 1>
|
||||
#define MF(a,b) Matrix<float, (a), (b)>
|
||||
#define VF(a) Matrix<float, (a), 1>
|
||||
|
||||
const M3D Eye3d(M3D::Identity());
|
||||
const M3F Eye3f(M3F::Identity());
|
||||
const V3D Zero3d(0, 0, 0);
|
||||
const V3F Zero3f(0, 0, 0);
|
||||
|
||||
struct MeasureGroup // Lidar data and imu dates for the curent process
|
||||
{
|
||||
MeasureGroup()
|
||||
{
|
||||
lidar_beg_time = 0.0;
|
||||
lidar_last_time = 0.0;
|
||||
this->lidar.reset(new PointCloudXYZI());
|
||||
};
|
||||
double lidar_beg_time;
|
||||
double lidar_last_time;
|
||||
PointCloudXYZI::Ptr lidar;
|
||||
deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu{};
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
T calc_dist(PointType p1, PointType p2){
|
||||
T d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
|
||||
return d;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T calc_dist(Eigen::Vector3d p1, PointType p2){
|
||||
T d = (p1(0) - p2.x) * (p1(0) - p2.x) + (p1(1) - p2.y) * (p1(1) - p2.y) + (p1(2) - p2.z) * (p1(2) - p2.z);
|
||||
return d;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
std::vector<int> time_compressing(const PointCloudXYZI::Ptr &point_cloud)
|
||||
{
|
||||
int points_size = point_cloud->points.size();
|
||||
int j = 0;
|
||||
std::vector<int> time_seq;
|
||||
// time_seq.clear();
|
||||
time_seq.reserve(points_size);
|
||||
for(int i = 0; i < points_size - 1; i++)
|
||||
{
|
||||
j++;
|
||||
if (point_cloud->points[i+1].curvature > point_cloud->points[i].curvature)
|
||||
{
|
||||
time_seq.emplace_back(j);
|
||||
j = 0;
|
||||
}
|
||||
}
|
||||
if (j == 0)
|
||||
{
|
||||
time_seq.emplace_back(1);
|
||||
}
|
||||
else
|
||||
{
|
||||
time_seq.emplace_back(j+1);
|
||||
}
|
||||
return time_seq;
|
||||
}
|
||||
|
||||
/* comment
|
||||
plane equation: Ax + By + Cz + D = 0
|
||||
convert to: A/D*x + B/D*y + C/D*z = -1
|
||||
solve: A0*x0 = b0
|
||||
where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T
|
||||
normvec: normalized x0
|
||||
*/
|
||||
template<typename T>
|
||||
bool esti_normvector(Matrix<T, 3, 1> &normvec, const PointVector &point, const T &threshold, const int &point_num)
|
||||
{
|
||||
MatrixXf A(point_num, 3);
|
||||
MatrixXf b(point_num, 1);
|
||||
b.setOnes();
|
||||
b *= -1.0f;
|
||||
|
||||
for (int j = 0; j < point_num; j++)
|
||||
{
|
||||
A(j,0) = point[j].x;
|
||||
A(j,1) = point[j].y;
|
||||
A(j,2) = point[j].z;
|
||||
}
|
||||
normvec = A.colPivHouseholderQr().solve(b);
|
||||
|
||||
for (int j = 0; j < point_num; j++)
|
||||
{
|
||||
if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
normvec.normalize();
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &threshold)
|
||||
{
|
||||
Matrix<T, NUM_MATCH_POINTS, 3> A;
|
||||
Matrix<T, NUM_MATCH_POINTS, 1> b;
|
||||
A.setZero();
|
||||
b.setOnes();
|
||||
b *= -1.0f;
|
||||
|
||||
for (int j = 0; j < NUM_MATCH_POINTS; j++)
|
||||
{
|
||||
A(j,0) = point[j].x;
|
||||
A(j,1) = point[j].y;
|
||||
A(j,2) = point[j].z;
|
||||
}
|
||||
|
||||
Matrix<T, 3, 1> normvec = A.colPivHouseholderQr().solve(b);
|
||||
|
||||
T n = normvec.norm();
|
||||
pca_result(0) = normvec(0) / n;
|
||||
pca_result(1) = normvec(1) / n;
|
||||
pca_result(2) = normvec(2) / n;
|
||||
pca_result(3) = 1.0 / n;
|
||||
|
||||
for (int j = 0; j < NUM_MATCH_POINTS; j++)
|
||||
{
|
||||
if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
inline double get_time_sec(const builtin_interfaces::msg::Time &time)
|
||||
{
|
||||
return rclcpp::Time(time).seconds();
|
||||
}
|
||||
|
||||
inline rclcpp::Time get_ros_time(double timestamp)
|
||||
{
|
||||
int32_t sec = std::floor(timestamp);
|
||||
auto nanosec_d = (timestamp - std::floor(timestamp)) * 1e9;
|
||||
uint32_t nanosec = nanosec_d;
|
||||
return rclcpp::Time(sec, nanosec);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,2 @@
|
||||
# ikd-Tree
|
||||
ikd-Tree is an incremental k-d tree for robotic applications.
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,344 @@
|
||||
#pragma once
|
||||
#include <stdio.h>
|
||||
#include <queue>
|
||||
#include <pthread.h>
|
||||
#include <chrono>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
#include <math.h>
|
||||
#include <algorithm>
|
||||
#include <memory.h>
|
||||
#include <pcl/point_types.h>
|
||||
|
||||
#define EPSS 1e-6
|
||||
#define Minimal_Unbalanced_Tree_Size 10
|
||||
#define Multi_Thread_Rebuild_Point_Num 1500
|
||||
#define DOWNSAMPLE_SWITCH true
|
||||
#define ForceRebuildPercentage 0.2
|
||||
#define Q_LEN 1000000
|
||||
|
||||
using namespace std;
|
||||
|
||||
// typedef pcl::PointXYZINormal PointType;
|
||||
// typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
|
||||
|
||||
struct BoxPointType
|
||||
{
|
||||
float vertex_min[3];
|
||||
float vertex_max[3];
|
||||
};
|
||||
|
||||
enum operation_set
|
||||
{
|
||||
ADD_POINT,
|
||||
DELETE_POINT,
|
||||
DELETE_BOX,
|
||||
ADD_BOX,
|
||||
DOWNSAMPLE_DELETE,
|
||||
PUSH_DOWN
|
||||
};
|
||||
|
||||
enum delete_point_storage_set
|
||||
{
|
||||
NOT_RECORD,
|
||||
DELETE_POINTS_REC,
|
||||
MULTI_THREAD_REC
|
||||
};
|
||||
|
||||
template <typename PointType>
|
||||
class KD_TREE
|
||||
{
|
||||
// using MANUAL_Q_ = MANUAL_Q<typename PointType>;
|
||||
// using PointVector = std::vector<PointType>;
|
||||
|
||||
// using MANUAL_Q_ = MANUAL_Q<typename PointType>;
|
||||
public:
|
||||
using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
|
||||
using Ptr = std::shared_ptr<KD_TREE<PointType>>;
|
||||
|
||||
struct KD_TREE_NODE
|
||||
{
|
||||
PointType point;
|
||||
int division_axis;
|
||||
int TreeSize = 1;
|
||||
int invalid_point_num = 0;
|
||||
int down_del_num = 0;
|
||||
bool point_deleted = false;
|
||||
bool tree_deleted = false;
|
||||
bool point_downsample_deleted = false;
|
||||
bool tree_downsample_deleted = false;
|
||||
bool need_push_down_to_left = false;
|
||||
bool need_push_down_to_right = false;
|
||||
bool working_flag = false;
|
||||
pthread_mutex_t push_down_mutex_lock;
|
||||
float node_range_x[2], node_range_y[2], node_range_z[2];
|
||||
float radius_sq;
|
||||
KD_TREE_NODE *left_son_ptr = nullptr;
|
||||
KD_TREE_NODE *right_son_ptr = nullptr;
|
||||
KD_TREE_NODE *father_ptr = nullptr;
|
||||
// For paper data record
|
||||
float alpha_del;
|
||||
float alpha_bal;
|
||||
};
|
||||
|
||||
struct Operation_Logger_Type
|
||||
{
|
||||
PointType point;
|
||||
BoxPointType boxpoint;
|
||||
bool tree_deleted, tree_downsample_deleted;
|
||||
operation_set op;
|
||||
};
|
||||
// static const PointType zeroP;
|
||||
|
||||
struct PointType_CMP
|
||||
{
|
||||
PointType point;
|
||||
float dist = 0.0;
|
||||
PointType_CMP(PointType p = PointType(), float d = INFINITY)
|
||||
{
|
||||
this->point = p;
|
||||
this->dist = d;
|
||||
};
|
||||
bool operator<(const PointType_CMP &a) const
|
||||
{
|
||||
if (fabs(dist - a.dist) < 1e-10)
|
||||
return point.x < a.point.x;
|
||||
else
|
||||
return dist < a.dist;
|
||||
}
|
||||
};
|
||||
|
||||
class MANUAL_HEAP
|
||||
{
|
||||
|
||||
public:
|
||||
MANUAL_HEAP(int max_capacity = 100)
|
||||
|
||||
{
|
||||
cap = max_capacity;
|
||||
heap = new PointType_CMP[max_capacity];
|
||||
heap_size = 0;
|
||||
}
|
||||
|
||||
~MANUAL_HEAP()
|
||||
{
|
||||
delete[] heap;
|
||||
}
|
||||
void pop()
|
||||
{
|
||||
if (heap_size == 0)
|
||||
return;
|
||||
heap[0] = heap[heap_size - 1];
|
||||
heap_size--;
|
||||
MoveDown(0);
|
||||
return;
|
||||
}
|
||||
PointType_CMP top()
|
||||
{
|
||||
return heap[0];
|
||||
}
|
||||
void push(PointType_CMP point)
|
||||
{
|
||||
if (heap_size >= cap)
|
||||
return;
|
||||
heap[heap_size] = point;
|
||||
FloatUp(heap_size);
|
||||
heap_size++;
|
||||
return;
|
||||
}
|
||||
int size()
|
||||
{
|
||||
return heap_size;
|
||||
}
|
||||
void clear()
|
||||
{
|
||||
heap_size = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
private:
|
||||
PointType_CMP *heap;
|
||||
void MoveDown(int heap_index)
|
||||
{
|
||||
int l = heap_index * 2 + 1;
|
||||
PointType_CMP tmp = heap[heap_index];
|
||||
while (l < heap_size)
|
||||
{
|
||||
if (l + 1 < heap_size && heap[l] < heap[l + 1])
|
||||
l++;
|
||||
if (tmp < heap[l])
|
||||
{
|
||||
heap[heap_index] = heap[l];
|
||||
heap_index = l;
|
||||
l = heap_index * 2 + 1;
|
||||
}
|
||||
else
|
||||
break;
|
||||
}
|
||||
heap[heap_index] = tmp;
|
||||
return;
|
||||
}
|
||||
void FloatUp(int heap_index)
|
||||
{
|
||||
int ancestor = (heap_index - 1) / 2;
|
||||
PointType_CMP tmp = heap[heap_index];
|
||||
while (heap_index > 0)
|
||||
{
|
||||
if (heap[ancestor] < tmp)
|
||||
{
|
||||
heap[heap_index] = heap[ancestor];
|
||||
heap_index = ancestor;
|
||||
ancestor = (heap_index - 1) / 2;
|
||||
}
|
||||
else
|
||||
break;
|
||||
}
|
||||
heap[heap_index] = tmp;
|
||||
return;
|
||||
}
|
||||
int heap_size = 0;
|
||||
int cap = 0;
|
||||
};
|
||||
|
||||
class MANUAL_Q
|
||||
{
|
||||
private:
|
||||
int head = 0, tail = 0, counter = 0;
|
||||
Operation_Logger_Type q[Q_LEN];
|
||||
bool is_empty;
|
||||
|
||||
public:
|
||||
void pop()
|
||||
{
|
||||
if (counter == 0)
|
||||
return;
|
||||
head++;
|
||||
head %= Q_LEN;
|
||||
counter--;
|
||||
if (counter == 0)
|
||||
is_empty = true;
|
||||
return;
|
||||
}
|
||||
Operation_Logger_Type front()
|
||||
{
|
||||
return q[head];
|
||||
}
|
||||
Operation_Logger_Type back()
|
||||
{
|
||||
return q[tail];
|
||||
}
|
||||
void clear()
|
||||
{
|
||||
head = 0;
|
||||
tail = 0;
|
||||
counter = 0;
|
||||
is_empty = true;
|
||||
return;
|
||||
}
|
||||
void push(Operation_Logger_Type op)
|
||||
{
|
||||
q[tail] = op;
|
||||
counter++;
|
||||
if (is_empty)
|
||||
is_empty = false;
|
||||
tail++;
|
||||
tail %= Q_LEN;
|
||||
}
|
||||
bool empty()
|
||||
{
|
||||
return is_empty;
|
||||
}
|
||||
int size()
|
||||
{
|
||||
return counter;
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
// Multi-thread Tree Rebuild
|
||||
bool termination_flag = false;
|
||||
bool rebuild_flag = false;
|
||||
pthread_t rebuild_thread;
|
||||
pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex;
|
||||
pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock;
|
||||
// queue<Operation_Logger_Type> Rebuild_Logger;
|
||||
MANUAL_Q Rebuild_Logger;
|
||||
PointVector Rebuild_PCL_Storage;
|
||||
KD_TREE_NODE **Rebuild_Ptr = nullptr;
|
||||
int search_mutex_counter = 0;
|
||||
static void *multi_thread_ptr(void *arg);
|
||||
void multi_thread_rebuild();
|
||||
void start_thread();
|
||||
void stop_thread();
|
||||
void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation);
|
||||
// KD Tree Functions and augmented variables
|
||||
int Treesize_tmp = 0, Validnum_tmp = 0;
|
||||
float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0;
|
||||
float delete_criterion_param = 0.5f;
|
||||
float balance_criterion_param = 0.7f;
|
||||
float downsample_size = 0.2f;
|
||||
bool Delete_Storage_Disabled = false;
|
||||
KD_TREE_NODE *STATIC_ROOT_NODE = nullptr;
|
||||
PointVector Points_deleted;
|
||||
PointVector Downsample_Storage;
|
||||
PointVector Multithread_Points_deleted;
|
||||
void InitTreeNode(KD_TREE_NODE *root);
|
||||
void Test_Lock_States(KD_TREE_NODE *root);
|
||||
void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage);
|
||||
void Rebuild(KD_TREE_NODE **root);
|
||||
int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample);
|
||||
void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild);
|
||||
void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis);
|
||||
void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild);
|
||||
void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue<PointType_CMP>
|
||||
void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage);
|
||||
void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage);
|
||||
bool Criterion_Check(KD_TREE_NODE *root);
|
||||
void Push_Down(KD_TREE_NODE *root);
|
||||
void Update(KD_TREE_NODE *root);
|
||||
void delete_tree_nodes(KD_TREE_NODE **root);
|
||||
void downsample(KD_TREE_NODE **root);
|
||||
bool same_point(PointType a, PointType b);
|
||||
float calc_dist(PointType a, PointType b);
|
||||
float calc_box_dist(KD_TREE_NODE *node, PointType point);
|
||||
static bool point_cmp_x(PointType a, PointType b);
|
||||
static bool point_cmp_y(PointType a, PointType b);
|
||||
static bool point_cmp_z(PointType a, PointType b);
|
||||
|
||||
public:
|
||||
KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2);
|
||||
~KD_TREE();
|
||||
void Set_delete_criterion_param(float delete_param)
|
||||
{
|
||||
delete_criterion_param = delete_param;
|
||||
}
|
||||
void Set_balance_criterion_param(float balance_param)
|
||||
{
|
||||
balance_criterion_param = balance_param;
|
||||
}
|
||||
void set_downsample_param(float downsample_param)
|
||||
{
|
||||
downsample_size = downsample_param;
|
||||
}
|
||||
void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2);
|
||||
int size();
|
||||
int validnum();
|
||||
void root_alpha(float &alpha_bal, float &alpha_del);
|
||||
void Build(PointVector point_cloud);
|
||||
void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> &Point_Distance, float max_dist = INFINITY);
|
||||
void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage);
|
||||
void Radius_Search(PointType point, const float radius, PointVector &Storage);
|
||||
int Add_Points(PointVector &PointToAdd, bool downsample_on);
|
||||
void Add_Point_Boxes(vector<BoxPointType> &BoxPoints);
|
||||
void Delete_Points(PointVector &PointToDel);
|
||||
int Delete_Point_Boxes(vector<BoxPointType> &BoxPoints);
|
||||
void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type);
|
||||
void acquire_removed_points(PointVector &removed_points);
|
||||
BoxPointType tree_range();
|
||||
PointVector PCL_Storage;
|
||||
KD_TREE_NODE *Root_Node = nullptr;
|
||||
int max_queue_size = 0;
|
||||
};
|
||||
|
||||
// template <typename PointType>
|
||||
// PointType KD_TREE<PointType>::zeroP = PointType(0,0,0);
|
||||
@@ -0,0 +1,113 @@
|
||||
#ifndef SO3_MATH_H
|
||||
#define SO3_MATH_H
|
||||
|
||||
#include <math.h>
|
||||
#include <Eigen/Core>
|
||||
|
||||
// #include <common_lib.h>
|
||||
|
||||
#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
|
||||
|
||||
template<typename T>
|
||||
Eigen::Matrix<T, 3, 3> skew_sym_mat(const Eigen::Matrix<T, 3, 1> &v)
|
||||
{
|
||||
Eigen::Matrix<T, 3, 3> skew_sym_mat;
|
||||
skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0;
|
||||
return skew_sym_mat;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
|
||||
{
|
||||
T ang_norm = ang.norm();
|
||||
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
|
||||
if (ang_norm > 0.0000001)
|
||||
{
|
||||
Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
|
||||
Eigen::Matrix<T, 3, 3> K;
|
||||
K << SKEW_SYM_MATRX(r_axis);
|
||||
/// Roderigous Tranformation
|
||||
return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
|
||||
}
|
||||
else
|
||||
{
|
||||
return Eye3;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T, typename Ts>
|
||||
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
|
||||
{
|
||||
T ang_vel_norm = ang_vel.norm();
|
||||
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
|
||||
|
||||
if (ang_vel_norm > 0.0000001)
|
||||
{
|
||||
Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;
|
||||
Eigen::Matrix<T, 3, 3> K;
|
||||
|
||||
K << SKEW_SYM_MATRX(r_axis);
|
||||
|
||||
T r_ang = ang_vel_norm * dt;
|
||||
|
||||
/// Roderigous Tranformation
|
||||
return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;
|
||||
}
|
||||
else
|
||||
{
|
||||
return Eye3;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
|
||||
{
|
||||
T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
|
||||
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
|
||||
if (norm > 0.00001)
|
||||
{
|
||||
T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
|
||||
Eigen::Matrix<T, 3, 3> K;
|
||||
K << SKEW_SYM_MATRX(r_ang);
|
||||
|
||||
/// Roderigous Tranformation
|
||||
return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
|
||||
}
|
||||
else
|
||||
{
|
||||
return Eye3;
|
||||
}
|
||||
}
|
||||
|
||||
/* Logrithm of a Rotation Matrix */
|
||||
template<typename T>
|
||||
Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> &R)
|
||||
{
|
||||
T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));
|
||||
Eigen::Matrix<T,3,1> K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
|
||||
return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
|
||||
{
|
||||
T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
|
||||
bool singular = sy < 1e-6;
|
||||
T x, y, z;
|
||||
if(!singular)
|
||||
{
|
||||
x = atan2(rot(2, 1), rot(2, 2));
|
||||
y = atan2(-rot(2, 0), sy);
|
||||
z = atan2(rot(1, 0), rot(0, 0));
|
||||
}
|
||||
else
|
||||
{
|
||||
x = atan2(-rot(1, 2), rot(1, 1));
|
||||
y = atan2(-rot(2, 0), sy);
|
||||
z = 0;
|
||||
}
|
||||
Eigen::Matrix<T, 3, 1> ang(x, y, z);
|
||||
return ang;
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user