Skip to content

Commit

Permalink
update lidar odometry (#46)
Browse files Browse the repository at this point in the history 
* add pose data type

* update lidar_odometry

* update lidar odometry node

* rename LidarFrame

* update code style for lidar odometry

* code format

* code format

* update tic_toc

* echo elapsed time of lidar odometray process

* code format

* update cloud_subscriber

* add advanced tic toc

* use advanced tic toc for lidar odometry

* set undistort_point_cloud false for kitti preprocess

* code format
  • Loading branch information
@gezp
gezp authored Oct 12, 2023
1 parent 8bd4d31 commit 792d525
Show file tree
Hide file tree
Showing 11 changed files with 332 additions and 116 deletions.
36 changes: 18 additions & 18 deletions lidar_odometry/include/lidar_odometry/lidar_odometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,29 +17,29 @@
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <deque>
#include <string>
#include <memory>
//

#include "localization_common/sensor_data/lidar_data.hpp"
#include "localization_common/sensor_data/odom_data.hpp"
#include "localization_common/sensor_data/pose_data.hpp"
#include "localization_common/cloud_filter/voxel_filter.hpp"
#include "localization_common/cloud_registration/cloud_registration_factory.hpp"

namespace lidar_odometry
{

struct LidarFrame
{
double time;
Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud;
};

class LidarOdometry
{
struct Frame
{
double time;
Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud;
};

public:
LidarOdometry();
bool init_config(const std::string & config_path);
explicit LidarOdometry(const YAML::Node & config);
~LidarOdometry() = default;
void set_extrinsic(const Eigen::Matrix4d & T_base_lidar);
void set_initial_pose(const Eigen::Matrix4d & initial_pose);
bool update(const localization_common::LidarData<pcl::PointXYZ> & lidar_data);
Expand All @@ -49,10 +49,11 @@ class LidarOdometry
bool has_new_local_map();

private:
bool update_history_pose(double time, const Eigen::Matrix4d & pose);
bool get_initial_pose_by_history(Eigen::Matrix4d & initial_pose);
bool check_new_key_frame(const Eigen::Matrix4d & pose);
bool update_local_map();
bool match_scan_to_map(const Eigen::Matrix4d & predict_pose);
bool get_initial_pose_by_history(Eigen::Matrix4d & initial_pose);
bool match_scan_to_map(const Eigen::Matrix4d & predict_pose, Eigen::Matrix4d & final_pose);

private:
std::shared_ptr<localization_common::CloudRegistrationFactory> registration_factory_;
Expand All @@ -67,13 +68,12 @@ class LidarOdometry
Eigen::Matrix4d T_base_lidar_ = Eigen::Matrix4d::Identity();
Eigen::Matrix4d T_lidar_base_ = Eigen::Matrix4d::Identity();
// data
bool has_new_local_map_ = false;
Eigen::Matrix4d initial_pose_ = Eigen::Matrix4d::Identity();
std::deque<LidarFrame> key_frames_;
Frame current_frame_;
std::deque<localization_common::PoseData> history_poses_;
std::deque<Frame> key_frames_;
pcl::PointCloud<pcl::PointXYZ>::Ptr local_map_;
localization_common::LidarData<pcl::PointXYZ> current_lidar_data_;
LidarFrame current_lidar_frame_;
std::deque<LidarFrame> history_frames_;
bool has_new_local_map_ = false;
};

} // namespace lidar_odometry
6 changes: 5 additions & 1 deletion lidar_odometry/include/lidar_odometry/lidar_odometry_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include "localization_common/extrinsics_manager.hpp"
#include "localization_common/msg_utils.hpp"
#include "localization_common/odom_data_buffer.hpp"
#include "localization_common/tic_toc.hpp"
#include "lidar_odometry/lidar_odometry.hpp"

namespace lidar_odometry
Expand All @@ -41,7 +42,8 @@ class LidarOdometryNode

private:
bool run();
bool set_initial_pose_by_reference_odom();
bool get_initial_pose_by_reference_odom(
double time, Eigen::Matrix4d & initial_pose, bool & is_old_data);
void publish_data();

private:
Expand Down Expand Up @@ -70,6 +72,8 @@ class LidarOdometryNode
// params
bool use_initial_pose_from_topic_{false};
bool inited_{false};
//
localization_common::AdvancedTicToc elapsed_time_statistics_;
};

} // namespace lidar_odometry
129 changes: 63 additions & 66 deletions lidar_odometry/src/lidar_odometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,30 +15,21 @@
#include "lidar_odometry/lidar_odometry.hpp"

