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

Detect duplicate range data. (#619) 

Checks that range data in a bag file changes between frames, which is one of the common mistakes listed in #529.
master
gaschler 2017-12-06 15:58:57 +01:00 committed by GitHub
parent 465e0434a1
commit 7bcdda4d37
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 45 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
cartographer_ros/cartographer_ros/rosbag_validate_main.cc
View File

@ -144,6 +144,47 @@ bool IsPointDataType(const std::string& data_type) {
return (kPointDataTypes.count(data_type) != 0);
}
class RangeDataChecker {
public:
template <typename MessageType>
void CheckMessage(const MessageType& message) {
const std::string& frame_id = message.header.frame_id;
RangeChecksum current_checksum = ComputeRangeChecksum(message);
if (current_checksum.first == 0) {
return;
}
auto it = frame_id_to_range_checksum_.find(frame_id);
if (it != frame_id_to_range_checksum_.end()) {
RangeChecksum previous_checksum = it->second;
if (previous_checksum == current_checksum) {
LOG_FIRST_N(ERROR, 3)
<< "Sensor with frame_id \"" << frame_id
<< "\" sends exactly the same range measurements multiple times. "
<< "Range data at time " << message.header.stamp
<< " equals preceding data.";
}
}
frame_id_to_range_checksum_[frame_id] = current_checksum;
}
private:
typedef std::pair<size_t /* num_points */, Eigen::Vector4f /* points_sum */>
RangeChecksum;
template <typename MessageType>
RangeChecksum ComputeRangeChecksum(const MessageType& message) {
const cartographer::sensor::TimedPointCloud& point_cloud =
ToPointCloudWithIntensities(message).points;
Eigen::Vector4f points_sum = Eigen::Vector4f::Zero();
for (const auto& point : point_cloud) {
points_sum += point;
}
return {point_cloud.size(), points_sum};
}
std::map<std::string, RangeChecksum> frame_id_to_range_checksum_;
};
void Run(const std::string& bag_filename, const bool dump_timing) {
rosbag::Bag bag;
bag.open(bag_filename, rosbag::bagmode::Read);
@ -153,6 +194,7 @@ void Run(const std::string& bag_filename, const bool dump_timing) {
size_t message_index = 0;
int num_imu_messages = 0;
double sum_imu_acceleration = 0.;
RangeDataChecker range_data_checker;
for (const rosbag::MessageInstance& message : view) {
++message_index;
std::string frame_id;
@ -161,14 +203,17 @@ void Run(const std::string& bag_filename, const bool dump_timing) {
auto msg = message.instantiate<sensor_msgs::PointCloud2>();
time = msg->header.stamp;
frame_id = msg->header.frame_id;
range_data_checker.CheckMessage(*msg);
} else if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
auto msg = message.instantiate<sensor_msgs::MultiEchoLaserScan>();
time = msg->header.stamp;
frame_id = msg->header.frame_id;
range_data_checker.CheckMessage(*msg);
} else if (message.isType<sensor_msgs::LaserScan>()) {
auto msg = message.instantiate<sensor_msgs::LaserScan>();
time = msg->header.stamp;
frame_id = msg->header.frame_id;
range_data_checker.CheckMessage(*msg);
} else if (message.isType<sensor_msgs::Imu>()) {
auto msg = message.instantiate<sensor_msgs::Imu>();
time = msg->header.stamp;