Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
ef3725cd
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
前往新版Gitcode,体验更适合开发者的 AI 搜索 >>
提交
ef3725cd
编写于
5月 23, 2019
作者:
X
Xiangquan Xiao
提交者:
Jiangtao Hu
5月 23, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Robot: Fix typos.
上级
cad9c06e
变更
13
隐藏空白更改
内联
并排
Showing
13 changed file
with
35 addition
and
35 deletion
+35
-35
modules/localization/msf/common/util/voxel_grid_covariance_hdmap.h
...ocalization/msf/common/util/voxel_grid_covariance_hdmap.h
+2
-2
modules/localization/msf/local_integ/localization_gnss_process.cc
...localization/msf/local_integ/localization_gnss_process.cc
+2
-2
modules/localization/msf/local_integ/localization_lidar_process.cc
...ocalization/msf/local_integ/localization_lidar_process.cc
+5
-5
modules/localization/msf/local_integ/measure_republish_process.cc
...localization/msf/local_integ/measure_republish_process.cc
+1
-1
modules/localization/msf/local_map/base_map/base_map_cache.h
modules/localization/msf/local_map/base_map/base_map_cache.h
+4
-4
modules/localization/msf/local_map/lossless_map/lossless_map_matrix.h
...lization/msf/local_map/lossless_map/lossless_map_matrix.h
+1
-1
modules/localization/msf/local_map/ndt_map/ndt_map_matrix.h
modules/localization/msf/local_map/ndt_map/ndt_map_matrix.h
+1
-1
modules/localization/ndt/localization_pose_buffer.cc
modules/localization/ndt/localization_pose_buffer.cc
+1
-1
modules/localization/ndt/ndt_localization.cc
modules/localization/ndt/ndt_localization.cc
+4
-4
modules/localization/ndt/ndt_locator/ndt_solver.h
modules/localization/ndt/ndt_locator/ndt_solver.h
+4
-4
modules/localization/ndt/ndt_locator/ndt_solver.hpp
modules/localization/ndt/ndt_locator/ndt_solver.hpp
+5
-5
modules/localization/ndt/ndt_locator/ndt_voxel_grid_covariance.h
.../localization/ndt/ndt_locator/ndt_voxel_grid_covariance.h
+3
-3
modules/localization/rtk/rtk_localization_test.cc
modules/localization/rtk/rtk_localization_test.cc
+2
-2
未找到文件。
modules/localization/msf/common/util/voxel_grid_covariance_hdmap.h
浏览文件 @
ef3725cd
...
...
@@ -580,7 +580,7 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT> {
// Flag to determine if voxel structure is searchable. */
bool
searchable_
;
// Minimum points contained with in a voxel to allow it to be us
e
able.
// Minimum points contained with in a voxel to allow it to be usable.
int
min_points_per_voxel_
;
// Minimum allowable ratio between eigenvalues.
...
...
@@ -590,7 +590,7 @@ class VoxelGridCovariance : public pcl::VoxelGrid<PointT> {
std
::
map
<
size_t
,
Leaf
>
leaves_
;
/* Point cloud containing centroids of voxels
* containing atleast minimum number of points. */
* containing at
least minimum number of points. */
PointCloudPtr
voxel_centroids_
;
/* Indices of leaf structurs associated with each point in
...
...
modules/localization/msf/local_integ/localization_gnss_process.cc
浏览文件 @
ef3725cd
...
...
@@ -136,7 +136,7 @@ void LocalizationGnssProcess::RawEphemerisProcess(
auto
gnss_orbit
=
msg
;
if
(
gnss_orbit
.
gnss_type
()
==
drivers
::
gnss
::
GnssType
::
GLO_SYS
)
{
/* caros driver (derived from rtklib src) set glonass eph toe as the GPST,
* and here convert it back to UTC(+0), so leap seconds shou
dl
be in
* and here convert it back to UTC(+0), so leap seconds shou
ld
be in
* accordance with the GNSS-Driver
*/
double
leap_sec
=
...
...
@@ -316,7 +316,7 @@ bool LocalizationGnssProcess::GnssPosition(EpochObservationMsg *raw_rover_obs) {
}
LogPnt
(
gnss_pnt_result_
,
gnss_solver_
->
get_ratio
());
if
(
!
sins_align_finish_
)
{
AWARN
<<
"Sins-ekf has not converged or finished its aligment!"
;
AWARN
<<
"Sins-ekf has not converged or finished its alig
n
ment!"
;
}
if
(
gnss_pnt_result_
.
has_std_pos_x_m
()
&&
gnss_pnt_result_
.
has_std_pos_y_m
()
&&
...
...
modules/localization/msf/local_integ/localization_lidar_process.cc
浏览文件 @
ef3725cd
...
...
@@ -107,8 +107,8 @@ Status LocalizationLidarProcess::Init(const LocalizationIntegParam& params) {
local_lidar_status_
=
LocalLidarStatus
::
MSF_LOCAL_LIDAR_UNDEFINED_STATUS
;
local_lidar_quality_
=
LocalLidarQuality
::
MSF_LOCAL_LIDAR_BAD
;
bool
sucess
=
LoadLidarExtrinsic
(
lidar_extrinsic_file_
,
&
lidar_extrinsic_
);
if
(
!
sucess
)
{
bool
suc
c
ess
=
LoadLidarExtrinsic
(
lidar_extrinsic_file_
,
&
lidar_extrinsic_
);
if
(
!
suc
c
ess
)
{
AERROR
<<
"LocalizationLidar: Fail to access the lidar"
" extrinsic file: "
<<
lidar_extrinsic_file_
;
...
...
@@ -116,8 +116,8 @@ Status LocalizationLidarProcess::Init(const LocalizationIntegParam& params) {
"Fail to access the lidar extrinsic file"
);
}
sucess
=
LoadLidarHeight
(
lidar_height_file_
,
&
lidar_height_
);
if
(
!
sucess
)
{
suc
c
ess
=
LoadLidarHeight
(
lidar_height_file_
,
&
lidar_height_
);
if
(
!
suc
c
ess
)
{
AWARN
<<
"LocalizationLidar: Fail to load the lidar"
" height file: "
<<
lidar_height_file_
<<
" Will use default value!"
;
...
...
@@ -312,7 +312,7 @@ bool LocalizationLidarProcess::GetPredictPose(const double lidar_time,
if
(
state
<
0
)
{
AINFO
<<
"LocalizationLidar GetPredictPose: "
<<
"Recive a lidar msg, but can't query predict pose."
;
<<
"Rec
e
ive a lidar msg, but can't query predict pose."
;
*
forecast_state
=
ForecastState
::
NOT_VALID
;
return
false
;
}
...
...
modules/localization/msf/local_integ/measure_republish_process.cc
浏览文件 @
ef3725cd
...
...
@@ -146,7 +146,7 @@ bool MeasureRepublishProcess::NovatelBestgnssposProcess(
// If sins is align, we only need measure of xyz from bestgnsspos.
// If sins is not align, in order to init sins, we need
// (1) send a initial measure of xyz; (2) send measure of xyz and velocity.
// (1) send a
n
initial measure of xyz; (2) send measure of xyz and velocity.
if
(
is_sins_align
)
{
TransferXYZFromBestgnsspose
(
bestgnsspos_msg
,
measure
);
}
else
{
...
...
modules/localization/msf/local_map/base_map/base_map_cache.h
浏览文件 @
ef3725cd
...
...
@@ -71,7 +71,7 @@ class LRUCache {
int
Capacity
()
{
return
capacity_
;
}
protected:
/**@brief do something before remove a element from cache.
/**@brief do something before remove a
n
element from cache.
* Return true if the element can be removed. Return false if the element
* can't be removed. Then the cache will try to find another element to
* remove. */
...
...
@@ -80,7 +80,7 @@ class LRUCache {
private:
/**@brief The max caoacity of LRUCache. */
int
capacity_
;
/**@brief Increse the search speed in queue. */
/**@brief Incre
a
se the search speed in queue. */
std
::
map
<
Key
,
ListIterator
>
map_
;
/**@brief The least recently used queue. */
std
::
list
<
std
::
pair
<
Key
,
Element
*>>
list_
;
...
...
@@ -195,7 +195,7 @@ class MapNodeCacheL1 : public LRUCache<Key, MapNode> {
explicit
MapNodeCacheL1
(
int
capacity
)
:
LRUCache
<
Key
,
MapNode
>
(
capacity
)
{}
protected:
/**@brief do something before remove a element from cache.
/**@brief do something before remove a
n
element from cache.
* Return true if the element can be removed. Return false if the element
* can't be removed. Then the cache will try to find another element to
* remove. */
...
...
@@ -212,7 +212,7 @@ class MapNodeCacheL2 : public LRUCache<Key, MapNode> {
explicit
MapNodeCacheL2
(
int
capacity
)
:
LRUCache
<
Key
,
MapNode
>
(
capacity
)
{}
protected:
/**@brief do something before remove a element from cache.
/**@brief do something before remove a
n
element from cache.
* Return true if the element can be removed. Return false if the element
* can't be removed. Then the cache will try to find another element to
* remove. */
...
...
modules/localization/msf/local_map/lossless_map/lossless_map_matrix.h
浏览文件 @
ef3725cd
...
...
@@ -90,7 +90,7 @@ struct LosslessMapCell {
/**@brief Get the binary size of the object. */
unsigned
int
GetBinarySize
()
const
;
/**@brief Match a layer in the map cell given a altitude.
/**@brief Match a layer in the map cell given a
n
altitude.
* @return The valid layer ID is 1 ~ N (The layer 0 is the layer includes all
* the samples). If there is no existing layer, return 0. */
unsigned
int
GetLayerId
(
double
altitude
)
const
;
...
...
modules/localization/msf/local_map/ndt_map/ndt_map_matrix.h
浏览文件 @
ef3725cd
...
...
@@ -82,7 +82,7 @@ class NdtMapSingleCell {
Eigen
::
Matrix3f
centroid_average_cov_
;
/**@brief the pose inverse covariance of the cell. */
Eigen
::
Matrix3f
centroid_icov_
;
/**@brief the inverse covariance ava
li
able flag. */
/**@brief the inverse covariance ava
il
able flag. */
unsigned
char
is_icov_available_
=
0
;
/**@brief minimum number of points needed. */
const
unsigned
int
minimum_points_threshold_
=
6
;
...
...
modules/localization/ndt/localization_pose_buffer.cc
浏览文件 @
ef3725cd
...
...
@@ -46,7 +46,7 @@ void LocalizationPoseBuffer::UpdateLidarPose(
++
used_buffer_size_
;
has_initialized_
=
true
;
}
else
{
// add 10
h
z pose
// add 10
H
z pose
unsigned
int
empty_position
=
(
head_index_
+
used_buffer_size_
)
%
s_buffer_size_
;
lidar_poses_
[
empty_position
].
locator_pose
=
locator_pose
;
...
...
modules/localization/ndt/ndt_localization.cc
浏览文件 @
ef3725cd
...
...
@@ -50,8 +50,8 @@ void NDTLocalization::Init() {
FLAGS_map_dir
+
"/"
+
FLAGS_ndt_map_dir
+
"/"
+
FLAGS_local_map_name
;
AINFO
<<
"map folder: "
<<
map_path_
;
velodyne_extrinsic_
=
Eigen
::
Affine3d
::
Identity
();
bool
sucess
=
LoadLidarExtrinsic
(
lidar_extrinsics_file
,
&
velodyne_extrinsic_
);
if
(
!
sucess
)
{
bool
suc
c
ess
=
LoadLidarExtrinsic
(
lidar_extrinsics_file
,
&
velodyne_extrinsic_
);
if
(
!
suc
c
ess
)
{
AERROR
<<
"LocalizationLidar: Fail to access the lidar"
" extrinsic file: "
<<
lidar_extrinsics_file
;
...
...
@@ -63,8 +63,8 @@ void NDTLocalization::Init() {
<<
velodyne_extrinsic_
.
translation
().
z
()
<<
", "
<<
ext_quat
.
x
()
<<
", "
<<
ext_quat
.
y
()
<<
", "
<<
ext_quat
.
z
()
<<
", "
<<
ext_quat
.
w
();
sucess
=
LoadLidarHeight
(
lidar_height_file
,
&
lidar_height_
);
if
(
!
sucess
)
{
suc
c
ess
=
LoadLidarHeight
(
lidar_height_file
,
&
lidar_height_
);
if
(
!
suc
c
ess
)
{
AWARN
<<
"LocalizationLidar: Fail to load the lidar"
" height file: "
<<
lidar_height_file
<<
" Will use default value!"
;
...
...
modules/localization/ndt/ndt_locator/ndt_solver.h
浏览文件 @
ef3725cd
...
...
@@ -254,7 +254,7 @@ class NormalDistributionsTransform {
Eigen
::
Matrix
<
double
,
6
,
1
>
*
p
,
bool
ComputeHessian
=
true
);
/**@brief Compute individual point cont
ir
butions to derivatives of
/**@brief Compute individual point cont
ri
butions to derivatives of
* probability function w.r.t. the transformation vector. */
double
UpdateDerivatives
(
Eigen
::
Matrix
<
double
,
6
,
1
>
*
score_gradient
,
Eigen
::
Matrix
<
double
,
6
,
6
>
*
hessian
,
...
...
@@ -276,7 +276,7 @@ class NormalDistributionsTransform {
const
PointCloudSource
&
trans_cloud
,
Eigen
::
Matrix
<
double
,
6
,
1
>
*
p
);
/**@brief Compute individual point cont
ir
butions to hessian of probability
/**@brief Compute individual point cont
ri
butions to hessian of probability
* function. */
void
UpdateHessian
(
Eigen
::
Matrix
<
double
,
6
,
6
>
*
hessian
,
const
Eigen
::
Vector3d
&
x_trans
,
...
...
@@ -302,14 +302,14 @@ class NormalDistributionsTransform {
double
f_u
,
double
g_u
,
double
a_t
,
double
f_t
,
double
g_t
);
/**@brief Auxilary function used to determine endpoints of More-Thuente
/**@brief Auxil
i
ary function used to determine endpoints of More-Thuente
* interval. */
inline
double
AuxilaryFunctionPsimt
(
double
a
,
double
f_a
,
double
f_0
,
double
g_0
,
double
mu
=
1.e-4
)
{
return
(
f_a
-
f_0
-
mu
*
g_0
*
a
);
}
/**@brief Auxilary function derivative used to determine endpoints of
/**@brief Auxil
i
ary function derivative used to determine endpoints of
* More-Thuente interval. */
inline
double
AuxilaryFunctionDpsimt
(
double
g_a
,
double
g_0
,
double
mu
=
1.e-4
)
{
...
...
modules/localization/ndt/ndt_locator/ndt_solver.hpp
浏览文件 @
ef3725cd
...
...
@@ -312,7 +312,7 @@ void NormalDistributionsTransform<PointSource, PointTarget>::
}
loop_timer
.
End
(
"Ndt loop."
);
// Store transformation probability. The re
al
tive differences within each
// Store transformation probability. The re
la
tive differences within each
// scan registration are accurate but the normalization constants need to be
// modified for it to be globally accurate
trans_probability_
=
score
/
static_cast
<
double
>
(
input_
->
points
.
size
());
...
...
@@ -345,7 +345,7 @@ NormalDistributionsTransform<PointSource, PointTarget>::ComputeDerivatives(
for
(
size_t
idx
=
0
;
idx
<
input_
->
points
.
size
();
idx
++
)
{
x_trans_pt
=
trans_cloud
->
points
[
idx
];
// Find n
ie
ghbors (Radius search has been experimentally faster than
// Find n
ei
ghbors (Radius search has been experimentally faster than
// direct neighbor checking.
std
::
vector
<
TargetGridLeafConstPtr
>
neighborhood
;
std
::
vector
<
float
>
distances
;
...
...
@@ -502,7 +502,7 @@ NormalDistributionsTransform<PointSource, PointTarget>::UpdateDerivatives(
// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson
// 2009]
double
e_x_cov_x
=
exp
(
-
gauss_d2_
*
x_trans
.
dot
(
c_inv
*
x_trans
)
/
2
);
// Calculate probability of transtormed points exist
a
nce, Equation 6.9
// Calculate probability of transtormed points exist
e
nce, Equation 6.9
// [Magnusson 2009]
double
score_inc
=
-
gauss_d1_
*
e_x_cov_x
;
...
...
@@ -562,7 +562,7 @@ void NormalDistributionsTransform<PointSource, PointTarget>::ComputeHessian(
for
(
size_t
idx
=
0
;
idx
<
input_
->
points
.
size
();
idx
++
)
{
x_trans_pt
=
trans_cloud
.
points
[
idx
];
// Find n
ie
ghbors (Radius search has been experimentally faster than
// Find n
ei
ghbors (Radius search has been experimentally faster than
// direct neighbor checking.
std
::
vector
<
TargetGridLeafConstPtr
>
neighborhood
;
std
::
vector
<
float
>
distances
;
...
...
@@ -833,7 +833,7 @@ NormalDistributionsTransform<PointSource, PointTarget>::ComputeStepLengthMt(
while
(
!
interval_converged
&&
step_iterations
<
max_step_iterations
&&
!
(
psi_t
<=
0
/*Sufficient Decrease*/
&&
d_phi_t
<=
-
nu
*
d_phi_0
/*Curvature Condition*/
))
{
// Use auxilary function if interval I is not closed
// Use auxil
i
ary function if interval I is not closed
if
(
open_interval
)
{
a_t
=
TrialValueSelectionMt
(
a_l
,
f_l
,
g_l
,
a_u
,
f_u
,
g_u
,
a_t
,
psi_t
,
d_psi_t
);
...
...
modules/localization/ndt/ndt_locator/ndt_voxel_grid_covariance.h
浏览文件 @
ef3725cd
...
...
@@ -95,7 +95,7 @@ typedef Leaf *LeafPtr;
/**@brief Const pointer to VoxelGridCovariance leaf structure */
typedef
const
Leaf
*
LeafConstPtr
;
/**@brief A searchable voxel strucure containing the mean and covariance of the
/**@brief A searchable voxel struc
t
ure containing the mean and covariance of the
* data. */
template
<
typename
PointT
>
class
VoxelGridCovariance
{
...
...
@@ -253,7 +253,7 @@ class VoxelGridCovariance {
}
protected:
/**@brief Minimum points contained with in a voxel to allow it to be us
e
able.
/**@brief Minimum points contained with in a voxel to allow it to be usable.
*/
int
min_points_per_voxel_
;
...
...
@@ -261,7 +261,7 @@ class VoxelGridCovariance {
* less than a sufficient number of points). */
std
::
map
<
size_t
,
Leaf
>
leaves_
;
/**@brief Point cloud containing centroids of voxels containing atleast
/**@brief Point cloud containing centroids of voxels containing at
least
* minimum number of points. */
PointCloudPtr
voxel_centroids_
;
...
...
modules/localization/rtk/rtk_localization_test.cc
浏览文件 @
ef3725cd
...
...
@@ -41,7 +41,7 @@ class RTKLocalizationTest : public ::testing::Test {
};
TEST_F
(
RTKLocalizationTest
,
InterpolateIMU
)
{
// timestamp inbetween + time_diff is big enough(>0.001), interpolate
// timestamp in
between + time_diff is big enough(>0.001), interpolate
{
apollo
::
localization
::
CorrectedImu
imu1
;
load_data
(
"modules/localization/testdata/1_imu_1.pb.txt"
,
&
imu1
);
...
...
@@ -60,7 +60,7 @@ TEST_F(RTKLocalizationTest, InterpolateIMU) {
EXPECT_EQ
(
expected_result
.
DebugString
(),
imu
.
DebugString
());
}
// timestamp inbetween + time_diff is too small(<0.001), no interpolate
// timestamp in
between + time_diff is too small(<0.001), no interpolate
{
apollo
::
localization
::
CorrectedImu
imu1
;
load_data
(
"modules/localization/testdata/2_imu_1.pb.txt"
,
&
imu1
);
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录