Apollo 10.0
自动驾驶开放平台
Public 成员函数 | 静态 Public 成员函数 | 所有成员列表
apollo::localization::msf::OfflineLocalVisualizer类 参考

Offline localization visualization tool. 更多...

#include <offline_local_visualizer.h>

apollo::localization::msf::OfflineLocalVisualizer 的协作图:

Public 成员函数

 OfflineLocalVisualizer ()
 
 ~OfflineLocalVisualizer ()
 
bool Init (const std::string &map_folder, const std::string &map_visual_folder, const std::string &pcd_folder, const std::string &pcd_timestamp_file, const std::string &gnss_loc_file, const std::string &lidar_loc_file, const std::string &fusion_loc_file, const std::string &extrinsic_file)
 
void Visualize ()
 
bool GetZoneIdFromMapFolder (const std::string &map_folder, const unsigned int resolution_id, int *zone_id)
 

静态 Public 成员函数

static void PoseAndStdInterpolationByTime (const ::apollo::common::EigenAffine3dVec &in_poses, const ::apollo::common::EigenVector3dVec &in_stds, const std::vector< double > &in_timestamps, const std::vector< double > &ref_timestamps, std::map< unsigned int, Eigen::Affine3d > *out_poses, std::map< unsigned int, Eigen::Vector3d > *out_stds)
 

详细描述

Offline localization visualization tool.

在文件 offline_local_visualizer.h 第 39 行定义.

构造及析构函数说明

◆ OfflineLocalVisualizer()

apollo::localization::msf::OfflineLocalVisualizer::OfflineLocalVisualizer ( )

在文件 offline_local_visualizer.cc 第 31 行定义.

32 : map_config_(), resolution_id_(0), zone_id_(0), visual_engine_() {}

◆ ~OfflineLocalVisualizer()

apollo::localization::msf::OfflineLocalVisualizer::~OfflineLocalVisualizer ( )

在文件 offline_local_visualizer.cc 第 34 行定义.

34{}

成员函数说明

◆ GetZoneIdFromMapFolder()

bool apollo::localization::msf::OfflineLocalVisualizer::GetZoneIdFromMapFolder ( const std::string &  map_folder,
const unsigned int  resolution_id,
int *  zone_id 
)

在文件 offline_local_visualizer.cc 第 294 行定义.

296 {
297 char buf[256];
298 snprintf(buf, sizeof(buf), "/%03u", resolution_id);
299 std::string folder_north = map_folder + "/map" + buf + "/north";
300 std::string folder_south = map_folder + "/map" + buf + "/south";
301 boost::filesystem::directory_iterator directory_end;
302 boost::filesystem::directory_iterator iter_north(folder_north);
303 if (iter_north == directory_end) {
304 boost::filesystem::directory_iterator iter_south(folder_south);
305 if (iter_south == directory_end) {
306 return false;
307 }
308 std::string zone_id_full_path = (*iter_south).path().string();
309 std::size_t pos = zone_id_full_path.find_last_of("/");
310 std::string zone_id_str =
311 zone_id_full_path.substr(pos + 1, zone_id_full_path.length());
312
313 *zone_id = -(std::stoi(zone_id_str));
314 AINFO << "Find zone id: " << *zone_id;
315 return true;
316 }
317 std::string zone_id_full_path = (*iter_north).path().string();
318 std::size_t pos = zone_id_full_path.find_last_of("/");
319 std::string zone_id_str =
320 zone_id_full_path.substr(pos + 1, zone_id_full_path.length());
321
322 *zone_id = (std::stoi(zone_id_str));
323 AINFO << "Find zone id: " << *zone_id;
324 return true;
325}
AINFO
#define AINFO
Definition log.h:42

◆ Init()

bool apollo::localization::msf::OfflineLocalVisualizer::Init ( const std::string &  map_folder,
const std::string &  map_visual_folder,
const std::string &  pcd_folder,
const std::string &  pcd_timestamp_file,
const std::string &  gnss_loc_file,
const std::string &  lidar_loc_file,
const std::string &  fusion_loc_file,
const std::string &  extrinsic_file 
)

在文件 offline_local_visualizer.cc 第 36 行定义.

40 {
41 map_folder_ = map_folder;
42 map_visual_folder_ = map_visual_folder;
43 pcd_folder_ = pcd_folder;
44 pcd_timestamp_file_ = pcd_timestamp_file;
45 gnss_loc_file_ = gnss_loc_file;
46 lidar_loc_file_ = lidar_loc_file;
47 fusion_loc_file_ = fusion_loc_file;
48 extrinsic_file_ = extrinsic_file;
49
50 pcd_timestamps_.clear();
51 gnss_poses_.clear();
52 lidar_poses_.clear();
53 fusion_poses_.clear();
54 gnss_stds_.clear();
55 lidar_stds_.clear();
56 fusion_stds_.clear();
57
58 const std::string config_file = map_folder_ + "/config.xml";
59 map_config_.map_version_ = "lossy_map";
60 bool success = map_config_.Load(config_file);
61 if (!success) {
62 AERROR << "Load map config failed.";
63 return false;
64 }
65 AINFO << "Load map config succeed.";
66
67 success = velodyne::LoadExtrinsic(extrinsic_file_, &velodyne_extrinsic_);
68 if (!success) {
69 AERROR << "Load velodyne extrinsic failed.";
70 return false;
71 }
72 AERROR << "Load velodyne extrinsic succeed.";
73
74 success = PCDTimestampFileHandler();
75 if (!success) {
76 AERROR << "Handle pcd timestamp file failed.";
77 return false;
78 }
79 AINFO << "Handle pcd timestamp file succeed.";
80
81 success = LidarLocFileHandler(pcd_timestamps_);
82 if (!success) {
83 AERROR << "Handle lidar localization file failed.";
84 return false;
85 }
86 AINFO << "Handle lidar localization file succeed.";
87
88 success = GnssLocFileHandler(pcd_timestamps_);
89 if (!success) {
90 AERROR << "Handle gnss localization file failed.";
91 return false;
92 }
93 AINFO << "Handle gnss localization file succeed.";
94
95 success = FusionLocFileHandler(pcd_timestamps_);
96 if (!success) {
97 AERROR << "Handle fusion localization file failed.";
98 return false;
99 }
100 AINFO << "Handle fusion localization file succeed.";
101
102 resolution_id_ = 0;
103 success = GetZoneIdFromMapFolder(map_folder_, resolution_id_, &zone_id_);
104 if (!success) {
105 AERROR << "Get zone id failed.";
106 return false;
107 }
108 AINFO << "Get zone id succeed.";
109
110 VisualMapParam map_param;
111 map_param.set(map_config_.map_resolutions_, map_config_.map_node_size_x_,
112 map_config_.map_node_size_y_, map_config_.map_range_.GetMinX(),
113 map_config_.map_range_.GetMinY(),
114 map_config_.map_range_.GetMaxX(),
115 map_config_.map_range_.GetMaxY());
116 success = visual_engine_.Init(map_folder_, map_visual_folder_, map_param,
117 resolution_id_, zone_id_, velodyne_extrinsic_,
118 LOC_INFO_NUM);
119 if (!success) {
120 AERROR << "Visualization engine init failed.";
121 return false;
122 }
123 AINFO << "Visualization engine init succeed.";
124
125 return true;
126}
apollo::localization::msf::OfflineLocalVisualizer::GetZoneIdFromMapFolder
bool GetZoneIdFromMapFolder(const std::string &map_folder, const unsigned int resolution_id, int *zone_id)
Definition offline_local_visualizer.cc:294
apollo::localization::msf::Rect2D::GetMaxY
T GetMaxY() const
Get the max y of the rectangle.
Definition rect2d.h:88
apollo::localization::msf::Rect2D::GetMaxX
T GetMaxX() const
Get the max x of the rectangle.
Definition rect2d.h:83
apollo::localization::msf::Rect2D::GetMinX
T GetMinX() const
Get the min x of the rectangle.
Definition rect2d.h:73
apollo::localization::msf::Rect2D::GetMinY
T GetMinY() const
Get the min y of the rectangle.
Definition rect2d.h:78
apollo::localization::msf::VisualizationEngine::Init
bool Init(const std::string &map_folder, const std::string &map_visual_folder, const VisualMapParam &map_param, const unsigned int resolution_id, const int zone_id, const Eigen::Affine3d &extrinsic, const unsigned int loc_info_num=1)
Definition visualization_engine.cc:88
apollo::localization::msf::pyramid_map::BaseMapConfig::map_node_size_y_
unsigned int map_node_size_y_
The map node size in pixels.
Definition base_map_config.h:80
apollo::localization::msf::pyramid_map::BaseMapConfig::map_resolutions_
std::vector< float > map_resolutions_
The pixel resolutions in the map in meters.
Definition base_map_config.h:76
apollo::localization::msf::pyramid_map::BaseMapConfig::map_range_
Rect2D< double > map_range_
The minimum and maximum UTM range in the map.
Definition base_map_config.h:85
apollo::localization::msf::pyramid_map::BaseMapConfig::Load
bool Load(const std::string &file_path)
Load the map option from a XML file.
Definition base_map_config.cc:54
apollo::localization::msf::pyramid_map::BaseMapConfig::map_node_size_x_
unsigned int map_node_size_x_
The map node size in pixels.
Definition base_map_config.h:78
apollo::localization::msf::pyramid_map::BaseMapConfig::map_version_
std::string map_version_
The version of map.
Definition base_map_config.h:74
AERROR
#define AERROR
Definition log.h:44
apollo::localization::msf::velodyne::LoadExtrinsic
bool LoadExtrinsic(const std::string &file_path, Eigen::Affine3d *extrinsic)
Load the velodyne extrinsic from a YAML file.
Definition velodyne_utility.cc:164
LOC_INFO_NUM
#define LOC_INFO_NUM
Definition visualization_manager.h:150

