cartographer: point_cloud_test.cc Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/sensor/point_cloud.h"
00018 
00019 #include <cmath>
00020 
00021 #include "cartographer/transform/transform.h"
00022 #include "gtest/gtest.h"
00023 
00024 namespace cartographer {
00025 namespace sensor {
00026 namespace {
00027 
00028 TEST(PointCloudTest, TransformPointCloud) {
00029   PointCloud point_cloud;
00030   point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}});
00031   point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}});
00032   const PointCloud transformed_point_cloud = TransformPointCloud(
00033       point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
00034   EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6);
00035   EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6);
00036   EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6);
00037   EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6);
00038 }
00039 
00040 TEST(PointCloudTest, TransformTimedPointCloud) {
00041   TimedPointCloud point_cloud;
00042   point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}, 0.f});
00043   point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}, 0.f});
00044   const TimedPointCloud transformed_point_cloud = TransformTimedPointCloud(
00045       point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
00046   EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6);
00047   EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6);
00048   EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6);
00049   EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6);
00050 }
00051 
00052 }  // namespace
00053 }  // namespace sensor
00054 }  // namespace cartographer