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

#include <ndt_localization.h>

apollo::localization::ndt::NDTLocalization 的协作图:

Public 成员函数

 NDTLocalization ()
 
 ~NDTLocalization ()
 
void Init ()
 init configuration
 
void OdometryCallback (const std::shared_ptr< localization::Gps > &odometry_msg)
 receive odometry message
 
void LidarCallback (const std::shared_ptr< drivers::PointCloud > &lidar_msg)
 receive lidar pointcloud message
 
void OdometryStatusCallback (const std::shared_ptr< drivers::gnss::InsStat > &status_msg)
 receive ins status message
 
bool IsServiceStarted ()
 service start status
 
void GetLocalization (LocalizationEstimate *localization) const
 output localization result
 
void GetLidarLocalization (LocalizationEstimate *lidar_localization) const
 get ndt localization result
 
void GetLocalizationStatus (LocalizationStatus *localization_status) const
 get localization status
 
int GetZoneId () const
 get zone id
 
double GetOnlineResolution () const
 get online resolution for ndt localization
 

详细描述

在文件 ndt_localization.h 第 47 行定义.

构造及析构函数说明

◆ NDTLocalization()

apollo::localization::ndt::NDTLocalization::NDTLocalization ( )
inline

在文件 ndt_localization.h 第 52 行定义.

52{}

◆ ~NDTLocalization()

apollo::localization::ndt::NDTLocalization::~NDTLocalization ( )
inline

在文件 ndt_localization.h 第 53 行定义.

53{ tf_buffer_ = nullptr; }

成员函数说明

◆ GetLidarLocalization()

void apollo::localization::ndt::NDTLocalization::GetLidarLocalization ( LocalizationEstimate *  lidar_localization) const

get ndt localization result

在文件 ndt_localization.cc 第 243 行定义.

244 {
245 *localization = lidar_localization_result_;
246}

◆ GetLocalization()

void apollo::localization::ndt::NDTLocalization::GetLocalization ( LocalizationEstimate *  localization) const

output localization result

在文件 ndt_localization.cc 第 238 行定义.

239 {
240 *localization = localization_result_;
241}

◆ GetLocalizationStatus()

void apollo::localization::ndt::NDTLocalization::GetLocalizationStatus ( LocalizationStatus *  localization_status) const

get localization status

在文件 ndt_localization.cc 第 248 行定义.

249 {
250 *localization_status = localization_status_;
251}

◆ GetOnlineResolution()

double apollo::localization::ndt::NDTLocalization::GetOnlineResolution ( ) const
inline

get online resolution for ndt localization

在文件 ndt_localization.h 第 74 行定义.

74{ return online_resolution_; }

◆ GetZoneId()

int apollo::localization::ndt::NDTLocalization::GetZoneId ( ) const
inline

get zone id

在文件 ndt_localization.h 第 72 行定义.

72{ return zone_id_; }

◆ Init()

void apollo::localization::ndt::NDTLocalization::Init ( )

init configuration

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