#include <pcl/common/transforms.h>
#include <pcl/filters/filter.h>

#include <filesystem>
#include <fstream>

namespace lidar_odometry
{
LidarOdometry::LidarOdometry()
LidarOdometry::LidarOdometry(const YAML::Node & config)
{
registration_factory_ = std::make_shared<localization_common::CloudRegistrationFactory>();
}

bool LidarOdometry::init_config(const std::string & config_path)
{
YAML::Node config_node = YAML::LoadFile(config_path);
//
local_frame_num_ = config_node["local_frame_num"].as<int>();
key_frame_distance_ = config_node["key_frame_distance"].as<float>();
local_frame_num_ = config["local_frame_num"].as<int>();
key_frame_distance_ = config["key_frame_distance"].as<float>();
// init registration and filter
registration_ = registration_factory_->create(config_node["registration"]);
registration_ = registration_factory_->create(config["registration"]);
using VoxelFilter = localization_common::VoxelFilter;
local_map_filter_ = std::make_shared<VoxelFilter>(config_node["local_map_filter"]);
current_scan_filter_ = std::make_shared<VoxelFilter>(config_node["current_scan_filter"]);
display_filter_ = std::make_shared<VoxelFilter>(config_node["display_filter"]);
local_map_filter_ = std::make_shared<VoxelFilter>(config["local_map_filter"]);
current_scan_filter_ = std::make_shared<VoxelFilter>(config["current_scan_filter"]);
display_filter_ = std::make_shared<VoxelFilter>(config["display_filter"]);
// print info
std::cout << "cloud registration:" << std::endl;
registration_->print_info();
Expand All @@ -48,7 +39,6 @@ bool LidarOdometry::init_config(const std::string & config_path)
current_scan_filter_->print_info();
std::cout << "display filter:" << std::endl;
display_filter_->print_info();
return true;
}

void LidarOdometry::set_extrinsic(const Eigen::Matrix4d & T_base_lidar)
Expand All @@ -65,55 +55,44 @@ void LidarOdometry::set_initial_pose(const Eigen::Matrix4d & initial_pose)
bool LidarOdometry::update(const localization_common::LidarData<pcl::PointXYZ> & lidar_data)
{
has_new_local_map_ = false;
current_lidar_data_ = lidar_data;
// remove invalid points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*lidar_data.point_cloud, *lidar_data.point_cloud, indices);
current_frame_.time = lidar_data.time;
current_frame_.point_cloud = lidar_data.point_cloud;
if (key_frames_.empty()) {
// initialize the first frame
current_lidar_frame_.time = current_lidar_data_.time;
current_lidar_frame_.point_cloud = current_lidar_data_.point_cloud;
current_lidar_frame_.pose = initial_pose_ * T_base_lidar_;
current_frame_.pose = initial_pose_ * T_base_lidar_;
} else {
// scan to map matching
Eigen::Matrix4d predict_pose = Eigen::Matrix4d::Identity();
if (!get_initial_pose_by_history(predict_pose)) {
std::cout << "failed to get predict pose by history" << std::endl;
}
match_scan_to_map(predict_pose);
}
// add into history_frames_
history_frames_.push_back(current_lidar_frame_);
if (history_frames_.size() > 100) {
history_frames_.pop_front();
match_scan_to_map(predict_pose, current_frame_.pose);
}
// add into history_poses_
update_history_pose(current_frame_.time, current_frame_.pose);
// check if add new frame and update local map
if (check_new_key_frame(current_lidar_frame_.pose)) {
if (check_new_key_frame(current_frame_.pose)) {
has_new_local_map_ = true;
// add new key frame
key_frames_.push_back(current_lidar_frame_);
// move window for local map
while (key_frames_.size() > static_cast<size_t>(local_frame_num_)) {
key_frames_.pop_front();
}
// update
// update local map
update_local_map();
// update target of registration
registration_->set_target(local_map_);
}
return true;
}

localization_common::OdomData LidarOdometry::get_current_odom()
{
localization_common::OdomData odom;
odom.time = current_lidar_frame_.time;
odom.pose = current_lidar_frame_.pose * T_lidar_base_;
odom.time = current_frame_.time;
odom.pose = current_frame_.pose * T_lidar_base_;
return odom;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr LidarOdometry::get_current_scan()
{
auto filtered_cloud = display_filter_->apply(current_lidar_frame_.point_cloud);
pcl::transformPointCloud(*filtered_cloud, *filtered_cloud, current_lidar_frame_.pose);
auto filtered_cloud = display_filter_->apply(current_frame_.point_cloud);
pcl::transformPointCloud(*filtered_cloud, *filtered_cloud, current_frame_.pose);
return filtered_cloud;
}

Expand All @@ -122,7 +101,38 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr LidarOdometry::get_local_map()
return display_filter_->apply(local_map_);
}

bool LidarOdometry::has_new_local_map() {return has_new_local_map_;}
bool LidarOdometry::has_new_local_map()
{
return has_new_local_map_;
}

bool LidarOdometry::update_history_pose(double time, const Eigen::Matrix4d & pose)
{
localization_common::PoseData pose_data;
pose_data.time = time;
pose_data.pose = pose;
history_poses_.push_back(pose_data);
if (history_poses_.size() > 100) {
history_poses_.pop_front();
}
return true;
}

bool LidarOdometry::get_initial_pose_by_history(Eigen::Matrix4d & initial_pose)
{
if (history_poses_.empty()) {
return false;
}
if (history_poses_.size() == 1) {
initial_pose = history_poses_.back().pose;
} else {
Eigen::Matrix4d last_pose1 = history_poses_[history_poses_.size() - 2].pose;
Eigen::Matrix4d last_pose2 = history_poses_.back().pose;
Eigen::Matrix4d step_pose = last_pose1.inverse() * last_pose2;
initial_pose = last_pose2 * step_pose;
}
return true;
}

bool LidarOdometry::check_new_key_frame(const Eigen::Matrix4d & pose)
{
Expand All @@ -138,6 +148,12 @@ bool LidarOdometry::check_new_key_frame(const Eigen::Matrix4d & pose)

bool LidarOdometry::update_local_map()
{
// add new key frame
key_frames_.push_back(current_frame_);
// move window for local map
while (key_frames_.size() > static_cast<size_t>(local_frame_num_)) {
key_frames_.pop_front();
}
// update local map
local_map_.reset(new pcl::PointCloud<pcl::PointXYZ>());
for (size_t i = 0; i < key_frames_.size(); ++i) {
Expand All @@ -149,37 +165,18 @@ bool LidarOdometry::update_local_map()
if (key_frames_.size() >= 10) {
local_map_ = local_map_filter_->apply(local_map_);
}
// update target of registration
registration_->set_target(local_map_);
return true;
}

bool LidarOdometry::match_scan_to_map(const Eigen::Matrix4d & predict_pose)
bool LidarOdometry::match_scan_to_map(
const Eigen::Matrix4d & predict_pose, Eigen::Matrix4d & final_pose)
{
// downsample current lidar point cloud
auto filtered_cloud = current_scan_filter_->apply(current_lidar_data_.point_cloud);
auto filtered_cloud = current_scan_filter_->apply(current_frame_.point_cloud);
// matching
registration_->match(filtered_cloud, predict_pose);
// result
current_lidar_frame_.time = current_lidar_data_.time;
current_lidar_frame_.point_cloud = current_lidar_data_.point_cloud;
current_lidar_frame_.pose = registration_->get_final_pose();
return true;
}

bool LidarOdometry::get_initial_pose_by_history(Eigen::Matrix4d & initial_pose)
{
if (history_frames_.empty()) {
return false;
}
if (history_frames_.size() == 1) {
initial_pose = history_frames_.back().pose;
} else {
Eigen::Matrix4d last_pose1 = history_frames_[history_frames_.size() - 2].pose;
Eigen::Matrix4d last_pose2 = history_frames_.back().pose;
Eigen::Matrix4d step_pose = last_pose1.inverse() * last_pose2;
initial_pose = last_pose2 * step_pose;
}
final_pose = registration_->get_final_pose();
return true;
}

Expand Down
Loading

0 comments on commit 792d525

Please sign in to comment.