rigid_transform.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 
18 
19 #include <vector>
20 
21 #include "Eigen/Core"
22 #include "Eigen/Geometry"
24 #include "glog/logging.h"
25 
26 namespace cartographer {
27 namespace transform {
28 
29 namespace {
30 
31 Eigen::Vector3d TranslationFromDictionary(
32  common::LuaParameterDictionary* dictionary) {
33  const std::vector<double> translation = dictionary->GetArrayValuesAsDoubles();
34  CHECK_EQ(3, translation.size()) << "Need (x, y, z) for translation.";
35  return Eigen::Vector3d(translation[0], translation[1], translation[2]);
36 }
37 
38 } // namespace
39 
40 Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch,
41  const double yaw) {
42  const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
43  const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
44  const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
45  return yaw_angle * pitch_angle * roll_angle;
46 }
47 
49  const Eigen::Vector3d translation =
50  TranslationFromDictionary(dictionary->GetDictionary("translation").get());
51 
52  auto rotation_dictionary = dictionary->GetDictionary("rotation");
53  if (rotation_dictionary->HasKey("w")) {
54  const Eigen::Quaterniond rotation(rotation_dictionary->GetDouble("w"),
55  rotation_dictionary->GetDouble("x"),
56  rotation_dictionary->GetDouble("y"),
57  rotation_dictionary->GetDouble("z"));
58  CHECK_NEAR(rotation.norm(), 1., 1e-9);
59  return transform::Rigid3d(translation, rotation);
60  } else {
61  const std::vector<double> rotation =
62  rotation_dictionary->GetArrayValuesAsDoubles();
63  CHECK_EQ(3, rotation.size()) << "Need (roll, pitch, yaw) for rotation.";
64  return transform::Rigid3d(
65  translation, RollPitchYaw(rotation[0], rotation[1], rotation[2]));
66  }
67 }
68 
69 } // namespace transform
70 } // namespace cartographer
Rigid3< double > Rigid3d
transform::Rigid3d FromDictionary(common::LuaParameterDictionary *dictionary)
std::unique_ptr< LuaParameterDictionary > GetDictionary(const std::string &key)
Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch, const double yaw)


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58