point_cloud_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
19 
20 #include <cmath>
21 
22 #include "gtest/gtest.h"
23 
24 namespace cartographer {
25 namespace sensor {
26 namespace {
27 
28 TEST(PointCloudTest, TransformPointCloud) {
29  PointCloud point_cloud;
30  point_cloud.emplace_back(0.5f, 0.5f, 1.f);
31  point_cloud.emplace_back(3.5f, 0.5f, 42.f);
32  const PointCloud transformed_point_cloud = TransformPointCloud(
33  point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
34  EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6);
35  EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6);
36  EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6);
37  EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
38 }
39 
40 } // namespace
41 } // namespace sensor
42 } // namespace cartographer
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
static Rigid2 Rotation(const double rotation)
Rigid3< T > Embed3D(const Rigid2< T > &transform)
Definition: transform.h:110
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58