LOAM系列——FLOAM配置、安装、问题解决及VLP16测试效果及关键记录
安装依赖
- Ubuntu and ROS
- Ceres Solver
- PCL
- Trajectory visualization
sudo apt-get install ros-melodic-hector-trajectory-server
安装
cd ~/test_ws/src
git clone https://github.com/wh200720041/floam.git
cd ..
catkin_make -DCATKIN_WHITELIST_PACKAGES="floam"
KITTI sequence 07
source ~/test_ws/devel/setup.bash
roslaunch floam floam.launch #launch文件中包含bag的运行
如若想同时生成地图，则运行下面命令
roslaunch floam floam_mapping.launch
其中 floam.launch文件内容如下：
<?xml version="1.0"?>
<launch>
<node pkg="rosbag" type="play" name="rosbag_play" args="--clock $(env HOME)/test_bag/2011_09_30_0027.bag"/>
<!-- For Velodyne VLP-16
<param name="scan_line" value="16" />
-->
<!-- For Velodyne HDL-32
<param name="scan_line" value="32" />
-->
<!-- For Velodyne HDL-64 -->
<param name="scan_line" value="64" />
<!--- Sim Time -->
<param name="/use_sim_time" value="true" />
<param name="scan_period" value="0.1" />
<param name="vertical_angle" type="double" value="2.0" />
<param name="max_dis" type="double" value="90.0" />
<param name="min_dis" type="double" value="3.0" />
<!--- -->
<node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
<node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
<node pkg="tf" type="static_transform_publisher" name="word2map_tf" args="0 0 0 0 0 0 /world /map 10" />
<arg name="rviz" default="true" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find floam)/rviz/floam.rviz" />
</group>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="gt" >
<param name="/target_frame_name" value="world" />
<param name="/source_frame_name" value="velodyne" />
<param name="/trajectory_update_rate" value="10.0" />
<param name="/trajectory_publish_rate" value="10.0" />
</node>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
<param name="/target_frame_name" value="world" />
<param name="/source_frame_name" value="base_link" />
<param name="/trajectory_update_rate" value="10.0" />
<param name="/trajectory_publish_rate" value="10.0" />
</node>
</launch>
其中 floam_mapping.launch内容如下：
<?xml version="1.0"?>
<launch>
<node pkg="rosbag" type="play" name="rosbag_play" args="--clock -r 0.5 $(env HOME)/test_bag/2011_09_30_0027.bag"/>
<!-- For Velodyne VLP-16
<param name="scan_line" value="16" />
-->
<!-- For Velodyne HDL-32
<param name="scan_line" value="32" />
-->
<!-- For Velodyne HDL-64 -->
<param name="scan_line" value="64" />
<!--- Sim Time -->
<param name="/use_sim_time" value="true" />
<param name="scan_period" value="0.1" />
<param name="vertical_angle" type="double" value="2.0" />
<param name="max_dis" type="double" value="90.0" />
<param name="map_resolution" type="double" value="0.4" />
<param name="min_dis" type="double" value="3.0" />
<!--- -->
<node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
<node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
<node pkg="floam" type="floam_laser_mapping_node" name="floam_laser_mapping_node" output="screen"/>
<node pkg="tf" type="static_transform_publisher" name="word2map_tf" args="0 0 0 0 0 0 /world /map 10" />
<arg name="rviz" default="true" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find floam)/rviz/floam_mapping.rviz" />
</group>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="gt" >
<param name="/target_frame_name" value="world" />
<param name="/source_frame_name" value="velodyne" />
<param name="/trajectory_update_rate" value="10.0" />
<param name="/trajectory_publish_rate" value="10.0" />
</node>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
<param name="/target_frame_name" value="world" />
<param name="/source_frame_name" value="base_link" />
<param name="/trajectory_update_rate" value="10.0" />
<param name="/trajectory_publish_rate" value="10.0" />
</node>
</launch>
VLP16 bag测试
source ~/test_ws/devel/setup.bash
roslaunch floam floam_velodyne.launch
cd ~/test_bag
rosbag play indoor_2021-03-30-15-27-23.bag
或者
roslaunch floam floam_velodyne_with_bag.launch
其中 floam_velodyne.launch 内容如下：
<?xml version="1.0"?>
<launch>
<param name="/use_sim_time" value="true" />
<!-- launch your velodyne here
<include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch"/>
-->
<!-- For Velodyne VLP-16 -->
<param name="scan_line" value="16" />
<!-- For Velodyne HDL-32
<param name="scan_line" value="32" />
-->
<!-- For Velodyne HDL-64
<param name="scan_line" value="64" />
-->
<!--- Sim Time -->
<param name="scan_period" value="0.1" />
<param name="vertical_angle" type="double" value="2.0" />
<param name="map_resolution" type="double" value="0.2" />
<param name="max_dis" type="double" value="90.0" />
<param name="min_dis" type="double" value="0.5" />
<!--- -->
<node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
<node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
<node pkg="floam" type="floam_laser_mapping_node" name="floam_laser_mapping_node" output="screen"/>
<node pkg="tf" type="static_transform_publisher" name="word2map_tf" args="0 0 0 0 0 0 /world /map 10" />
<arg name="rviz" default="true" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find floam)/rviz/floam_velodyne.rviz" />
</group>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
<!-- <param name="/target_frame_name" value="world" /> -->
<param name="/target_frame_name" value="map" />
<param name="/source_frame_name" value="base_link" />
<param name="/trajectory_update_rate" value="10.0" />
<param name="/trajectory_publish_rate" value="10.0" />
</node>
</launch>
其中 loam_velodyne_with_bag.launch 内容如下：
<?xml version="1.0"?>
<launch>
<param name="/use_sim_time" value="true" />
<node pkg="rosbag" type="play" name="rosbag_play" args="--clock -r 0.5 $(env HOME)/test_bag/indoor_2021-03-30-15-27-23.bag"/>
<!-- For Velodyne VLP-16 -->
<param name="scan_line" value="16" />
<!-- For Velodyne HDL-32
<param name="scan_line" value="32" />
-->
<!-- For Velodyne HDL-64
<param name="scan_line" value="64" />
-->
<!--- Sim Time -->
<param name="scan_period" value="0.1" />
<param name="vertical_angle" type="double" value="2.0" />
<param name="map_resolution" type="double" value="0.2" />
<param name="max_dis" type="double" value="90.0" />
<param name="min_dis" type="double" value="0.5" />
<!--- -->
<node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
<node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
<node pkg="floam" type="floam_laser_mapping_node" name="floam_laser_mapping_node" output="screen"/>
<node pkg="tf" type="static_transform_publisher" name="word2map_tf" args="0 0 0 0 0 0 /world /map 10" />
<arg name="rviz" default="true" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find floam)/rviz/floam_velodyne.rviz" />
</group>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
<param name="/target_frame_name" value="world" />
<param name="/source_frame_name" value="base_link" />
<param name="/trajectory_update_rate" value="10.0" />
<param name="/trajectory_publish_rate" value="10.0" />
</node>
</launch>
测试视频链接
floam
问题解决
问题1
问题： floam_laser_mapping_node节点，可能会出现运行一段时间就报错，崩溃了
关键记录
FLOAM主要特点
整体和ALOAM类似，只是使用残差函数的雅可比使用的是解析式求导
代码：https://github.com/wh200720041/floam
雅可比矩阵推导相关知识
注：
这里用的推导方式与上面的稍微有些不同，使用的是如下形式，图1，
这里需要多一步转换，如图2
关键代码
//定义
class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> {
public:
EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_);
virtual ~EdgeAnalyticCostFunction() {}
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
Eigen::Vector3d curr_point;
Eigen::Vector3d last_point_a;
Eigen::Vector3d last_point_b;
};
//实现
EdgeAnalyticCostFunction::EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_)
: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_){
}
bool EdgeAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[0] + 4);
Eigen::Vector3d lp;
lp = q_last_curr * curr_point + t_last_curr;
Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
Eigen::Vector3d de = last_point_a - last_point_b;
double de_norm = de.norm();
residuals[0] = nu.norm()/de_norm;
if(jacobians != NULL)
{
if(jacobians[0] != NULL)
{
Eigen::Matrix3d skew_lp = skew(lp);
Eigen::Matrix<double, 3, 6> dp_by_se3;
dp_by_se3.block<3,3>(0,0) = -skew_lp;
(dp_by_se3.block<3,3>(0, 3)).setIdentity();
Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
J_se3.setZero();
Eigen::Matrix3d skew_de = skew(de);
J_se3.block<1,6>(0,0) = - nu.transpose() / nu.norm() * skew_de * dp_by_se3/de_norm; //关于这里最前面的“-”号问题，是因为 nu = (lp - last_point_a).cross(lp - last_point_b);，推导的时候是(lp - last_point_b)叉乘 (lp - last_point_a)
}
}
return true;
}
//定义
class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> {
public:
SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_);
virtual ~SurfNormAnalyticCostFunction() {}
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
Eigen::Vector3d curr_point;
Eigen::Vector3d plane_unit_norm;
double negative_OA_dot_norm;
};
//实现
SurfNormAnalyticCostFunction::SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_)
: curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_){
}
bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
Eigen::Map<const Eigen::Quaterniond> q_w_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_w_curr(parameters[0] + 4);
Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr;
residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm;
if(jacobians != NULL)
{
if(jacobians[0] != NULL)
{
Eigen::Matrix3d skew_point_w = skew(point_w);
Eigen::Matrix<double, 3, 6> dp_by_se3;
dp_by_se3.block<3,3>(0,0) = -skew_point_w;
(dp_by_se3.block<3,3>(0, 3)).setIdentity();
Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
J_se3.setZero();
J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_se3;
}
}
return true;
}
//定义
class PoseSE3Parameterization : public ceres::LocalParameterization {
public:
PoseSE3Parameterization() {}
virtual ~PoseSE3Parameterization() {}
virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x, double* jacobian) const;
virtual int GlobalSize() const { return 7; }
virtual int LocalSize() const { return 6; }
};
//实现
bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
Eigen::Map<const Eigen::Vector3d> trans(x + 4);
Eigen::Quaterniond delta_q;
Eigen::Vector3d delta_t;
getTransformFromSe3(Eigen::Map<const Eigen::Matrix<double,6,1>>(delta), delta_q, delta_t);
Eigen::Map<const Eigen::Quaterniond> quater(x);
Eigen::Map<Eigen::Quaterniond> quater_plus(x_plus_delta);
Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 4);
quater_plus = delta_q * quater;
trans_plus = delta_q * trans + delta_t;
return true;
}
bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const
{
Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
(j.topRows(6)).setIdentity();
(j.bottomRows(1)).setZero();
return true;
}
......
double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
......
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(parameters, 7, new PoseSE3Parameterization());
......
ceres::CostFunction *cost_function = new EdgeAnalyticCostFunction(curr_point, point_a, point_b);
problem.AddResidualBlock(cost_function, loss_function, parameters);
ceres::CostFunction *cost_function = new SurfNormAnalyticCostFunction(curr_point, norm, negative_OA_dot_norm);
problem.AddResidualBlock(cost_function, loss_function, parameters);
......
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
注：以下内容需要深入理解
Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
J_se3.setZero();
Eigen::Matrix3d skew_de = skew(de);
J_se3.block<1,6>(0,0) = - nu.transpose() / nu.norm() * skew_de * dp_by_se3/de_norm;
Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
J_se3.setZero();
J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_se3;
552
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