34 {
35 tf_buffer_ = apollo::transform::Buffer::Instance();
36 tf_buffer_->Init();
37
38 resolution_id_ = 0;
39 zone_id_ = FLAGS_local_utm_zone_id;
40 online_resolution_ = FLAGS_online_resolution;
41 ndt_debug_log_flag_ = FLAGS_ndt_debug_log_flag;
42 tf_source_frame_id_ = FLAGS_broadcast_tf_frame_id;
43 tf_target_frame_id_ = FLAGS_broadcast_tf_child_frame_id;
44 std::string lidar_height_file = FLAGS_lidar_height_file;
45 std::string lidar_extrinsics_file = FLAGS_lidar_extrinsics_file;
46 bad_score_count_threshold_ = FLAGS_ndt_bad_score_count_threshold;
47 warnning_ndt_score_ = FLAGS_ndt_warnning_ndt_score;
48 error_ndt_score_ = FLAGS_ndt_error_ndt_score;
49
50 std::string map_path_ =
51 FLAGS_map_dir + "/" + FLAGS_ndt_map_dir + "/" + FLAGS_local_map_name;
52 AINFO << "map folder: " << map_path_;
53 velodyne_extrinsic_ = Eigen::Affine3d::Identity();
54 bool success =
55 LoadLidarExtrinsic(lidar_extrinsics_file, &velodyne_extrinsic_);
56 if (!success) {
57 AERROR << "LocalizationLidar: Fail to access the lidar"
58 " extrinsic file: "
59 << lidar_extrinsics_file;
60 return;
61 }
62 Eigen::Quaterniond ext_quat(velodyne_extrinsic_.linear());
63 AINFO << "lidar extrinsics: " << velodyne_extrinsic_.translation().x() << ", "
64 << velodyne_extrinsic_.translation().y() << ", "
65 << velodyne_extrinsic_.translation().z() << ", " << ext_quat.x() << ", "
66 << ext_quat.y() << ", " << ext_quat.z() << ", " << ext_quat.w();
67
68 success = LoadLidarHeight(lidar_height_file, &lidar_height_);
69 if (!success) {
70 AWARN << "LocalizationLidar: Fail to load the lidar"
71 " height file: "
72 << lidar_height_file << " Will use default value!";
73 lidar_height_.height = FLAGS_lidar_height_default;
74 }
75
76 // try load zone id from local_map folder
77 if (FLAGS_if_utm_zone_id_from_folder) {
78 bool success = LoadZoneIdFromFolder(map_path_, &zone_id_);
79 if (!success) {
80 AWARN << "Can't load utm zone id from map folder, use default value.";
81 }
82 }
83 AINFO << "utm zone id: " << zone_id_;
84
85 lidar_locator_.SetMapFolderPath(map_path_);
86 lidar_locator_.SetVelodyneExtrinsic(velodyne_extrinsic_);
87 lidar_locator_.SetLidarHeight(lidar_height_.height);
88 lidar_locator_.SetOnlineCloudResolution(
89 static_cast<float>(online_resolution_));
90
91 odometry_buffer_.clear();
92 odometry_buffer_size_ = 0;
93
94 is_service_started_ = false;
95}
apollo::localization::ndt::LidarLocatorNdt::SetVelodyneExtrinsic
void SetVelodyneExtrinsic(const Eigen::Affine3d &extrinsic)
Set the extrinsic calibration.
Definition lidar_locator_ndt.cc:109
apollo::localization::ndt::LidarLocatorNdt::SetMapFolderPath
void SetMapFolderPath(const std::string folder_path)
Set the map folder.
Definition lidar_locator_ndt.cc:103
apollo::localization::ndt::LidarLocatorNdt::SetOnlineCloudResolution
void SetOnlineCloudResolution(const float &online_resolution)
Set online cloud resolution.
Definition lidar_locator_ndt.cc:118
apollo::localization::ndt::LidarLocatorNdt::SetLidarHeight
void SetLidarHeight(double height)
Set the lidar height.
Definition lidar_locator_ndt.cc:113
apollo::transform::Buffer::Init
int Init()
Constructor for a Buffer object
Definition buffer.cc:38
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
AWARN
#define AWARN
Definition log.h:43

◆ IsServiceStarted()

bool apollo::localization::ndt::NDTLocalization::IsServiceStarted ( )

service start status

在文件 ndt_localization.cc 第 253 行定义.

253{ return is_service_started_; }

◆ LidarCallback()

void apollo::localization::ndt::NDTLocalization::LidarCallback ( const std::shared_ptr< drivers::PointCloud > &  lidar_msg)

receive lidar pointcloud message

在文件 ndt_localization.cc 第 171 行定义.

