12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Check overlapping range data correctly (#804) 

FIXES=#771
master
gaschler 2018-04-05 19:26:06 +02:00 committed by Wally B. Feed
parent b3e3dfd7d4
commit 4e0fbcb38b
1 changed files with 33 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
cartographer_ros/cartographer_ros/rosbag_validate_main.cc
View File

@ -151,29 +151,35 @@ class RangeDataChecker {
const std::string& frame_id = message.header.frame_id; const std::string& frame_id = message.header.frame_id;
ros::Time current_time_stamp = message.header.stamp; ros::Time current_time_stamp = message.header.stamp;
RangeChecksum current_checksum; RangeChecksum current_checksum;
ros::Duration range_duration; cartographer::common::Time time_from, time_to;
ReadRangeMessage(message, &current_checksum, &range_duration); ReadRangeMessage(message, &current_checksum, &time_from, &time_to);
ros::Time current_time_first_point = current_time_stamp - range_duration; auto previous_time_to_it = frame_id_to_previous_time_to_.find(frame_id);
auto previous_time_stamp_it = if (previous_time_to_it != frame_id_to_previous_time_to_.end() &&
frame_id_to_previous_time_stamp_.find(frame_id); previous_time_to_it->second >= time_from) {
if (previous_time_stamp_it != frame_id_to_previous_time_stamp_.end() && if (previous_time_to_it->second >= time_to) {
previous_time_stamp_it->second >= current_time_first_point) { LOG_FIRST_N(ERROR, 3) << "Sensor with frame_id \"" << frame_id
<< "\" is not sequential in time."
<< "Previous range message ends at time "
<< previous_time_to_it->second
<< ", current one at time " << time_to;
} else {
LOG_FIRST_N(WARNING, 3) LOG_FIRST_N(WARNING, 3)
<< "Sensor with frame_id \"" << frame_id << "Sensor with frame_id \"" << frame_id
<< "\" measurements overlap in time " << "\" measurements overlap in time. "
<< "Previous range message, ending at time stamp " << "Previous range message, ending at time stamp "
<< previous_time_stamp_it->second << previous_time_to_it->second
<< ", must finish before current range message, " << ", must finish before current range message, "
<< "which ranges from " << current_time_first_point << " to " << "which ranges from " << time_from << " to " << time_to;
<< current_time_stamp; }
double overlap = double overlap = cartographer::common::ToSeconds(
(previous_time_stamp_it->second - current_time_first_point).toSec(); previous_time_to_it->second - time_from);
auto it = frame_id_to_max_overlap_duration_.find(frame_id); auto it = frame_id_to_max_overlap_duration_.find(frame_id);
if (it == frame_id_to_max_overlap_duration_.end() || if (it == frame_id_to_max_overlap_duration_.end() ||
overlap > frame_id_to_max_overlap_duration_.at(frame_id)) { overlap > frame_id_to_max_overlap_duration_.at(frame_id)) {
frame_id_to_max_overlap_duration_[frame_id] = overlap; frame_id_to_max_overlap_duration_[frame_id] = overlap;
} }
} }
frame_id_to_previous_time_to_[frame_id] = time_to;
if (current_checksum.first == 0) { if (current_checksum.first == 0) {
return; return;
} }
@ -190,7 +196,6 @@ class RangeDataChecker {
} }
} }
frame_id_to_range_checksum_[frame_id] = current_checksum; frame_id_to_range_checksum_[frame_id] = current_checksum;
frame_id_to_previous_time_stamp_[frame_id] = message.header.stamp;
} }
void PrintReport() { void PrintReport() {
@ -208,10 +213,13 @@ class RangeDataChecker {
template <typename MessageType> template <typename MessageType>
static void ReadRangeMessage(const MessageType& message, static void ReadRangeMessage(const MessageType& message,
RangeChecksum* range_checksum, RangeChecksum* range_checksum,
ros::Duration* range_duration) { cartographer::common::Time* from,
auto point_cloud_with_intensities = ToPointCloudWithIntensities(message); cartographer::common::Time* to) {
auto point_cloud_time = ToPointCloudWithIntensities(message);
const cartographer::sensor::TimedPointCloud& point_cloud = const cartographer::sensor::TimedPointCloud& point_cloud =
std::get<0>(point_cloud_with_intensities).points; std::get<0>(point_cloud_time).points;
*to = std::get<1>(point_cloud_time);
*from = *to + cartographer::common::FromSeconds(point_cloud[0][3]);
Eigen::Vector4f points_sum = Eigen::Vector4f::Zero(); Eigen::Vector4f points_sum = Eigen::Vector4f::Zero();
for (const Eigen::Vector4f& point : point_cloud) { for (const Eigen::Vector4f& point : point_cloud) {
points_sum += point; points_sum += point;
@ -226,18 +234,18 @@ class RangeDataChecker {
<< first_point_relative_time << " s, must negative or zero."; << first_point_relative_time << " s, must negative or zero.";
} }
if (last_point_relative_time != 0) { if (last_point_relative_time != 0) {
LOG_FIRST_N(ERROR, 1) LOG_FIRST_N(INFO, 1)
<< "Sensor with frame_id \"" << message.header.frame_id << "Sensor with frame_id \"" << message.header.frame_id
<< "\" has range data whose last point has relative time " << "\" has range data whose last point has relative time "
<< last_point_relative_time << " s, must be zero."; << last_point_relative_time << " s, should be zero.";
} }
} }
*range_checksum = {point_cloud.size(), points_sum}; *range_checksum = {point_cloud.size(), points_sum};
*range_duration = ros::Duration(-point_cloud[0][3]);
} }
std::map<std::string, RangeChecksum> frame_id_to_range_checksum_; std::map<std::string, RangeChecksum> frame_id_to_range_checksum_;
std::map<std::string, ros::Time> frame_id_to_previous_time_stamp_; std::map<std::string, cartographer::common::Time>
frame_id_to_previous_time_to_;
std::map<std::string, double> frame_id_to_max_overlap_duration_; std::map<std::string, double> frame_id_to_max_overlap_duration_;
}; };