Skip to content

Commit

Permalink
FAST-LIO 1 updated
Browse files Browse the repository at this point in the history
  • Loading branch information
@XW-HKU
XW-HKU committed Jan 14, 2021
1 parent fc3f451 commit 487dcb0
Show file tree
Hide file tree
Showing 9 changed files with 876 additions and 467 deletions.
Empty file added config/fast_lio.yaml
View file
Empty file.
79 changes: 55 additions & 24 deletions include/common_lib.h
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#ifndef COMMON_LIB_H
#define COMMON_LIB_H

#include <Exp_mat.h>
#include <so3_math.h>
#include <Eigen/Eigen>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
Expand All @@ -14,21 +14,23 @@


// #define DEBUG_PRINT
// #define USE_ikdtree

#define PI_M (3.14159265358)
#define G_m_s2 (9.8099) // Gravaty const in GuangDong/China
#define DIM_OF_STATES (18) // Dimension of states (Let Dim(SO(3)) = 3)
#define DIM_OF_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 INIT_COV (0.0001)

#define VEC_FROM_ARRAY(v) v[0],v[1],v[2]
#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) std::vector<decltype(mat)::Scalar> (mat.data(), mat.data() + mat.rows() * mat.cols())


#define DEBUG_FILE_DIR(name) (std::string(std::string(ROOT_DIR) + "Log/"+ name))

typedef fast_lio::Pose6D Pose6D;
Expand Down Expand Up @@ -65,9 +67,45 @@ struct StatesGroup
this->cov = Eigen::Matrix<double,DIM_OF_STATES,DIM_OF_STATES>::Identity() * INIT_COV;
};

StatesGroup(const StatesGroup& b) {
this->rot_end = b.rot_end;
this->pos_end = b.pos_end;
this->vel_end = b.vel_end;
this->bias_g = b.bias_g;
this->bias_a = b.bias_a;
this->gravity = b.gravity;
this->cov = b.cov;
};

StatesGroup& operator=(const StatesGroup& b)
{
this->rot_end = b.rot_end;
this->pos_end = b.pos_end;
this->vel_end = b.vel_end;
this->bias_g = b.bias_g;
this->bias_a = b.bias_a;
this->gravity = b.gravity;
this->cov = b.cov;
return *this;
};


StatesGroup operator+(const Eigen::Matrix<double, DIM_OF_STATES, 1> &state_add)
{
StatesGroup a;
a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
a.pos_end = this->pos_end + state_add.block<3,1>(3,0);
a.vel_end = this->vel_end + state_add.block<3,1>(6,0);
a.bias_g = this->bias_g + state_add.block<3,1>(9,0);
a.bias_a = this->bias_a + state_add.block<3,1>(12,0);
a.gravity = this->gravity + state_add.block<3,1>(15,0);
a.cov = this->cov;
return a;
};

StatesGroup& operator+=(const Eigen::Matrix<double, DIM_OF_STATES, 1> &state_add)
{
this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
this->pos_end += state_add.block<3,1>(3,0);
this->vel_end += state_add.block<3,1>(6,0);
this->bias_g += state_add.block<3,1>(9,0);
Expand All @@ -76,6 +114,19 @@ struct StatesGroup
return *this;
};

Eigen::Matrix<double, DIM_OF_STATES, 1> operator-(const StatesGroup& b)
{
Eigen::Matrix<double, DIM_OF_STATES, 1> a;
Eigen::Matrix3d rotd(b.rot_end.transpose() * this->rot_end);
a.block<3,1>(0,0) = Log(rotd);
a.block<3,1>(3,0) = this->pos_end - b.pos_end;
a.block<3,1>(6,0) = this->vel_end - b.vel_end;
a.block<3,1>(9,0) = this->bias_g - b.bias_g;
a.block<3,1>(12,0) = this->bias_a - b.bias_a;
a.block<3,1>(15,0) = this->gravity - b.gravity;
return a;
};

Eigen::Matrix3d rot_end; // the estimated attitude (rotation matrix) at the end lidar point
Eigen::Vector3d pos_end; // the estimated position at the end lidar point (world frame)
Eigen::Vector3d vel_end; // the estimated velocity at the end lidar point (world frame)
Expand Down Expand Up @@ -115,26 +166,6 @@ auto set_pose6d(const double t, const Eigen::Matrix<T, 3, 1> &a, const Eigen::Ma
return std::move(rot_kp);
}

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
105 changes: 105 additions & 0 deletions include/so3_math.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
#ifndef SO3_MATH_H
#define SO3_MATH_H

#include <math.h>
#include <Eigen/Core>
#include <opencv/cv.h>
// #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> 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
6 changes: 3 additions & 3 deletions launch/mapping_avia.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,12 @@
<param name="edgeb" type="double" value="0.1"/>
<param name="smallp_intersect" type="double" value="172.5"/>
<param name="smallp_ratio" type="double" value="1.2"/>
<param name="point_filter_num" type="int" value="4"/>
<param name="point_filter_num" type="int" value="1"/>
<node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>

<node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">
<node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen" required="true">
<param name="map_file_path" type="string" value=" " />
<param name="max_iteration" type="int" value="2" />
<param name="max_iteration" type="int" value="10" />
<param name="dense_map_enable" type="bool" value="true" />
<param name="fov_degree" type="double" value="75" />
<param name="filter_size_corner" type="double" value="0.3" />
Expand Down
10 changes: 5 additions & 5 deletions launch/mapping_avia_outdoor.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,22 +20,22 @@
<param name="edgeb" type="double" value="0.1"/>
<param name="smallp_intersect" type="double" value="172.5"/>
<param name="smallp_ratio" type="double" value="1.2"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="point_filter_num" type="int" value="1"/>
<node pkg="fast_lio" type="loam_feat_extract" name="feature_extract" output="screen"/>

<node pkg="fast_lio" type="loam_laserMapping" name="laserMapping" output="screen">
<param name="map_file_path" type="string" value=" " />
<param name="max_iteration" type="int" value="10" />
<param name="dense_map_enable" type="bool" value="true" />
<param name="fov_degree" type="double" value="80" />
<param name="fov_degree" type="double" value="70" />
<param name="filter_size_corner" type="double" value="0.5" />
<param name="filter_size_surf" type="double" value="0.4" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.4" />
<param name="cube_side_length" type="double" value="50" />
</node>

<group if="$(arg rviz)">
<!-- <group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
</group>
</group> -->

</launch>
Loading

0 comments on commit 487dcb0

Please sign in to comment.