diff --git a/apps/globalmap_server_nodelet.cpp b/apps/globalmap_server_nodelet.cpp index bd2cc585..bf384c2a 100644 --- a/apps/globalmap_server_nodelet.cpp +++ b/apps/globalmap_server_nodelet.cpp @@ -28,7 +28,6 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZRGBI, namespace hdl_localization { - class GlobalmapServerNodelet : public nodelet::Nodelet { public: @@ -62,18 +61,18 @@ class GlobalmapServerNodelet : public nodelet::Nodelet { // read globalmap from a pcd file std::string globalmap_pcd = private_nh_.param("globalmap_pcd", ""); - globalmap.reset(new pcl::PointCloud()); - pcl::io::loadPCDFile(globalmap_pcd, *globalmap); - globalmap->header.frame_id = "map"; + globalmap_.reset(new pcl::PointCloud()); + pcl::io::loadPCDFile(globalmap_pcd, *globalmap_); + globalmap_->header.frame_id = "map"; std::ifstream utm_file(globalmap_pcd + ".utm"); - if (utm_file.is_open() && private_nh.param("convert_utm_to_local", true)) + if (utm_file.is_open() && private_nh_.param("convert_utm_to_local", true)) { double utm_easting; double utm_northing; double altitude; utm_file >> utm_easting >> utm_northing >> altitude; - for (auto& pt : globalmap->points) + for (auto& pt : globalmap_->points) { pt.getVector3fMap() -= Eigen::Vector3f(utm_easting, utm_northing, altitude); } @@ -85,38 +84,38 @@ class GlobalmapServerNodelet : public nodelet::Nodelet double downsample_resolution = private_nh_.param("downsample_resolution", 0.1); boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); - voxelgrid->setInputCloud(globalmap); + voxelgrid->setInputCloud(globalmap_); pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); voxelgrid->filter(*filtered); - globalmap = filtered; + globalmap_ = filtered; } - void pubOnceCb(const ros::WallTimerEvent& /*event*/) + void pubOnceCb(const ros::WallTimerEvent& event) { - globalmap_pub.publish(globalmap); + globalmap_pub_.publish(globalmap_); } void mapUpdateCallback(const std_msgs::String& msg) { ROS_INFO_STREAM("Received map request, map path : " << msg.data); std::string globalmap_pcd = msg.data; - globalmap.reset(new pcl::PointCloud()); - pcl::io::loadPCDFile(globalmap_pcd, *globalmap); - globalmap->header.frame_id = "map"; + globalmap_.reset(new pcl::PointCloud()); + pcl::io::loadPCDFile(globalmap_pcd, *globalmap_); + globalmap_->header.frame_id = "map"; // downsample globalmap double downsample_resolution = private_nh_.param("downsample_resolution", 0.1); boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); - voxelgrid->setInputCloud(globalmap); + voxelgrid->setInputCloud(globalmap_); pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); voxelgrid->filter(*filtered); - globalmap = filtered; - globalmap_pub.publish(globalmap); + globalmap_ = filtered; + globalmap_pub_.publish(globalmap_); } private: diff --git a/apps/hdl_localization_nodelet.cpp b/apps/hdl_localization_nodelet.cpp index ed0d75f2..780f1414 100644 --- a/apps/hdl_localization_nodelet.cpp +++ b/apps/hdl_localization_nodelet.cpp @@ -34,13 +34,12 @@ namespace hdl_localization { - class HdlLocalizationNodelet : public nodelet::Nodelet { public: using PointT = pcl::PointXYZI; - HdlLocalizationNodelet() : tf_buffer(), tf_listener(tf_buffer) + HdlLocalizationNodelet() : tf_buffer_(), tf_listener_(tf_buffer_) { } ~HdlLocalizationNodelet() override @@ -95,8 +94,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet query_global_localization_service_ = nh_.serviceClient("/hdl_" "global_" "localiz" - "at" - "ion/" + "ation/" "query"); relocalize_server_ = nh_.advertiseService("/relocalize", &HdlLocalizationNodelet::relocalize, this); @@ -190,27 +188,28 @@ class HdlLocalizationNodelet : public nodelet::Nodelet double downsample_resolution = private_nh_.param("downsample_resolution", 0.1); boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); - downsample_filter = voxelgrid; + downsample_filter_ = voxelgrid; NODELET_INFO("create registration method for localization"); - registration = create_registration(); + registration_ = createRegistration(); // global localization NODELET_INFO("create registration method for fallback during relocalization"); relocalizing_ = false; - delta_estimater.reset(new DeltaEstimater(create_registration())); + delta_estimater_.reset(new DeltaEstimater(createRegistration())); // initialize pose estimator if (private_nh_.param("specify_init_pose", true)) { NODELET_INFO("initialize pose estimator with specified parameters!!"); - pose_estimator.reset(new hdl_localization::PoseEstimator( - registration, - Eigen::Vector3f(private_nh.param("init_pos_x", 0.0), private_nh.param("init_pos_y", 0.0), - private_nh.param("init_pos_z", 0.0)), - Eigen::Quaternionf(private_nh.param("init_ori_w", 1.0), private_nh.param("init_ori_x", 0.0), - private_nh.param("init_ori_y", 0.0), private_nh.param("init_ori_z", 0.0)), - private_nh.param("cool_time_duration", 0.5))); + pose_estimator_.reset(new hdl_localization::PoseEstimator( + registration_, + Eigen::Vector3f(private_nh_.param("init_pos_x", 0.0), private_nh_.param("init_pos_y", 0.0), + private_nh_.param("init_pos_z", 0.0)), + Eigen::Quaternionf(private_nh_.param("init_ori_w", 1.0), private_nh_.param("init_ori_x", 0.0), + private_nh_.param("init_ori_y", 0.0), + private_nh_.param("init_ori_z", 0.0)), + private_nh_.param("cool_time_duration", 0.5))); } } @@ -222,7 +221,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet void imuCallback(const sensor_msgs::ImuConstPtr& imu_msg) { std::lock_guard lock(imu_data_mutex_); - imu_data.push_back(imu_msg); + imu_data_.push_back(imu_msg); } /** @@ -231,7 +230,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet */ void pointsCallback(const sensor_msgs::PointCloud2ConstPtr& points_msg) { - if (!globalmap) + if (!globalmap_) { NODELET_ERROR("globalmap has not been received!!"); return; @@ -247,7 +246,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet return; } - ros::Time last_correction_time = pose_estimator->last_correction_time(); + ros::Time last_correction_time = pose_estimator_->lastCorrectionTime(); // Skip calculation if timestamp is wrong if (stamp < last_correction_time) { @@ -256,10 +255,10 @@ class HdlLocalizationNodelet : public nodelet::Nodelet // transform pointcloud into odom_child_frame_id std::string tf_error; pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - if (this->tf_buffer.canTransform(odom_child_frame_id, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), - &tfError)) + if (this->tf_buffer_.canTransform(odom_child_frame_id_, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), + &tf_error)) { - if (!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) + 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; @@ -267,32 +266,32 @@ class HdlLocalizationNodelet : public nodelet::Nodelet } else { - NODELET_ERROR(tf_error.c_str()); + NODELET_ERROR_STREAM(tf_error.c_str()); return; } auto filtered = downsample(cloud); - last_scan = filtered; + last_scan_ = filtered; if (relocalizing_) { - delta_estimater->add_frame(filtered); + delta_estimater_->addFrame(filtered); } std::lock_guard estimator_lock(pose_estimator_mutex_); - if (!pose_estimator) + if (!pose_estimator_) { NODELET_ERROR("waiting for initial pose input!!"); return; } - Eigen::Matrix4f before = pose_estimator->matrix(); + Eigen::Matrix4f before = pose_estimator_->matrix(); if (use_imu_) { // PointClouds + IMU prediction std::lock_guard lock(imu_data_mutex_); - auto imu_iter = imu_data.begin(); - for (imu_iter; imu_iter != imu_data.end(); imu_iter++) + auto imu_iter = imu_data_.begin(); + for (imu_iter; imu_iter != imu_data_.end(); imu_iter++) { if (stamp < (*imu_iter)->header.stamp) { @@ -300,27 +299,27 @@ class HdlLocalizationNodelet : public nodelet::Nodelet } const auto& acc = (*imu_iter)->linear_acceleration; const auto& gyro = (*imu_iter)->angular_velocity; - double acc_sign = invert_acc ? -1.0 : 1.0; - double gyro_sign = invert_gyro ? -1.0 : 1.0; - pose_estimator->predict_imu((*imu_iter)->header.stamp, acc_sign * Eigen::Vector3f(acc.x, acc.y, acc.z), + double acc_sign = invert_acc_ ? -1.0 : 1.0; + double gyro_sign = invert_gyro_ ? -1.0 : 1.0; + pose_estimator_->predictImu((*imu_iter)->header.stamp, acc_sign * Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z)); } - imu_data.erase(imu_data.begin(), imu_iter); + imu_data_.erase(imu_data_.begin(), imu_iter); } else if (use_odom_) { // PointClouds + Oodometry prediction - if (tf_buffer.canTransform(odom_child_frame_id, odom_stamp_last, odom_child_frame_id, ros::Time(0), - robot_odom_frame_id, ros::Duration(0))) + if (tf_buffer_.canTransform(odom_child_frame_id_, odom_stamp_last_, odom_child_frame_id_, ros::Time(0), + robot_odom_frame_id_, ros::Duration(0))) { // Get the amount of odometry movement since the last calculation // Coordinate system where the front of the robot is x geometry_msgs::TransformStamped odom_delta = - tf_buffer.lookupTransform(odom_child_frame_id, odom_stamp_last, odom_child_frame_id, ros::Time(0), - robot_odom_frame_id, ros::Duration(0)); + tf_buffer_.lookupTransform(odom_child_frame_id_, odom_stamp_last_, odom_child_frame_id_, ros::Time(0), + robot_odom_frame_id_, ros::Duration(0)); // Get the latest odom_child_frame_id to get the time geometry_msgs::TransformStamped odom_now = - tf_buffer.lookupTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0)); + tf_buffer_.lookupTransform(robot_odom_frame_id_, odom_child_frame_id_, ros::Time(0)); ros::Time odom_stamp = odom_now.header.stamp; ros::Duration odom_time_diff = odom_stamp - odom_stamp_last_; double odom_time_diff_sec = odom_time_diff.toSec(); @@ -339,49 +338,49 @@ class HdlLocalizationNodelet : public nodelet::Nodelet tf::Matrix3x3(odom_travel_angular).getRPY(roll, pitch, yaw); Eigen::Vector3f odom_twist_angular(roll / odom_time_diff_sec, pitch / odom_time_diff_sec, yaw / odom_time_diff_sec); - pose_estimator->predict_odom(odom_stamp, odom_twist_linear, odom_twist_angular); + pose_estimator_->predictOdom(odom_stamp, odom_twist_linear, odom_twist_angular); } odom_stamp_last_ = odom_stamp; } else { - if (tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) + if (tf_buffer_.canTransform(robot_odom_frame_id_, odom_child_frame_id_, ros::Time(0))) { NODELET_WARN_STREAM("The last timestamp is wrong, skip localization"); // Get the latest odom_child_frame_id to get the time geometry_msgs::TransformStamped odom_now = - tf_buffer.lookupTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0)); - odom_stamp_last = odom_now.header.stamp; + tf_buffer_.lookupTransform(robot_odom_frame_id_, odom_child_frame_id_, ros::Time(0)); + odom_stamp_last_ = odom_now.header.stamp; } else { NODELET_WARN_STREAM("Failed to look up transform between " << cloud->header.frame_id << " and " - << robot_odom_frame_id); + << robot_odom_frame_id_); } } } else { // PointClouds-only prediction - pose_estimator->predict(stamp); + pose_estimator_->predict(stamp); } // Perform scan matching using the calculated position as the initial value double fitness_score; - auto aligned = pose_estimator->correct(stamp, filtered, fitness_score); + auto aligned = pose_estimator_->correct(stamp, filtered, fitness_score); if (aligned_pub_.getNumSubscribers()) { aligned->header.frame_id = "map"; aligned->header.stamp = cloud->header.stamp; - aligned_pub.publish(aligned); + aligned_pub_.publish(aligned); } if (status_pub_.getNumSubscribers()) { - publish_scan_matching_status(points_msg->header, aligned); + publishScanMatchingStatus(points_msg->header, aligned); } - publish_odometry(points_msg->header.stamp, pose_estimator->matrix(), fitness_score); + publishOdometry(points_msg->header.stamp, pose_estimator_->matrix(), fitness_score); } /** @@ -393,15 +392,15 @@ class HdlLocalizationNodelet : public nodelet::Nodelet NODELET_INFO("globalmap received!"); pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); pcl::fromROSMsg(*points_msg, *cloud); - globalmap = cloud; + globalmap_ = cloud; - registration->setInputTarget(globalmap); + registration_->setInputTarget(globalmap_); if (use_global_localization_) { NODELET_INFO("set globalmap for global localization!"); hdl_global_localization::SetGlobalMap srv; - pcl::toROSMsg(*globalmap, srv.request.global_map); + pcl::toROSMsg(*globalmap_, srv.request.global_map); if (!set_global_map_service_.call(srv)) { @@ -420,15 +419,15 @@ class HdlLocalizationNodelet : public nodelet::Nodelet */ 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; } relocalizing_ = true; - delta_estimater->reset(); - pcl::PointCloud::ConstPtr scan = last_scan; + delta_estimater_->reset(); + pcl::PointCloud::ConstPtr scan = last_scan_; hdl_global_localization::QueryGlobalLocalization srv; pcl::toROSMsg(*scan, srv.request.cloud); @@ -455,12 +454,12 @@ class HdlLocalizationNodelet : public nodelet::Nodelet Eigen::Quaternionf(result.orientation.w, result.orientation.x, result.orientation.y, result.orientation.z) .toRotationMatrix(); pose.translation() = Eigen::Vector3f(result.position.x, result.position.y, result.position.z); - pose = pose * delta_estimater->estimated_delta(); + pose = pose * delta_estimater_->estimatedDelta(); std::lock_guard lock(pose_estimator_mutex_); - pose_estimator.reset(new hdl_localization::PoseEstimator(registration, pose.translation(), - Eigen::Quaternionf(pose.linear()), - private_nh.param("cool_time_duration", 0.5))); + pose_estimator_.reset(new hdl_localization::PoseEstimator(registration_, pose.translation(), + Eigen::Quaternionf(pose.linear()), + private_nh_.param("cool_time_duration", 0.5))); relocalizing_ = false; @@ -477,9 +476,9 @@ class HdlLocalizationNodelet : public nodelet::Nodelet std::lock_guard 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, Eigen::Vector3f(p.x, p.y, p.z), - Eigen::Quaternionf(q.w, q.x, q.y, q.z), - private_nh.param("cool_time_duration", 0.5))); + pose_estimator_.reset(new hdl_localization::PoseEstimator(registration_, Eigen::Vector3f(p.x, p.y, p.z), + Eigen::Quaternionf(q.w, q.x, q.y, q.z), + private_nh_.param("cool_time_duration", 0.5))); } /** @@ -487,16 +486,16 @@ class HdlLocalizationNodelet : public nodelet::Nodelet * @param cloud input cloud * @return downsampled cloud */ - pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& /*cloud*/) const + pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { - if (!downsample_filter) + if (!downsample_filter_) { return cloud; } pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); - downsample_filter->setInputCloud(cloud); - downsample_filter->filter(*filtered); + downsample_filter_->setInputCloud(cloud); + downsample_filter_->filter(*filtered); filtered->header = cloud->header; return filtered; @@ -512,16 +511,16 @@ class HdlLocalizationNodelet : public nodelet::Nodelet // broadcast the transform over tf if (enable_tf_) { - if (tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) + 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())); map_wrt_frame.header.stamp = stamp; - map_wrt_frame.header.frame_id = odom_child_frame_id; + map_wrt_frame.header.frame_id = odom_child_frame_id_; map_wrt_frame.child_frame_id = "map"; geometry_msgs::TransformStamped frame_wrt_odom = - tf_buffer.lookupTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0), ros::Duration(0.1)); + tf_buffer_.lookupTransform(robot_odom_frame_id_, odom_child_frame_id_, ros::Time(0), ros::Duration(0.1)); Eigen::Matrix4f frame2odom = tf2::transformToEigen(frame_wrt_odom).cast().matrix(); geometry_msgs::TransformStamped map_wrt_odom; @@ -535,17 +534,17 @@ class HdlLocalizationNodelet : public nodelet::Nodelet odom_trans.transform = tf2::toMsg(odom_wrt_map); odom_trans.header.stamp = stamp; odom_trans.header.frame_id = "map"; - odom_trans.child_frame_id = robot_odom_frame_id; + odom_trans.child_frame_id = robot_odom_frame_id_; - tf_broadcaster.sendTransform(odom_trans); + tf_broadcaster_.sendTransform(odom_trans); } else { geometry_msgs::TransformStamped odom_trans = tf2::eigenToTransform(Eigen::Isometry3d(pose.cast())); odom_trans.header.stamp = stamp; odom_trans.header.frame_id = "map"; - odom_trans.child_frame_id = odom_child_frame_id; - tf_broadcaster.sendTransform(odom_trans); + odom_trans.child_frame_id = odom_child_frame_id_; + tf_broadcaster_.sendTransform(odom_trans); } } @@ -555,30 +554,30 @@ class HdlLocalizationNodelet : public nodelet::Nodelet odom.header.frame_id = "map"; tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast()), odom.pose.pose); - odom.pose.covariance[0] = private_nh.param("cov_scaling_factor_x", 1.0) * fitness_score; - odom.pose.covariance[7] = private_nh.param("cov_scaling_factor_y", 1.0) * fitness_score; - odom.pose.covariance[14] = private_nh.param("cov_scaling_factor_z", 1.0) * fitness_score; - odom.pose.covariance[21] = private_nh.param("cov_scaling_factor_R", 1.0) * fitness_score; - odom.pose.covariance[28] = private_nh.param("cov_scaling_factor_P", 1.0) * fitness_score; - odom.pose.covariance[35] = private_nh.param("cov_scaling_factor_Y", 1.0) * fitness_score; - - odom.child_frame_id = odom_child_frame_id; + odom.pose.covariance[0] = private_nh_.param("cov_scaling_factor_x", 1.0) * fitness_score; + odom.pose.covariance[7] = private_nh_.param("cov_scaling_factor_y", 1.0) * fitness_score; + odom.pose.covariance[14] = private_nh_.param("cov_scaling_factor_z", 1.0) * fitness_score; + odom.pose.covariance[21] = private_nh_.param("cov_scaling_factor_R", 1.0) * fitness_score; + odom.pose.covariance[28] = private_nh_.param("cov_scaling_factor_P", 1.0) * fitness_score; + odom.pose.covariance[35] = private_nh_.param("cov_scaling_factor_Y", 1.0) * fitness_score; + + 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; - pose_pub.publish(odom); + pose_pub_.publish(odom); } /** * @brief publish scan matching status information */ - void publishScanMatchingStatus(const std_msgs::Header& header, pcl::PointCloud::ConstPtr /*aligned*/) + void publishScanMatchingStatus(const std_msgs::Header& header, pcl::PointCloud::ConstPtr aligned) { ScanMatchingStatus status; status.header = header; - status.has_converged = registration->hasConverged(); + status.has_converged = registration_->hasConverged(); status.matching_error = 0.0; const double max_correspondence_dist = private_nh_.param("status_max_correspondence_dist", 0.5); @@ -597,7 +596,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet } num_valid_points++; - registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists); + registration_->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists); if (k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) { status.matching_error += k_sq_dists[0]; @@ -608,41 +607,41 @@ class HdlLocalizationNodelet : public nodelet::Nodelet status.matching_error /= num_inliers; status.inlier_fraction = static_cast(num_inliers) / std::max(1, num_valid_points); status.relative_pose = - tf2::eigenToTransform(Eigen::Isometry3d(registration->getFinalTransformation().cast())).transform; + tf2::eigenToTransform(Eigen::Isometry3d(registration_->getFinalTransformation().cast())).transform; status.prediction_labels.reserve(2); status.prediction_errors.reserve(2); std::vector errors(6, 0.0); - if (pose_estimator->wo_prediction_error()) + if (pose_estimator_->woPredictionError()) { 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())) + tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator_->woPredictionError().get().cast())) .transform); } - if (pose_estimator->imu_prediction_error()) + if (pose_estimator_->imuPredictionError()) { status.prediction_labels.push_back(std_msgs::String()); - status.prediction_labels.back().data = use_imu ? "imu" : "motion_model"; + 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())) + tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator_->imuPredictionError().get().cast())) .transform); } - if (pose_estimator->odom_prediction_error()) + if (pose_estimator_->odomPredictionError()) { 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())) + tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator_->odomPredictionError().get().cast())) .transform); } - status_pub.publish(status); + status_pub_.publish(status); } private: @@ -677,7 +676,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet std::mutex imu_data_mutex_; std::vector imu_data_; - // globalmap and registration method + // globalmap_ and registration method pcl::PointCloud::Ptr globalmap_; pcl::Filter::Ptr downsample_filter_; pcl::Registration::Ptr registration_; diff --git a/include/hdl_localization/delta_estimater.hpp b/include/hdl_localization/delta_estimater.hpp index 3301ba57..0203acfc 100644 --- a/include/hdl_localization/delta_estimater.hpp +++ b/include/hdl_localization/delta_estimater.hpp @@ -8,13 +8,12 @@ namespace hdl_localization { - class DeltaEstimater { public: using PointT = pcl::PointXYZI; - DeltaEstimater(pcl::Registration::Ptr /*reg*/) : delta(Eigen::Isometry3f::Identity()), reg(reg) + DeltaEstimater(pcl::Registration::Ptr reg) : delta_(Eigen::Isometry3f::Identity()), reg_(reg) { } ~DeltaEstimater() @@ -24,35 +23,35 @@ class DeltaEstimater void reset() { std::lock_guard lock(mutex_); - delta.setIdentity(); - last_frame.reset(); + delta_.setIdentity(); + last_frame_.reset(); } - void addFrame(pcl::PointCloud::ConstPtr /*frame*/) + void addFrame(pcl::PointCloud::ConstPtr frame) { - std::unique_lock lock(mutex); - if (last_frame == nullptr) + std::unique_lock lock(mutex_); + if (last_frame_ == nullptr) { - last_frame = frame; + last_frame_ = frame; return; } - reg->setInputTarget(last_frame); - reg->setInputSource(frame); + reg_->setInputTarget(last_frame_); + reg_->setInputSource(frame); lock.unlock(); pcl::PointCloud aligned; - reg->align(aligned); + reg_->align(aligned); lock.lock(); - last_frame = frame; - delta = delta * Eigen::Isometry3f(reg->getFinalTransformation()); + last_frame_ = frame; + delta_ = delta_ * Eigen::Isometry3f(reg_->getFinalTransformation()); } Eigen::Isometry3f estimatedDelta() const { std::lock_guard lock(mutex_); - return delta; + return delta_; } private: diff --git a/include/hdl_localization/pose_estimator.hpp b/include/hdl_localization/pose_estimator.hpp index 9c012ac7..395860ba 100644 --- a/include/hdl_localization/pose_estimator.hpp +++ b/include/hdl_localization/pose_estimator.hpp @@ -20,7 +20,6 @@ class UnscentedKalmanFilterX; namespace hdl_localization { - class PoseSystem; class OdomSystem; diff --git a/include/hdl_localization/pose_system.hpp b/include/hdl_localization/pose_system.hpp index d4e0d502..f6c66d34 100644 --- a/include/hdl_localization/pose_system.hpp +++ b/include/hdl_localization/pose_system.hpp @@ -28,7 +28,7 @@ class PoseSystem // system equation (without input) VectorXt f(const VectorXt& state) const { - VectorXt next_state(16); + VectorXt state_next(16); Vector3t pt = state.middleRows(0, 3); Vector3t vt = state.middleRows(3, 3); @@ -39,25 +39,23 @@ class PoseSystem Vector3t gyro_bias = state.middleRows(13, 3); // position - next_state.middleRows(0, 3) = pt + vt * dt_; // + state_next.middleRows(0, 3) = pt + vt * dt_; // // velocity - next_state.middleRows(3, 3) = vt; + state_next.middleRows(3, 3) = vt; // orientation - Quaterniont qt = qt; + state_next.middleRows(6, 4) << qt.w(), qt.x(), qt.y(), qt.z(); + state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration + state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity - next_state.middleRows(6, 4) << qt.w(), qt.x(), qt.y(), qt.z(); - next_state.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration - next_state.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity - - return next_state; + return state_next; } // system equation VectorXt fImu(const VectorXt& state, const Eigen::Vector3f& imu_acc, const Eigen::Vector3f& imu_gyro) const { - VectorXt next_state(16); + VectorXt state_next(16); Vector3t pt = state.middleRows(0, 3); Vector3t vt = state.middleRows(3, 3); @@ -71,58 +69,57 @@ class PoseSystem const Vector3t& raw_gyro = imu_gyro; // position - next_state.middleRows(0, 3) = pt + vt * dt_; // + state_next.middleRows(0, 3) = pt + vt * dt_; // // velocity Vector3t g(0.0f, 0.0f, 9.80665f); - Vector3t acc = raw_acc - acc_bias; - Vector3t acc = qt * acc; - next_state.middleRows(3, 3) = vt + (acc - g) * dt_; - // next_state.middleRows(3, 3) = vt; // + (acc - g) * dt; // acceleration didn't contribute to accuracy due to + Vector3t acc = qt * (raw_acc - acc_bias); + state_next.middleRows(3, 3) = vt + (acc - g) * dt_; + // state_next.middleRows(3, 3) = vt; // + (acc - g) * dt_; // acceleration didn't contribute to accuracy due to // large noise // orientation Vector3t gyro = raw_gyro - gyro_bias; - Quaterniont dq(1, gyro[0] * dt / 2, gyro[1] * dt / 2, gyro[2] * dt / 2); + Quaterniont dq(1, gyro[0] * dt_ / 2, gyro[1] * dt_ / 2, gyro[2] * dt_ / 2); dq.normalize(); - Quaterniont qt = (qt * dq).normalized(); - next_state.middleRows(6, 4) << qt.w(), qt.x(), qt.y(), qt.z(); + Quaterniont qt_next = (qt * dq).normalized(); + state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z(); - next_state.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration - next_state.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity + state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration + state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity - return next_state; + return state_next; } VectorXt fOdom(const VectorXt& state, const Eigen::Vector3f& odom_twist_lin, const Eigen::Vector3f& odom_twist_ang) const { - VectorXt next_state(16); + VectorXt state_next(16); Vector3t pt = state.middleRows(0, 3); Vector3t vt = state.middleRows(3, 3); Quaterniont qt(state[6], state[7], state[8], state[9]); qt.normalize(); - const Vector3t& raw_lin_vel = odom_twist_lin; - Vector3t raw_ang_vel = odom_twist_ang; + Vector3t lin_vel_raw = odom_twist_lin; + Vector3t ang_vel_raw = odom_twist_ang; // position - next_state.middleRows(0, 3) = pt + vt * dt_; + state_next.middleRows(0, 3) = pt + vt * dt_; // velocity - Vector3t vel = qt * raw_lin_vel; - next_state.middleRows(3, 3) = vel; + Vector3t vel = qt * lin_vel_raw; + state_next.middleRows(3, 3) = vel; // orientation - Quaterniont dq(1, raw_ang_vel[0] * dt / 2, raw_ang_vel[1] * dt / 2, raw_ang_vel[2] * dt / 2); + Quaterniont dq(1, ang_vel_raw[0] * dt_ / 2, ang_vel_raw[1] * dt_ / 2, ang_vel_raw[2] * dt_ / 2); dq.normalize(); - Quaterniont qt = (qt * dq).normalized(); - next_state.middleRows(6, 4) << qt.w(), qt.x(), qt.y(), qt.z(); + Quaterniont qt_next = (qt * dq).normalized(); + state_next.middleRows(6, 4) << qt_next.w(), qt_next.x(), qt_next.y(), qt_next.z(); - next_state.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration - next_state.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity + state_next.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration + state_next.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity - return next_state; + return state_next; } // observation equation diff --git a/include/kkl/alg/unscented_kalman_filter.hpp b/include/kkl/alg/unscented_kalman_filter.hpp index 47e65a89..9dfc7cd8 100644 --- a/include/kkl/alg/unscented_kalman_filter.hpp +++ b/include/kkl/alg/unscented_kalman_filter.hpp @@ -13,7 +13,6 @@ namespace kkl { namespace alg { - /** * @brief Unscented Kalman Filter class * @param T scaler type @@ -37,16 +36,11 @@ class UnscentedKalmanFilterX * @param mean initial mean * @param cov initial covariance */ - UnscentedKalmanFilterX(const System& system, int state_dim, int input_dim, int measurement_dim, - const MatrixXt& process_noise, const MatrixXt& measurement_noise, const VectorXt& mean, - const MatrixXt& cov) + UnscentedKalmanFilterX(const System& system, int state_dim, int measurement_dim, const MatrixXt& process_noise, + const MatrixXt& measurement_noise, const VectorXt& mean, const MatrixXt& cov) : state_dim_(state_dim) - , input_dim_(input_dim) , measurement_dim_(measurement_dim) - , N_(state_dim) - , M_(input_dim) - , K_(measurement_dim) - , S_(2 * state_dim + 1) + , sigma_points_samples_(2 * state_dim + 1) , mean_(mean) , cov_(cov) , system_(system) @@ -55,27 +49,25 @@ class UnscentedKalmanFilterX , lambda_(1) , normal_dist_(0.0, 1.0) { - weights_.resize(S_, 1); - sigma_points_.resize(S_, N_); - ext_weights_.resize(2 * (N_ + K_) + 1, 1); - ext_sigma_points_.resize(2 * (N_ + K_) + 1, N_ + K_); - expected_measurements_.resize(2 * (N_ + K_) + 1, K_); + weights_.resize(sigma_points_samples_, 1); + sigma_points_.resize(sigma_points_samples_, state_dim_); + ext_weights_.resize(2 * (state_dim_ + measurement_dim_) + 1, 1); + ext_sigma_points_.resize(2 * (state_dim_ + measurement_dim_) + 1, state_dim_ + measurement_dim_); + expected_measurements_.resize(2 * (state_dim_ + measurement_dim_) + 1, measurement_dim_); // initialize weights for unscented filter - weights[0] = lambda / (N + lambda); - for (int i = 1; i < 2 * N_ + 1; i++) + weights_[0] = lambda_ / (state_dim_ + lambda_); + for (int i = 1; i < 2 * state_dim_ + 1; i++) { - weights[i] = 1 / (2 * (N + lambda)); + weights_[i] = 1 / (2 * (state_dim_ + lambda_)); } - - // weights for extended state space which includes error variances - ext_weights[0] = lambda / (N + K + lambda); - for (int i = 1; i < 2 * (N_ + K_) + 1; i++) + // weights_ for extended state space which includes error variances + ext_weights_[0] = lambda_ / (state_dim_ + measurement_dim_ + lambda_); + for (int i = 1; i < 2 * (state_dim_ + measurement_dim_) + 1; i++) { - ext_weights[i] = 1 / (2 * (N + K + lambda)); + ext_weights_[i] = 1 / (2 * (state_dim_ + measurement_dim_ + lambda_)); } } - /** * @brief predict * @note state = [px, py, pz, vx, vy, vz, qw, qx, qy, qz, 0, 0, 0, 0, 0, 0] @@ -85,7 +77,7 @@ class UnscentedKalmanFilterX // calculate sigma points ensurePositiveFinite(cov_); computeSigmaPoints(mean_, cov_, sigma_points_); - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { sigma_points_.row(i) = system_.f(sigma_points_.row(i)); } @@ -98,17 +90,16 @@ class UnscentedKalmanFilterX mean_pred.setZero(); cov_pred.setZero(); - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { - mean_pred += weights[i] * sigma_points.row(i); + mean_pred += weights_[i] * sigma_points_.row(i); } - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { VectorXt diff = sigma_points_.row(i).transpose() - mean_pred; - cov_pred += weights[i] * diff * diff.transpose(); + cov_pred += weights_[i] * diff * diff.transpose(); } cov_pred += r; - mean_ = mean_pred; cov_ = cov_pred; } @@ -125,9 +116,9 @@ class UnscentedKalmanFilterX // calculate sigma points ensurePositiveFinite(cov_); computeSigmaPoints(mean_, cov_, sigma_points_); - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { - sigma_points_.row(i) = system_.f_imu(sigma_points_.row(i), imu_acc, imu_gyro); + sigma_points_.row(i) = system_.fImu(sigma_points_.row(i), imu_acc, imu_gyro); } const auto& r = process_noise_; @@ -138,17 +129,16 @@ class UnscentedKalmanFilterX mean_pred.setZero(); cov_pred.setZero(); - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { - mean_pred += weights[i] * sigma_points.row(i); + mean_pred += weights_[i] * sigma_points_.row(i); } - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { VectorXt diff = sigma_points_.row(i).transpose() - mean_pred; - cov_pred += weights[i] * diff * diff.transpose(); + cov_pred += weights_[i] * diff * diff.transpose(); } cov_pred += r; - mean_ = mean_pred; cov_ = cov_pred; } @@ -159,14 +149,14 @@ class UnscentedKalmanFilterX * @param odom_twist_angular angular velocity * @note state = [px, py, pz, vx, vy, vz, qw, qx, qy, qz, 0, 0, 0, 0, 0, 0] */ - void predictOdom(Eigen::Vector3f /*odom_twist_linear*/, Eigen::Vector3f /*odom_twist_angular*/) + void predictOdom(const Eigen::Vector3f& odom_twist_linear, const Eigen::Vector3f& odom_twist_angular) { // calculate sigma points ensurePositiveFinite(cov_); computeSigmaPoints(mean_, cov_, sigma_points_); - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { - sigma_points.row(i) = system.f_odom(sigma_points.row(i), odom_twist_linear, odom_twist_angular); + sigma_points_.row(i) = system_.fOdom(sigma_points_.row(i), odom_twist_linear, odom_twist_angular); } const auto& r = process_noise_; // unscented transform @@ -174,14 +164,14 @@ class UnscentedKalmanFilterX MatrixXt cov_pred(cov_.rows(), cov_.cols()); mean_pred.setZero(); cov_pred.setZero(); - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { - mean_pred += weights[i] * sigma_points.row(i); + mean_pred += weights_[i] * sigma_points_.row(i); } - for (int i = 0; i < S_; i++) + for (int i = 0; i < sigma_points_samples_; i++) { VectorXt diff = sigma_points_.row(i).transpose() - mean_pred; - cov_pred += weights[i] * diff * diff.transpose(); + cov_pred += weights_[i] * diff * diff.transpose(); } cov_pred += r; mean_ = mean_pred; @@ -195,11 +185,11 @@ class UnscentedKalmanFilterX void correct(const VectorXt& measurement) { // create extended state space which includes error variances - VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1); - MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K); - ext_mean_pred.topLeftCorner(N_, 1) = VectorXt(mean_); - ext_cov_pred.topLeftCorner(N_, N_) = MatrixXt(cov_); - ext_cov_pred.bottomRightCorner(K_, K_) = measurement_noise_; + VectorXt ext_mean_pred = VectorXt::Zero(state_dim_ + measurement_dim_, 1); + MatrixXt ext_cov_pred = MatrixXt::Zero(state_dim_ + measurement_dim_, state_dim_ + measurement_dim_); + ext_mean_pred.topLeftCorner(state_dim_, 1) = VectorXt(mean_); + ext_cov_pred.topLeftCorner(state_dim_, state_dim_) = MatrixXt(cov_); + ext_cov_pred.bottomRightCorner(measurement_dim_, measurement_dim_) = measurement_noise_; ensurePositiveFinite(ext_cov_pred); computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points_); @@ -208,29 +198,29 @@ class UnscentedKalmanFilterX expected_measurements_.setZero(); for (int i = 0; i < ext_sigma_points_.rows(); i++) { - expected_measurements_.row(i) = system_.h(ext_sigma_points_.row(i).transpose().topLeftCorner(N_, 1)); - expected_measurements_.row(i) += VectorXt(ext_sigma_points_.row(i).transpose().bottomRightCorner(K_, 1)); + expected_measurements_.row(i) = system_.h(ext_sigma_points_.row(i).transpose().topLeftCorner(state_dim_, 1)); + expected_measurements_.row(i) += + VectorXt(ext_sigma_points_.row(i).transpose().bottomRightCorner(measurement_dim_, 1)); } - - VectorXt expected_measurement_mean = VectorXt::Zero(K); + VectorXt expected_measurement_mean = VectorXt::Zero(measurement_dim_); for (int i = 0; i < ext_sigma_points_.rows(); i++) { - expected_measurement_mean += ext_weights[i] * expected_measurements.row(i); + expected_measurement_mean += ext_weights_[i] * expected_measurements_.row(i); } - MatrixXt expected_measurement_cov = MatrixXt::Zero(K, K); + MatrixXt expected_measurement_cov = MatrixXt::Zero(measurement_dim_, measurement_dim_); for (int i = 0; i < ext_sigma_points_.rows(); i++) { VectorXt diff = expected_measurements_.row(i).transpose() - expected_measurement_mean; - expected_measurement_cov += ext_weights[i] * diff * diff.transpose(); + expected_measurement_cov += ext_weights_[i] * diff * diff.transpose(); } - // calculated transformed covariance - MatrixXt sigma = MatrixXt::Zero(N + K, K); + // calculated transformed cov_ariance + MatrixXt sigma = MatrixXt::Zero(state_dim_ + measurement_dim_, measurement_dim_); for (int i = 0; i < ext_sigma_points_.rows(); i++) { auto diff_a = (ext_sigma_points_.row(i).transpose() - ext_mean_pred); auto diff_b = (expected_measurements_.row(i).transpose() - expected_measurement_mean); - sigma += ext_weights[i] * (diffA * diffB.transpose()); + sigma += ext_weights_[i] * (diff_a * diff_b.transpose()); } MatrixXt expexted_inv = expected_measurement_cov.completeOrthogonalDecomposition().pseudoInverse(); @@ -240,8 +230,8 @@ class UnscentedKalmanFilterX VectorXt ext_mean = ext_mean_pred + k * (measurement - expected_measurement_mean); MatrixXt ext_cov = ext_cov_pred - k * expected_measurement_cov * k.transpose(); - mean_ = ext_mean.topLeftCorner(N_, 1); - cov_ = ext_cov.topLeftCorner(N_, N_); + mean_ = ext_mean.topLeftCorner(state_dim_, 1); + cov_ = ext_cov.topLeftCorner(state_dim_, state_dim_); } /* getter */ @@ -306,21 +296,16 @@ class UnscentedKalmanFilterX EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: const int state_dim_; - const int input_dim_; const int measurement_dim_; - - const int N_; - const int M_; - const int K_; - const int S_; + const int sigma_points_samples_; public: VectorXt mean_; MatrixXt cov_; System system_; - MatrixXt process_noise_; // - MatrixXt measurement_noise_; // + MatrixXt process_noise_; + MatrixXt measurement_noise_; T lambda_; VectorXt weights_; @@ -344,7 +329,7 @@ class UnscentedKalmanFilterX assert(cov.rows() == n && cov.cols() == n); Eigen::LLT llt; - llt.compute((n + lambda) * cov); + llt.compute((n + lambda_) * cov); MatrixXt l = llt.matrixL(); sigma_points.row(0) = mean; diff --git a/src/hdl_localization/pose_estimator.cpp b/src/hdl_localization/pose_estimator.cpp index 46d3cbe8..eb64eb92 100644 --- a/src/hdl_localization/pose_estimator.cpp +++ b/src/hdl_localization/pose_estimator.cpp @@ -6,7 +6,6 @@ namespace hdl_localization { - /** * @brief constructor * @param registration registration method @@ -14,20 +13,20 @@ namespace hdl_localization * @param quat initial orientation * @param cool_time_duration during "cool time", prediction is not performed */ -PoseEstimator::PoseEstimator(pcl::Registration::Ptr& /*registration*/, const Eigen::Vector3f& pos, +PoseEstimator::PoseEstimator(pcl::Registration::Ptr& registration, const Eigen::Vector3f& pos, const Eigen::Quaternionf& quat, double cool_time_duration) - : registration(registration), cool_time_duration_(cool_time_duration) + : registration_(registration), cool_time_duration_(cool_time_duration) { - last_observation = Eigen::Matrix4f::Identity(); - last_observation.block<3, 3>(0, 0) = quat.toRotationMatrix(); - last_observation.block<3, 1>(0, 3) = pos; + last_observation_ = Eigen::Matrix4f::Identity(); + last_observation_.block<3, 3>(0, 0) = quat.toRotationMatrix(); + last_observation_.block<3, 1>(0, 3) = pos; - process_noise = Eigen::MatrixXf::Identity(16, 16); - process_noise.middleRows(0, 3) *= 1.0; - process_noise.middleRows(3, 3) *= 1.0; - process_noise.middleRows(6, 4) *= 0.5; - process_noise.middleRows(10, 3) *= 1e-6; - process_noise.middleRows(13, 3) *= 1e-6; + process_noise_ = Eigen::MatrixXf::Identity(16, 16); + process_noise_.middleRows(0, 3) *= 1.0; + process_noise_.middleRows(3, 3) *= 1.0; + process_noise_.middleRows(6, 4) *= 0.5; + process_noise_.middleRows(10, 3) *= 1e-6; + process_noise_.middleRows(13, 3) *= 1e-6; Eigen::MatrixXf measurement_noise = Eigen::MatrixXf::Identity(7, 7); measurement_noise.middleRows(0, 3) *= 0.01; @@ -43,12 +42,12 @@ PoseEstimator::PoseEstimator(pcl::Registration::Ptr& /*registrat Eigen::MatrixXf cov = Eigen::MatrixXf::Identity(16, 16) * 0.01; PoseSystem system; - ukf.reset(new kkl::alg::UnscentedKalmanFilterX(system, 16, 6, 7, process_noise, measurement_noise, - mean, cov)); + ukf_.reset(new kkl::alg::UnscentedKalmanFilterX(system, 16, 7, process_noise_, measurement_noise, + mean, cov)); // TODO: Change odom covariance constants to ROS params // or subscribe an odometry topic and use it's covariance - odom_process_noise = Eigen::MatrixXf::Identity(16, 16) * 1e-5; - odom_process_noise.middleRows(6, 4) *= 1e-2; + odom_process_noise_ = Eigen::MatrixXf::Identity(16, 16) * 1e-5; + odom_process_noise_.middleRows(6, 4) *= 1e-2; } PoseEstimator::~PoseEstimator() @@ -77,10 +76,10 @@ void PoseEstimator::predict(const ros::Time& stamp) double dt = (stamp - prev_stamp_).toSec(); prev_stamp_ = stamp; - ukf->setProcessNoiseCov(process_noise * dt); - ukf->system.dt = dt; + ukf_->setProcessNoiseCov(process_noise_ * dt); + ukf_->system_.dt_ = dt; - ukf->predict(); + ukf_->predict(); } /** @@ -105,9 +104,9 @@ void PoseEstimator::predictImu(const ros::Time& stamp, const Eigen::Vector3f& im double dt = (stamp - prev_stamp_).toSec(); prev_stamp_ = stamp; - ukf->setProcessNoiseCov(process_noise * dt); - ukf->system.dt = dt; - ukf->predict_imu(imu_acc, imu_gyro); + ukf_->setProcessNoiseCov(process_noise_ * dt); + ukf_->system_.dt_ = dt; + ukf_->predictImu(imu_acc, imu_gyro); } /** @@ -128,10 +127,10 @@ void PoseEstimator::predictOdom(const ros::Time& stamp, const Eigen::Vector3f& o double dt = (stamp - prev_stamp_).toSec(); prev_stamp_ = stamp; - ukf->setProcessNoiseCov(odom_process_noise * dt); - ukf->system.dt = dt; + ukf_->setProcessNoiseCov(odom_process_noise_ * dt); + ukf_->system_.dt_ = dt; - ukf->predict_odom(odom_twist_linear, odom_twist_angular); + ukf_->predictOdom(odom_twist_linear, odom_twist_angular); } /** @@ -140,7 +139,7 @@ void PoseEstimator::predictOdom(const ros::Time& stamp, const Eigen::Vector3f& o * @return cloud aligned to the globalmap */ pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Time& stamp, - const pcl::PointCloud::ConstPtr& /*cloud*/, + const pcl::PointCloud::ConstPtr& cloud, double& fitness_score) { if (init_stamp_.is_zero()) @@ -150,15 +149,15 @@ pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Ti last_correction_stamp_ = stamp; - Eigen::Matrix4f no_guess = last_observation; + Eigen::Matrix4f no_guess = last_observation_; Eigen::Matrix4f init_guess = matrix(); pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); - registration->setInputSource(cloud); - registration->align(*aligned, init_guess); - fitness_score = registration->getFitnessScore(); + registration_->setInputSource(cloud); + registration_->align(*aligned, init_guess); + fitness_score = registration_->getFitnessScore(); - Eigen::Matrix4f trans = registration->getFinalTransformation(); + Eigen::Matrix4f trans = registration_->getFinalTransformation(); Eigen::Vector3f p = trans.block<3, 1>(0, 3); Eigen::Quaternionf q(trans.block<3, 3>(0, 0)); @@ -170,12 +169,12 @@ pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Ti Eigen::VectorXf observation(7); observation.middleRows(0, 3) = p; observation.middleRows(3, 4) = Eigen::Vector4f(q.w(), q.x(), q.y(), q.z()); - last_observation = trans; + last_observation_ = trans; - wo_pred_error = no_guess.inverse() * registration->getFinalTransformation(); - ukf->correct(observation); - imu_pred_error = init_guess.inverse() * registration->getFinalTransformation(); - odom_pred_error = imu_pred_error; + wo_pred_error_ = no_guess.inverse() * registration_->getFinalTransformation(); + ukf_->correct(observation); + imu_pred_error_ = init_guess.inverse() * registration_->getFinalTransformation(); + odom_pred_error_ = imu_pred_error_; return aligned; } @@ -188,17 +187,17 @@ ros::Time PoseEstimator::lastCorrectionTime() const Eigen::Vector3f PoseEstimator::pos() const { - return Eigen::Vector3f(ukf->mean[0], ukf->mean[1], ukf->mean[2]); + return Eigen::Vector3f(ukf_->mean_[0], ukf_->mean_[1], ukf_->mean_[2]); } Eigen::Vector3f PoseEstimator::vel() const { - return Eigen::Vector3f(ukf->mean[3], ukf->mean[4], ukf->mean[5]); + return Eigen::Vector3f(ukf_->mean_[3], ukf_->mean_[4], ukf_->mean_[5]); } Eigen::Quaternionf PoseEstimator::quat() const { - return Eigen::Quaternionf(ukf->mean[6], ukf->mean[7], ukf->mean[8], ukf->mean[9]).normalized(); + return Eigen::Quaternionf(ukf_->mean_[6], ukf_->mean_[7], ukf_->mean_[8], ukf_->mean_[9]).normalized(); } Eigen::Matrix4f PoseEstimator::matrix() const @@ -211,16 +210,16 @@ Eigen::Matrix4f PoseEstimator::matrix() const const boost::optional& PoseEstimator::woPredictionError() const { - return wo_pred_error; + return wo_pred_error_; } const boost::optional& PoseEstimator::imuPredictionError() const { - return imu_pred_error; + return imu_pred_error_; } const boost::optional& PoseEstimator::odomPredictionError() const { - return odom_pred_error; + return odom_pred_error_; } } // namespace hdl_localization