Skip to content

Commit

Permalink
Merge pull request #2 from sbgisen/feature/odom_with_covariance
Browse files Browse the repository at this point in the history 
Publish odom with covariance.
  • Loading branch information
@jsupratman13
jsupratman13 authored Nov 2, 2022
2 parents 5138478 + 7e497a9 commit 5c1361b
Show file tree
Hide file tree
Showing 4 changed files with 77 additions and 62 deletions.
133 changes: 73 additions & 60 deletions apps/hdl_localization_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
public:
using PointT = pcl::PointXYZI;

HdlLocalizationNodelet() : tf_buffer(), tf_listener(tf_buffer) {
}
virtual ~HdlLocalizationNodelet() {
}
HdlLocalizationNodelet() : tf_buffer(), tf_listener(tf_buffer) {}
virtual ~HdlLocalizationNodelet() {}

void onInit() override {
nh = getNodeHandle();
Expand Down Expand Up @@ -71,7 +69,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {

// global localization
use_global_localization = private_nh.param<bool>("use_global_localization", true);
if(use_global_localization) {
if (use_global_localization) {
NODELET_INFO_STREAM("wait for global localization services");
ros::service::waitForService("/hdl_global_localization/set_global_map");
ros::service::waitForService("/hdl_global_localization/query");
Expand All @@ -90,7 +88,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
double ndt_neighbor_search_radius = private_nh.param<double>("ndt_neighbor_search_radius", 2.0);
double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);

if(reg_method == "NDT_OMP") {
if (reg_method == "NDT_OMP") {
NODELET_INFO("NDT_OMP is selected");
pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
ndt->setTransformationEpsilon(0.01);
Expand All @@ -111,12 +109,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
}
return ndt;
} else if(reg_method.find("NDT_CUDA") != std::string::npos) {
} else if (reg_method.find("NDT_CUDA") != std::string::npos) {
NODELET_INFO("NDT_CUDA is selected");
boost::shared_ptr<fast_gicp::NDTCuda<PointT, PointT>> ndt(new fast_gicp::NDTCuda<PointT, PointT>);
ndt->setResolution(ndt_resolution);

if(reg_method.find("D2D") != std::string::npos) {
if (reg_method.find("D2D") != std::string::npos) {
ndt->setDistanceMode(fast_gicp::NDTDistanceMode::D2D);
} else if (reg_method.find("P2D") != std::string::npos) {
ndt->setDistanceMode(fast_gicp::NDTDistanceMode::P2D);
Expand Down Expand Up @@ -157,14 +155,18 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
delta_estimater.reset(new DeltaEstimater(create_registration()));

// initialize pose estimator
if(private_nh.param<bool>("specify_init_pose", true)) {
if (private_nh.param<bool>("specify_init_pose", true)) {
NODELET_INFO("initialize pose estimator with specified parameters!!");
pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
pose_estimator.reset(new hdl_localization::PoseEstimator(
registration,
ros::Time::now(),
Eigen::Vector3f(private_nh.param<double>("init_pos_x", 0.0), private_nh.param<double>("init_pos_y", 0.0), private_nh.param<double>("init_pos_z", 0.0)),
Eigen::Quaternionf(private_nh.param<double>("init_ori_w", 1.0), private_nh.param<double>("init_ori_x", 0.0), private_nh.param<double>("init_ori_y", 0.0), private_nh.param<double>("init_ori_z", 0.0)),
private_nh.param<double>("cool_time_duration", 0.5)
));
Eigen::Quaternionf(
private_nh.param<double>("init_ori_w", 1.0),
private_nh.param<double>("init_ori_x", 0.0),
private_nh.param<double>("init_ori_y", 0.0),
private_nh.param<double>("init_ori_z", 0.0)),
private_nh.param<double>("cool_time_duration", 0.5)));
}
}

Expand All @@ -184,12 +186,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
*/
void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
std::lock_guard<std::mutex> estimator_lock(pose_estimator_mutex);
if(!pose_estimator) {
if (!pose_estimator) {
NODELET_ERROR("waiting for initial pose input!!");
return;
}

if(!globalmap) {
if (!globalmap) {
NODELET_ERROR("globalmap has not been received!!");
return;
}
Expand All @@ -198,43 +200,41 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
pcl::fromROSMsg(*points_msg, *pcl_cloud);

if(pcl_cloud->empty()) {
if (pcl_cloud->empty()) {
NODELET_ERROR("cloud is empty!!");
return;
}

// transform pointcloud into odom_child_frame_id
std::string tfError;
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
if(this->tf_buffer.canTransform(odom_child_frame_id, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), &tfError))
{
if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) {
NODELET_ERROR("point cloud cannot be transformed into target frame!!");
return;
}
}else
{
NODELET_ERROR(tfError.c_str());
if (this->tf_buffer.canTransform(odom_child_frame_id, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), &tfError)) {
if (!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) {
NODELET_ERROR("point cloud cannot be transformed into target frame!!");
return;
}
} else {
NODELET_ERROR(tfError.c_str());
return;
}

auto filtered = downsample(cloud);
last_scan = filtered;

if(relocalizing) {
if (relocalizing) {
delta_estimater->add_frame(filtered);
}

Eigen::Matrix4f before = pose_estimator->matrix();

// predict
if(!use_imu) {
if (!use_imu) {
pose_estimator->predict(stamp);
} else {
std::lock_guard<std::mutex> lock(imu_data_mutex);
auto imu_iter = imu_data.begin();
for(imu_iter; imu_iter != imu_data.end(); imu_iter++) {
if(stamp < (*imu_iter)->header.stamp) {
for (imu_iter; imu_iter != imu_data.end(); imu_iter++) {
if (stamp < (*imu_iter)->header.stamp) {
break;
}
const auto& acc = (*imu_iter)->linear_acceleration;
Expand All @@ -248,15 +248,15 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {

// odometry-based prediction
ros::Time last_correction_time = pose_estimator->last_correction_time();
if(private_nh.param<bool>("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) {
if (private_nh.param<bool>("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) {
geometry_msgs::TransformStamped odom_delta;
if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0.1))) {
if (tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0.1))) {
odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0));
} else if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0))) {
} else if (tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0))) {
odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0));
}

if(odom_delta.header.stamp.isZero()) {
if (odom_delta.header.stamp.isZero()) {
NODELET_WARN_STREAM("failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id);
} else {
Eigen::Isometry3d delta = tf2::transformToEigen(odom_delta);
Expand All @@ -265,19 +265,20 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
}

// correct
auto aligned = pose_estimator->correct(stamp, filtered);
double fitness_score;
auto aligned = pose_estimator->correct(stamp, filtered, &fitness_score);

if(aligned_pub.getNumSubscribers()) {
if (aligned_pub.getNumSubscribers()) {
aligned->header.frame_id = "map";
aligned->header.stamp = cloud->header.stamp;
aligned_pub.publish(aligned);
}

if(status_pub.getNumSubscribers()) {
if (status_pub.getNumSubscribers()) {
publish_scan_matching_status(points_msg->header, aligned);
}

publish_odometry(points_msg->header.stamp, pose_estimator->matrix());
publish_odometry(points_msg->header.stamp, pose_estimator->matrix(), fitness_score);
}

/**
Expand All @@ -292,12 +293,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {

registration->setInputTarget(globalmap);

if(use_global_localization) {
if (use_global_localization) {
NODELET_INFO("set globalmap for global localization!");
hdl_global_localization::SetGlobalMap srv;
pcl::toROSMsg(*globalmap, srv.request.global_map);

if(!set_global_map_service.call(srv)) {
if (!set_global_map_service.call(srv)) {
NODELET_INFO("failed to set global map");
} else {
NODELET_INFO("done");
Expand All @@ -310,7 +311,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
* @param
*/
bool relocalize(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) {
if(last_scan == nullptr) {
if (last_scan == nullptr) {
NODELET_INFO_STREAM("no scan has been received");
return false;
}
Expand All @@ -323,7 +324,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
pcl::toROSMsg(*scan, srv.request.cloud);
srv.request.max_num_candidates = 1;

if(!query_global_localization_service.call(srv) || srv.response.poses.empty()) {
if (!query_global_localization_service.call(srv) || srv.response.poses.empty()) {
relocalizing = false;
NODELET_INFO_STREAM("global localization failed");
return false;
Expand Down Expand Up @@ -364,14 +365,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
std::lock_guard<std::mutex> lock(pose_estimator_mutex);
const auto& p = pose_msg->pose.pose.position;
const auto& q = pose_msg->pose.pose.orientation;
pose_estimator.reset(
new hdl_localization::PoseEstimator(
registration,
ros::Time::now(),
Eigen::Vector3f(p.x, p.y, p.z),
Eigen::Quaternionf(q.w, q.x, q.y, q.z),
private_nh.param<double>("cool_time_duration", 0.5))
);
pose_estimator.reset(new hdl_localization::PoseEstimator(
registration,
ros::Time::now(),
Eigen::Vector3f(p.x, p.y, p.z),
Eigen::Quaternionf(q.w, q.x, q.y, q.z),
private_nh.param<double>("cool_time_duration", 0.5)));
}

/**
Expand All @@ -380,7 +379,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
* @return downsampled cloud
*/
pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
if(!downsample_filter) {
if (!downsample_filter) {
return cloud;
}

Expand All @@ -397,10 +396,10 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
* @param stamp timestamp
* @param pose odometry pose to be published
*/
void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) {
void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose, const double fitness_score) {
// broadcast the transform over tf
if(enable_tf){
if(tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) {
if (enable_tf) {
if (tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) {
geometry_msgs::TransformStamped map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(pose.inverse().cast<double>()));
map_wrt_frame.header.stamp = stamp;
map_wrt_frame.header.frame_id = odom_child_frame_id;
Expand Down Expand Up @@ -437,11 +436,26 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
odom.header.stamp = stamp;
odom.header.frame_id = "map";

double fitness_score_coefficient = private_nh.param<double>("odom_covariance_coefficient", 1.0);
tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast<double>()), odom.pose.pose);
for (int i = 0; i < 36; i++) {
if (i % 7 == 0) {
odom.pose.covariance[i] = fitness_score_coefficient * fitness_score;
} else {
odom.pose.covariance[i] = 0.0;
}
}
odom.child_frame_id = odom_child_frame_id;
odom.twist.twist.linear.x = 0.0;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.angular.z = 0.0;
for (int i = 0; i < 36; i++) {
if (i % 7 == 0) {
odom.twist.covariance[i] = fitness_score_coefficient * fitness_score;
} else {
odom.twist.covariance[i] = 0.0;
}
}

pose_pub.publish(odom);
}
Expand All @@ -461,10 +475,10 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
int num_inliers = 0;
std::vector<int> k_indices;
std::vector<float> k_sq_dists;
for(int i = 0; i < aligned->size(); i++) {
for (int i = 0; i < aligned->size(); i++) {
const auto& pt = aligned->at(i);
registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
if (k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
num_inliers++;
}
}
Expand All @@ -476,19 +490,19 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {

std::vector<double> errors(6, 0.0);

if(pose_estimator->wo_prediction_error()) {
if (pose_estimator->wo_prediction_error()) {
status.prediction_labels.push_back(std_msgs::String());
status.prediction_labels.back().data = "without_pred";
status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->wo_prediction_error().get().cast<double>())).transform);
}

if(pose_estimator->imu_prediction_error()) {
if (pose_estimator->imu_prediction_error()) {
status.prediction_labels.push_back(std_msgs::String());
status.prediction_labels.back().data = use_imu ? "imu" : "motion_model";
status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->imu_prediction_error().get().cast<double>())).transform);
}

if(pose_estimator->odom_prediction_error()) {
if (pose_estimator->odom_prediction_error()) {
status.prediction_labels.push_back(std_msgs::String());
status.prediction_labels.back().data = "odom";
status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->odom_prediction_error().get().cast<double>())).transform);
Expand Down Expand Up @@ -546,7 +560,6 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
ros::ServiceClient set_global_map_service;
ros::ServiceClient query_global_localization_service;
};
}

} // namespace hdl_localization

PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet)
2 changes: 1 addition & 1 deletion include/hdl_localization/pose_estimator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ class PoseEstimator {
* @param cloud input cloud
* @return cloud aligned to the globalmap
*/
pcl::PointCloud<PointT>::Ptr correct(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud);
pcl::PointCloud<PointT>::Ptr correct(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud, double* fitness_score);

/* getters */
ros::Time last_correction_time() const;
Expand Down
1 change: 1 addition & 0 deletions launch/hdl_localization.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@
<param name="init_ori_x" value="0.0" />
<param name="init_ori_y" value="0.0" />
<param name="init_ori_z" value="0.0" />
<param name="odom_covariance_coefficient" value="1.0" />

<param name="use_global_localization" value="$(arg use_global_localization)" />
</node>
Expand Down
3 changes: 2 additions & 1 deletion src/hdl_localization/pose_estimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,7 @@ void PoseEstimator::predict_odom(const Eigen::Matrix4f& odom_delta) {
* @param cloud input cloud
* @return cloud aligned to the globalmap
*/
pcl::PointCloud<PoseEstimator::PointT>::Ptr PoseEstimator::correct(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {
pcl::PointCloud<PoseEstimator::PointT>::Ptr PoseEstimator::correct(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud, double* fitness_score) {
last_correction_stamp = stamp;

Eigen::Matrix4f no_guess = last_observation;
Expand Down Expand Up @@ -177,6 +177,7 @@ pcl::PointCloud<PoseEstimator::PointT>::Ptr PoseEstimator::correct(const ros::Ti
pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
registration->setInputSource(cloud);
registration->align(*aligned, init_guess);
*fitness_score = registration->getFitnessScore();

Eigen::Matrix4f trans = registration->getFinalTransformation();
Eigen::Vector3f p = trans.block<3, 1>(0, 3);
Expand Down

0 comments on commit 5c1361b

Please sign in to comment.