172 {
173 static unsigned int frame_idx = 0;
174 LidarFrame lidar_frame;
175 LidarMsgTransfer(lidar_msg, &lidar_frame);
176
177 double time_stamp = lidar_frame.measurement_time;
178 Eigen::Affine3d odometry_pose = Eigen::Affine3d::Identity();
179 if (!QueryPoseFromBuffer(time_stamp, &odometry_pose)) {
180 if (!QueryPoseFromTF(time_stamp, &odometry_pose)) {
181 AERROR << "Can not query forecast pose";
182 return;
183 }
184 AINFO << "Query pose from TF";
185 } else {
186 AINFO << "Query pose from buffer";
187 }
188 if (!lidar_locator_.IsInitialized()) {
189 lidar_locator_.Init(odometry_pose, resolution_id_, zone_id_);
190 return;
191 }
192 lidar_locator_.Update(frame_idx++, odometry_pose, lidar_frame);
193 lidar_pose_ = lidar_locator_.GetPose();
194 pose_buffer_.UpdateLidarPose(time_stamp, lidar_pose_, odometry_pose);
195 ComposeLidarResult(time_stamp, lidar_pose_, &lidar_localization_result_);
196 ndt_score_ = lidar_locator_.GetFitnessScore();
197 if (ndt_score_ > warnning_ndt_score_) {
198 bad_score_count_++;
199 } else {
200 bad_score_count_ = 0;
201 }
202 if (ndt_debug_log_flag_) {
203 Eigen::Quaterniond tmp_quat(lidar_pose_.linear());
204 AINFO << "NDTLocalization Debug Log: lidar pose: " << std::setprecision(15)
205 << "time: " << time_stamp << ", "
206 << "x: " << lidar_pose_.translation()[0] << ", "
207 << "y: " << lidar_pose_.translation()[1] << ", "
208 << "z: " << lidar_pose_.translation()[2] << ", "
209 << "qx: " << tmp_quat.x() << ", "
210 << "qy: " << tmp_quat.y() << ", "
211 << "qz: " << tmp_quat.z() << ", "
212 << "qw: " << tmp_quat.w();
213
214 Eigen::Quaterniond qbn(odometry_pose.linear());
215 AINFO << "NDTLocalization Debug Log: odometry for lidar pose: "
216 << std::setprecision(15) << "time: " << time_stamp << ", "
217 << "x: " << odometry_pose.translation()[0] << ", "
218 << "y: " << odometry_pose.translation()[1] << ", "
219 << "z: " << odometry_pose.translation()[2] << ", "
220 << "qx: " << qbn.x() << ", "
221 << "qy: " << qbn.y() << ", "
222 << "qz: " << qbn.z() << ", "
223 << "qw: " << qbn.w();
224 }
225}
apollo::localization::ndt::LidarLocatorNdt::IsInitialized
bool IsInitialized() const
Is the locator initialized.
Definition lidar_locator_ndt.h:123
apollo::localization::ndt::LidarLocatorNdt::Update
int Update(unsigned int frame_idx, const Eigen::Affine3d &pose, const LidarFrame &lidar_frame)
Update the histogram filter.
Definition lidar_locator_ndt.cc:123
apollo::localization::ndt::LidarLocatorNdt::Init
void Init(const Eigen::Affine3d &init_location, unsigned int resolution_id, int zone_id)
Initialize the locator.
Definition lidar_locator_ndt.cc:50
apollo::localization::ndt::LidarLocatorNdt::GetPose
Eigen::Affine3d GetPose() const
Get the current optimal pose result.
Definition lidar_locator_ndt.cc:249
apollo::localization::ndt::LidarLocatorNdt::GetFitnessScore
double GetFitnessScore() const
Get ndt matching score
Definition lidar_locator_ndt.h:129
apollo::localization::ndt::LocalizationPoseBuffer::UpdateLidarPose
void UpdateLidarPose(double timestamp, const Eigen::Affine3d &locator_pose, const Eigen::Affine3d &novatel_pose)
receive a pair of lidar pose and odometry pose which almost have the same timestame
Definition localization_pose_buffer.cc:41

◆ OdometryCallback()

void apollo::localization::ndt::NDTLocalization::OdometryCallback ( const std::shared_ptr< localization::Gps > &  odometry_msg)

receive odometry message

在文件 ndt_localization.cc 第 97 行定义.