◆ PoseAndStdInterpolationByTime()

void apollo::localization::msf::OfflineLocalVisualizer::PoseAndStdInterpolationByTime ( const ::apollo::common::EigenAffine3dVec &  in_poses,
const ::apollo::common::EigenVector3dVec &  in_stds,
const std::vector< double > &  in_timestamps,
const std::vector< double > &  ref_timestamps,
std::map< unsigned int, Eigen::Affine3d > *  out_poses,
std::map< unsigned int, Eigen::Vector3d > *  out_stds 
)
static

在文件 offline_local_visualizer.cc 第 239 行定义.

245 {
246 unsigned int index = 0;
247 for (size_t i = 0; i < ref_timestamps.size(); ++i) {
248 double ref_timestamp = ref_timestamps[i];
249 while (index < in_timestamps.size() &&
250 in_timestamps.at(index) < ref_timestamp) {
251 ++index;
252 }
253
254 if (index >= in_timestamps.size()) {
255 AERROR << "[ERROR] No more poses. Exit now.";
256 break;
257 }
258 if (index >= 1) {
259 double cur_timestamp = in_timestamps[index];
260 double pre_timestamp = in_timestamps[index - 1];
261 DCHECK_NE(cur_timestamp, pre_timestamp);
262
263 double t =
264 (cur_timestamp - ref_timestamp) / (cur_timestamp - pre_timestamp);
265 DCHECK_GE(t, 0.0);
266 DCHECK_LE(t, 1.0);
267
268 Eigen::Affine3d pre_pose = in_poses[index - 1];
269 Eigen::Affine3d cur_pose = in_poses[index];
270 Eigen::Quaterniond pre_quatd(pre_pose.linear());
271 Eigen::Translation3d pre_transd(pre_pose.translation());
272 Eigen::Quaterniond cur_quatd(cur_pose.linear());
273 Eigen::Translation3d cur_transd(cur_pose.translation());
274
275 Eigen::Quaterniond res_quatd = pre_quatd.slerp(1 - t, cur_quatd);
276
277 Eigen::Translation3d re_transd;
278 re_transd.x() = pre_transd.x() * t + cur_transd.x() * (1 - t);
279 re_transd.y() = pre_transd.y() * t + cur_transd.y() * (1 - t);
280 re_transd.z() = pre_transd.z() * t + cur_transd.z() * (1 - t);
281 (*out_poses)[static_cast<unsigned int>(i)] = re_transd * res_quatd;
282
283 Eigen::Vector3d pre_std = in_stds[index - 1];
284 Eigen::Vector3d cur_std = in_stds[index];
285 Eigen::Vector3d std;
286 std[0] = pre_std[0] * t + cur_std[0] * (1 - t);
287 std[1] = pre_std[1] * t + cur_std[1] * (1 - t);
288 std[2] = pre_std[2] * t + cur_std[2] * (1 - t);
289 (*out_stds)[static_cast<unsigned int>(i)] = std;
290 }
291 }
292}
std
Definition future.h:29

◆ Visualize()

void apollo::localization::msf::OfflineLocalVisualizer::Visualize ( )

在文件 offline_local_visualizer.cc 第 128 行定义.

128 {
129 for (unsigned int idx = 0; idx < pcd_timestamps_.size(); ++idx) {
130 LocalizatonInfo lidar_loc_info;
131 LocalizatonInfo gnss_loc_info;
132 LocalizatonInfo fusion_loc_info;
133
134 AINFO << "Frame id: " << idx + 1;
135 auto pose_found_iter = lidar_poses_.find(idx);
136 auto std_found_iter = lidar_stds_.find(idx);
137 if (pose_found_iter != lidar_poses_.end() &&
138 std_found_iter != lidar_stds_.end()) {
139 AINFO << "Find lidar pose.";
140 const Eigen::Affine3d &lidar_pose = pose_found_iter->second;
141 const Eigen::Vector3d &lidar_std = std_found_iter->second;
142 lidar_loc_info.set(Eigen::Translation3d(lidar_pose.translation()),
143 Eigen::Quaterniond(lidar_pose.linear()), lidar_std,
144 "Lidar.", pcd_timestamps_[idx], idx + 1);
145 }
146
147 pose_found_iter = gnss_poses_.find(idx);
148 std_found_iter = gnss_stds_.find(idx);
149 if (pose_found_iter != gnss_poses_.end() &&
150 std_found_iter != gnss_stds_.end()) {
151 AINFO << "Find gnss pose.";
152 const Eigen::Affine3d &gnss_pose = pose_found_iter->second;
153 const Eigen::Vector3d &gnss_std = std_found_iter->second;
154 gnss_loc_info.set(Eigen::Translation3d(gnss_pose.translation()), gnss_std,
155 "GNSS.", pcd_timestamps_[idx], idx + 1);
156 }
157
158 pose_found_iter = fusion_poses_.find(idx);
159 std_found_iter = fusion_stds_.find(idx);
160 if (pose_found_iter != fusion_poses_.end() &&
161 std_found_iter != fusion_stds_.end()) {
162 AINFO << "Find fusion pose.";
163 const Eigen::Affine3d &fusion_pose = pose_found_iter->second;
164 const Eigen::Vector3d &fusion_std = std_found_iter->second;
165 fusion_loc_info.set(Eigen::Translation3d(fusion_pose.translation()),
166 Eigen::Quaterniond(fusion_pose.linear()), fusion_std,
167 "Fusion.", pcd_timestamps_[idx], idx + 1);
168 }
169
170 std::string pcd_file_path = absl::StrCat(pcd_folder_, "/", idx + 1, ".pcd");
171 ::apollo::common::EigenVector3dVec pt3ds;
172 std::vector<unsigned char> intensities;
173 apollo::localization::msf::velodyne::LoadPcds(
174 pcd_file_path, idx, lidar_loc_info.pose, &pt3ds, &intensities, false);
175
176 ::apollo::common::EigenVector<LocalizatonInfo> loc_infos{
177 lidar_loc_info, gnss_loc_info, fusion_loc_info};
178 visual_engine_.Visualize(std::move(loc_infos), pt3ds);
179 }
180}
apollo::localization::msf::VisualizationEngine::Visualize
void Visualize(::apollo::common::EigenVector< LocalizatonInfo > &&loc_infos, const ::apollo::common::EigenVector3dVec &cloud)
Definition visualization_engine.cc:144
apollo::common::EigenVector
std::vector< EigenType, Eigen::aligned_allocator< EigenType > > EigenVector
Definition eigen_defs.h:33
apollo::common::EigenVector3dVec
EigenVector< Eigen::Vector3d > EigenVector3dVec
Definition eigen_defs.h:46
apollo::localization::msf::velodyne::LoadPcds
void LoadPcds(const std::string &file_path, const unsigned int frame_index, const Eigen::Affine3d &pose, VelodyneFrame *velodyne_frame, bool is_global)
Definition velodyne_utility.cc:30
该类的文档由以下文件生成: