diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index aba52a4..6058ffc 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -50,25 +50,6 @@ message Matrix { repeated double data = 3; } -// Modeled after ROS's MultiEchoLaserScan message. -// http://docs.ros.org/api/sensor_msgs/html/msg/MultiEchoLaserScan.html -message LaserScan { - message Values { - repeated float value = 1 [packed = true]; - } - - optional float angle_min = 2; - optional float angle_max = 3; - optional float angle_increment = 4; - optional float time_increment = 5; - optional float scan_time = 6; - optional float range_min = 7; - optional float range_max = 8; - - repeated Values range = 9; - repeated Values intensity = 10; -} - // Proto representation of ::cartographer::sensor::RangeData message RangeData { optional transform.proto.Vector3f origin = 1; diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 8b05c05..c9299ad 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -22,41 +22,6 @@ namespace cartographer { namespace sensor { -PointCloud ToPointCloud(const proto::LaserScan& proto) { - return ToPointCloudWithIntensities(proto).points; -} - -PointCloudWithIntensities ToPointCloudWithIntensities( - const proto::LaserScan& proto) { - CHECK_GE(proto.range_min(), 0.f); - CHECK_GE(proto.range_max(), proto.range_min()); - if (proto.angle_increment() > 0.f) { - CHECK_GT(proto.angle_max(), proto.angle_min()); - } else { - CHECK_GT(proto.angle_min(), proto.angle_max()); - } - PointCloudWithIntensities point_cloud; - float angle = proto.angle_min(); - for (int i = 0; i < proto.range_size(); ++i) { - const auto& range = proto.range(i); - if (range.value_size() > 0) { - const float first_echo = range.value(0); - if (proto.range_min() <= first_echo && first_echo <= proto.range_max()) { - const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ()); - point_cloud.points.push_back(rotation * - (first_echo * Eigen::Vector3f::UnitX())); - if (proto.intensity_size() > 0) { - point_cloud.intensities.push_back(proto.intensity(i).value(0)); - } else { - point_cloud.intensities.push_back(0.f); - } - } - } - angle += proto.angle_increment(); - } - return point_cloud; -} - proto::RangeData ToProto(const RangeData& range_data) { proto::RangeData proto; *proto.mutable_origin() = transform::ToProto(range_data.origin); diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index 2cb01a2..bb73f23 100644 --- a/cartographer/sensor/range_data.h +++ b/cartographer/sensor/range_data.h @@ -25,7 +25,7 @@ namespace cartographer { namespace sensor { -// Rays begin at 'origin'. 'returns' are the points where laser returns were +// Rays begin at 'origin'. 'returns' are the points where obstructions were // detected. 'misses' are points in the direction of rays for which no return // was detected, and were inserted at a configured distance. It is assumed that // between the 'origin' and 'misses' is free space. @@ -35,17 +35,6 @@ struct RangeData { PointCloud misses; }; -// Builds a PointCloud of returns from 'proto', dropping any beams with ranges -// outside the valid range described by 'proto'. -PointCloud ToPointCloud(const proto::LaserScan& proto); - -// Like above, but also extracts intensities of ouf the laser scan. The -// intensities of the laser are device specific and therefore require -// normalization to be comparable. In case the 'proto' does not contain -// intensities, this will return all 0. for the intensities. -PointCloudWithIntensities ToPointCloudWithIntensities( - const proto::LaserScan& proto); - // Converts 'range_data' to a proto::RangeData. proto::RangeData ToProto(const RangeData& range_data); diff --git a/cartographer/sensor/range_data_test.cc b/cartographer/sensor/range_data_test.cc index 72dca81..1a29048 100644 --- a/cartographer/sensor/range_data_test.cc +++ b/cartographer/sensor/range_data_test.cc @@ -28,52 +28,6 @@ namespace { using ::testing::Contains; using ::testing::PrintToString; -TEST(LaserTest, ToPointCloud) { - proto::LaserScan laser_scan; - for (int i = 0; i < 8; ++i) { - laser_scan.add_range()->add_value(1.f); - } - laser_scan.set_angle_min(0.f); - laser_scan.set_angle_max(8.f * static_cast(M_PI_4)); - laser_scan.set_angle_increment(static_cast(M_PI_4)); - laser_scan.set_range_min(0.f); - laser_scan.set_range_max(10.f); - - const auto point_cloud = ToPointCloud(laser_scan); - EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[1].isApprox( - Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[3].isApprox( - Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[5].isApprox( - Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), - 1e-6)); - EXPECT_TRUE(point_cloud[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[7].isApprox( - Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6)); -} - -TEST(LaserTest, ToPointCloudWithInfinityAndNaN) { - proto::LaserScan laser_scan; - laser_scan.add_range()->add_value(1.f); - laser_scan.add_range()->add_value(std::numeric_limits::infinity()); - laser_scan.add_range()->add_value(2.f); - laser_scan.add_range()->add_value(std::numeric_limits::quiet_NaN()); - laser_scan.add_range()->add_value(3.f); - laser_scan.set_angle_min(0.f); - laser_scan.set_angle_max(3.f * static_cast(M_PI_4)); - laser_scan.set_angle_increment(static_cast(M_PI_4)); - laser_scan.set_range_min(2.f); - laser_scan.set_range_max(10.f); - - const auto point_cloud = ToPointCloud(laser_scan); - ASSERT_EQ(2, point_cloud.size()); - EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6)); - EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6)); -} - // Custom matcher for Eigen::Vector3f entries. MATCHER_P(ApproximatelyEquals, expected, string("is equal to ") + PrintToString(expected)) {