98 {
99 double odometry_time = odometry_msg->header().timestamp_sec();
100 static double pre_odometry_time = odometry_time;
101 double time_delay = odometry_time - pre_odometry_time;
102 if (time_delay > 0.1) {
103 AINFO << "Odometry message loss more than 10ms, the pre time and cur time: "
104 << pre_odometry_time << ", " << odometry_time;
105 } else if (time_delay < 0.0) {
106 AINFO << "Odometry message's time is earlier than last one, "
107 << "the pre time and cur time: " << pre_odometry_time << ", "
108 << odometry_time;
109 }
110 pre_odometry_time = odometry_time;
111 Eigen::Affine3d odometry_pose = Eigen::Affine3d::Identity();
112 if (odometry_msg->has_localization()) {
113 odometry_pose.translation()[0] =
114 odometry_msg->localization().position().x();
115 odometry_pose.translation()[1] =
116 odometry_msg->localization().position().y();
117 odometry_pose.translation()[2] =
118 odometry_msg->localization().position().z();
119 Eigen::Quaterniond tmp_quat(
120 odometry_msg->localization().orientation().qw(),
121 odometry_msg->localization().orientation().qx(),
122 odometry_msg->localization().orientation().qy(),
123 odometry_msg->localization().orientation().qz());
124 odometry_pose.linear() = tmp_quat.toRotationMatrix();
125 }
126 if (ZeroOdometry(odometry_pose)) {
127 AINFO << "Detect Zero Odometry";
128 return;
129 }
130
131 TimeStampPose timestamp_pose;
132 timestamp_pose.timestamp = odometry_time;
133 timestamp_pose.pose = odometry_pose;
134 {
135 std::lock_guard<std::mutex> lock(odometry_buffer_mutex_);
136 if (odometry_buffer_size_ < max_odometry_buffer_size_) {
137 odometry_buffer_.push_back(timestamp_pose);
138 odometry_buffer_size_++;
139 } else {
140 odometry_buffer_.pop_front();
141 odometry_buffer_.push_back(timestamp_pose);
142 }
143 }
144
145 if (ndt_debug_log_flag_) {
146 AINFO << "NDTLocalization Debug Log: odometry msg: "
147 << std::setprecision(15) << "time: " << odometry_time << ", "
148 << "x: " << odometry_msg->localization().position().x() << ", "
149 << "y: " << odometry_msg->localization().position().y() << ", "
150 << "z: " << odometry_msg->localization().position().z() << ", "
151 << "qx: " << odometry_msg->localization().orientation().qx() << ", "
152 << "qy: " << odometry_msg->localization().orientation().qy() << ", "
153 << "qz: " << odometry_msg->localization().orientation().qz() << ", "
154 << "qw: " << odometry_msg->localization().orientation().qw();
155 }
156
157 Eigen::Affine3d localization_pose = Eigen::Affine3d::Identity();
158 if (lidar_locator_.IsInitialized()) {
159 localization_pose =
160 pose_buffer_.UpdateOdometryPose(odometry_time, odometry_pose);
161 }
162 ComposeLocalizationEstimate(localization_pose, odometry_msg,
163 &localization_result_);
164 drivers::gnss::InsStat odometry_status;
165 FindNearestOdometryStatus(odometry_time, &odometry_status);
166 ComposeLocalizationStatus(odometry_status, &localization_status_);
167 is_service_started_ = true;
168}
apollo::localization::ndt::LocalizationPoseBuffer::UpdateOdometryPose
Eigen::Affine3d UpdateOdometryPose(double timestamp, const Eigen::Affine3d &novatel_pose)
receive an odometry pose and estimate the output pose according to last lidar pose recorded.
Definition localization_pose_buffer.cc:67

◆ OdometryStatusCallback()

void apollo::localization::ndt::NDTLocalization::OdometryStatusCallback ( const std::shared_ptr< drivers::gnss::InsStat > &  status_msg)

receive ins status message

在文件 ndt_localization.cc 第 227 行定义.

228 {
229 std::unique_lock<std::mutex> lock(odometry_status_list_mutex_);
230 if (odometry_status_list_.size() < odometry_status_list_max_size_) {
231 odometry_status_list_.push_back(*status_msg);
232 } else {
233 odometry_status_list_.pop_front();
234 odometry_status_list_.push_back(*status_msg);
235 }
236}
该类的文档由以下文件生成: