Apollo 11.0
自动驾驶开放平台
lossless_map_node.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2018 The Apollo Authors. All Rights Reserved.
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
17#include "modules/localization/msf/local_map/lossless_map/lossless_map_node.h"
18
19#include "cyber/common/log.h"
20#include "modules/localization/msf/local_map/lossless_map/lossless_map_config.h"
21
22namespace apollo {
23namespace localization {
24namespace msf {
27
28LosslessMapNode::~LosslessMapNode() {}
29
30void LosslessMapNode::SetValue(const Eigen::Vector3d& coordinate,
31 unsigned char intensity) {
32 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
33 unsigned int x = 0;
34 unsigned int y = 0;
35 bool is_success = GetCoordinate(coord2d, &x, &y);
36 DCHECK(is_success);
37 LosslessMapCell& map_cell =
38 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
39 map_cell.SetValue(coordinate[2], intensity);
40 if (min_altitude_ > coordinate[2]) {
41 min_altitude_ = static_cast<float>(coordinate[2]);
42 }
43 is_changed_ = true;
44}
45
46bool LosslessMapNode::SetValueIfInBound(const Eigen::Vector3d& coordinate,
47 unsigned char intensity) {
48 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
49 unsigned int x = 0;
50 unsigned int y = 0;
51 bool is_success = GetCoordinate(coord2d, &x, &y);
52 if (is_success) {
53 LosslessMapCell& map_cell =
54 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
55 map_cell.SetValue(coordinate[2], intensity);
56 if (min_altitude_ > coordinate[2]) {
57 min_altitude_ = static_cast<float>(coordinate[2]);
58 }
59 is_changed_ = true;
60 return true;
61 } else {
62 return false;
63 }
64}
65
66void LosslessMapNode::SetValueLayer(const Eigen::Vector3d& coordinate,
67 unsigned char intensity) {
68 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
69 unsigned int x = 0;
70 unsigned int y = 0;
71 bool is_success = GetCoordinate(coord2d, &x, &y);
72 DCHECK(is_success);
73 LosslessMapCell& map_cell =
74 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
75 map_cell.SetValueLayer(
76 coordinate[2], intensity,
77 static_cast<const LosslessMapConfig*>(map_config_)->map_layer_alt_thres_);
78 is_changed_ = true;
79}
80
81void LosslessMapNode::GetValue(const Eigen::Vector3d& coordinate,
82 std::vector<unsigned char>* values) const {
83 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
84 unsigned int x = 0;
85 unsigned int y = 0;
86 bool is_success = GetCoordinate(coord2d, &x, &y);
87 DCHECK(is_success);
88 LosslessMapCell& map_cell =
89 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
90 map_cell.GetValue(values);
91}
92
93void LosslessMapNode::GetVar(const Eigen::Vector3d& coordinate,
94 std::vector<float>* vars) const {
95 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
96 unsigned int x = 0;
97 unsigned int y = 0;
98 bool is_success = GetCoordinate(coord2d, &x, &y);
99 DCHECK(is_success);
100 LosslessMapCell& map_cell =
101 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
102 map_cell.GetVar(vars);
103}
104
105void LosslessMapNode::GetAlt(const Eigen::Vector3d& coordinate,
106 std::vector<float>* alts) const {
107 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
108 unsigned int x = 0;
109 unsigned int y = 0;
110 bool is_success = GetCoordinate(coord2d, &x, &y);
111 DCHECK(is_success);
112 LosslessMapCell& map_cell =
113 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
114 map_cell.GetAlt(alts);
115}
116
117void LosslessMapNode::GetAltVar(const Eigen::Vector3d& coordinate,
118 std::vector<float>* alt_vars) const {
119 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
120 unsigned int x = 0;
121 unsigned int y = 0;
122 bool is_success = GetCoordinate(coord2d, &x, &y);
123 DCHECK(is_success);
124 LosslessMapCell& map_cell =
125 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
126 map_cell.GetAltVar(alt_vars);
127}
128
129void LosslessMapNode::GetCount(const Eigen::Vector3d& coordinate,
130 std::vector<unsigned int>* counts) const {
131 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
132 unsigned int x = 0;
133 unsigned int y = 0;
134 bool is_success = GetCoordinate(coord2d, &x, &y);
135 DCHECK(is_success);
136 LosslessMapCell& map_cell =
137 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
138 map_cell.GetCount(counts);
139}
140
141unsigned char LosslessMapNode::GetValue(
142 const Eigen::Vector3d& coordinate) const {
143 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
144 unsigned int x = 0;
145 unsigned int y = 0;
146 bool is_success = GetCoordinate(coord2d, &x, &y);
147 DCHECK(is_success);
148 LosslessMapCell& map_cell =
149 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
150 return map_cell.GetValue();
151}
152
153float LosslessMapNode::GetVar(const Eigen::Vector3d& coordinate) const {
154 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
155 unsigned int x = 0;
156 unsigned int y = 0;
157 bool is_success = GetCoordinate(coord2d, &x, &y);
158 DCHECK(is_success);
159 LosslessMapCell& map_cell =
160 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
161 return map_cell.GetVar();
162}
163
164float LosslessMapNode::GetAlt(const Eigen::Vector3d& coordinate) const {
165 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
166 unsigned int x = 0;
167 unsigned int y = 0;
168 bool is_success = GetCoordinate(coord2d, &x, &y);
169 DCHECK(is_success);
170 LosslessMapCell& map_cell =
171 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
172 return map_cell.GetAlt();
173}
174
175float LosslessMapNode::GetAltVar(const Eigen::Vector3d& coordinate) const {
176 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
177 unsigned int x = 0;
178 unsigned int y = 0;
179 bool is_success = GetCoordinate(coord2d, &x, &y);
180 DCHECK(is_success);
181 LosslessMapCell& map_cell =
182 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
183 return map_cell.GetAltVar();
184}
185
186unsigned int LosslessMapNode::GetCount(
187 const Eigen::Vector3d& coordinate) const {
188 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
189 unsigned int x = 0;
190 unsigned int y = 0;
191 bool is_success = GetCoordinate(coord2d, &x, &y);
192 DCHECK(is_success);
193 LosslessMapCell& map_cell =
194 static_cast<LosslessMapMatrix*>(map_matrix_)->GetMapCell(y, x);
195 return map_cell.GetCount();
196}
197
198} // namespace msf
199} // namespace localization
200} // namespace apollo
apollo::localization::msf::BaseMapNode
The data structure of a Node in the map.
Definition base_map_node.h:32
apollo::localization::msf::BaseMapNode::map_matrix_
BaseMapMatrix * map_matrix_
The data structure of the map datas, which is a matrix.
Definition base_map_node.h:178
apollo::localization::msf::BaseMapNode::is_changed_
bool is_changed_
Has the map node been changed.
Definition base_map_node.h:182
apollo::localization::msf::BaseMapNode::map_config_
const BaseMapConfig * map_config_
The map settings.
Definition base_map_node.h:170
apollo::localization::msf::BaseMapNode::GetCoordinate
bool GetCoordinate(const Eigen::Vector2d &coordinate, unsigned int *x, unsigned int *y) const
Given the global coordinate, get the local 2D coordinate of the map cell matrix.
Definition base_map_node.cc:352
apollo::localization::msf::LosslessMapConfig
The options of the reflectance map.
Definition lossless_map_config.h:28
apollo::localization::msf::LosslessMapConfig::map_layer_alt_thres_
float map_layer_alt_thres_
The threshold to split more layers in the map node.
Definition lossless_map_config.h:35
apollo::localization::msf::LosslessMapNode::SetValue
void SetValue(const Eigen::Vector3d &coordinate, unsigned char intensity)
Set the value of a pixel in the map node.
Definition lossless_map_node.cc:30
apollo::localization::msf::LosslessMapNode::min_altitude_
float min_altitude_
The min altitude of point cloud in the node.
Definition lossless_map_node.h:138
apollo::localization::msf::LosslessMapNode::GetAlt
void GetAlt(const Eigen::Vector3d &coordinate, std::vector< float > *alts) const
Given the 3D global coordinate, get the map cell's average altitude of each layer.
Definition lossless_map_node.cc:105
apollo::localization::msf::LosslessMapNode::SetValueLayer
void SetValueLayer(const Eigen::Vector3d &coordinate, unsigned char intensity)
Set the value of a pixel in a layer in the map node.
Definition lossless_map_node.cc:66
apollo::localization::msf::LosslessMapNode::SetValueIfInBound
bool SetValueIfInBound(const Eigen::Vector3d &coordinate, unsigned char intensity)
Set the value of a pixel in the map node if the pixel in the node.
Definition lossless_map_node.cc:46
apollo::localization::msf::LosslessMapNode::GetAltVar
void GetAltVar(const Eigen::Vector3d &coordinate, std::vector< float > *alt_vars) const
Given the 3D global coordinate, get the map cell's variance of the altitude of each layer.
Definition lossless_map_node.cc:117
apollo::localization::msf::LosslessMapNode::GetValue
void GetValue(const Eigen::Vector3d &coordinate, std::vector< unsigned char > *values) const
Given the 3D global coordinate, get the map cell average intensity of each layer.
Definition lossless_map_node.cc:81
apollo::localization::msf::LosslessMapNode::GetCount
void GetCount(const Eigen::Vector3d &coordinate, std::vector< unsigned int > *counts) const
Given the 3D global coordinate, get the map cell's count of the samples of each layer.
Definition lossless_map_node.cc:129
apollo::localization::msf::LosslessMapNode::GetVar
void GetVar(const Eigen::Vector3d &coordinate, std::vector< float > *vars) const
Given the 3D global coordinate, get the map cell variance of the intensity of each layer.
Definition lossless_map_node.cc:93
lossless_map_config.h
lossless_map_node.h
apollo
class register implement
Definition arena_queue.h:37
apollo::localization::msf::LosslessMapCell::GetCount
void GetCount(std::vector< unsigned int > *counts) const
Get the count of the samples of all layers in the map cell.
Definition lossless_map_matrix.cc:252
apollo::localization::msf::LosslessMapCell::GetVar
void GetVar(std::vector< float > *vars) const
Get the variance of the intensity of all layers in the map cell.
Definition lossless_map_matrix.cc:228
apollo::localization::msf::LosslessMapCell::SetValueLayer
void SetValueLayer(double altitude, unsigned char intensity, double altitude_thres=10.0)
Set the value of a layer that layer_id > 0.
Definition lossless_map_matrix.cc:114
apollo::localization::msf::LosslessMapCell::GetAlt
void GetAlt(std::vector< float > *alts) const
Get the average altitude of all layers in the map cell.
Definition lossless_map_matrix.cc:236
apollo::localization::msf::LosslessMapCell::GetAltVar
void GetAltVar(std::vector< float > *alt_vars) const
Get the variance of the altitude of all layers in the map cell.
Definition lossless_map_matrix.cc:244
apollo::localization::msf::LosslessMapCell::SetValue
void SetValue(double altitude, unsigned char intensity)
Set the value.
Definition lossless_map_matrix.cc:156
apollo::localization::msf::LosslessMapCell::GetValue
void GetValue(std::vector< unsigned char > *values) const
Load the map cell from a binary chunk.
Definition lossless_map_matrix.cc:220