Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
b22476e6
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,发现更多精彩内容 >>
提交
b22476e6
编写于
9月 22, 2018
作者:
L
Liangliang Zhang
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Perception: added secure_matrix and hungarian optimizer.
上级
8e9dd92c
变更
6
隐藏空白更改
内联
并排
Showing
6 changed file
with
1260 addition
and
5 deletion
+1260
-5
apollo.sh
apollo.sh
+13
-5
modules/perception/common/graph/BUILD
modules/perception/common/graph/BUILD
+44
-0
modules/perception/common/graph/hungarian_optimizer.h
modules/perception/common/graph/hungarian_optimizer.h
+730
-0
modules/perception/common/graph/hungarian_optimizer_test.cc
modules/perception/common/graph/hungarian_optimizer_test.cc
+311
-0
modules/perception/common/graph/secure_matrix.h
modules/perception/common/graph/secure_matrix.h
+91
-0
modules/perception/common/graph/secure_matrix_test.cc
modules/perception/common/graph/secure_matrix_test.cc
+71
-0
未找到文件。
apollo.sh
浏览文件 @
b22476e6
...
...
@@ -117,8 +117,7 @@ function generate_build_targets() {
`
bazel query //modules/localization/msf/local_tool/local_visualization/...
`
//modules/map/...
//modules/monitor/...
//modules/perception/common/...
//modules/perception/proto/...
//modules/perception/...
//modules/planning/...
//modules/prediction/...
//modules/routing/...
...
...
@@ -209,17 +208,26 @@ function cibuild() {
info
"Building with
$JOB_ARG
for
$MACHINE_ARCH
"
BUILD_TARGETS
=
"
//modules/canbus/...
//modules/common/...
//modules/control/...
//modules/dreamview/...
//modules/drivers/radar/conti_radar/...
//modules/drivers/gnss/...
//modules/drivers/velodyne/...
//modules/drivers/usb_cam/...
//modules/drivers/radar/ultrasonic_radar/...
//modules/guardian/...
//modules/localization/proto/...
//modules/localization/rtk/...
`
bazel query //modules/localization/msf/... except //modules/localization/msf/local_tool/...
`
`
bazel query //modules/localization/msf/local_tool/local_visualization/...
`
//modules/
perception/common
/...
//modules/perception/
proto/
...
//modules/
map
/...
//modules/perception/...
//modules/planning/...
//modules/prediction/...
//modules/routing/..."
//modules/routing/...
//modules/third_party_perception/..."
bazel build
$JOB_ARG
$DEFINES
$@
$BUILD_TARGETS
...
...
modules/perception/common/graph/BUILD
浏览文件 @
b22476e6
...
...
@@ -50,4 +50,48 @@ cc_test(
],
)
cc_library
(
name
=
"secure_matrix"
,
hdrs
=
[
"secure_matrix.h"
,
],
deps
=
[
"@eigen"
,
],
)
cc_test
(
name
=
"secure_matrix_test"
,
size
=
"small"
,
srcs
=
[
"secure_matrix_test.cc"
,
],
deps
=
[
":secure_matrix"
,
"@gtest//:main"
,
"@eigen"
,
],
)
cc_library
(
name
=
"hungarian_optimizer"
,
hdrs
=
[
"hungarian_optimizer.h"
,
],
deps
=
[
"@eigen"
,
],
)
cc_test
(
name
=
"hungarian_optimizer_test"
,
size
=
"small"
,
srcs
=
[
"hungarian_optimizer_test.cc"
,
],
deps
=
[
":hungarian_optimizer"
,
":secure_matrix"
,
"@gtest//:main"
,
],
)
cpplint
()
modules/perception/common/graph/hungarian_optimizer.h
0 → 100644
浏览文件 @
b22476e6
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_COMMON_GRAPH_HUNGARIAN_OPTIMIZER_H_
#define MODULES_PERCEPTION_COMMON_GRAPH_HUNGARIAN_OPTIMIZER_H_
#include <algorithm>
#include <cstdio>
#include <functional>
#include <limits>
#include <utility>
#include <vector>
#include "Eigen/Dense"
#include "modules/perception/common/graph/secure_matrix.h"
namespace
apollo
{
namespace
perception
{
namespace
common
{
template
<
typename
T
>
class
HungarianOptimizer
{
static
const
int
kHungarianOptimizerRowNotFound
=
-
1
;
static
const
int
kHungarianOptimizerColNotFound
=
-
2
;
public:
/* Setup the initial conditions for the algorithm. */
HungarianOptimizer
();
explicit
HungarianOptimizer
(
const
int
max_optimization_size
);
~
HungarianOptimizer
()
{}
SecureMat
<
T
>*
costs
()
{
return
&
costs_
;
}
T
*
costs
(
const
size_t
row
,
const
size_t
col
)
{
return
&
(
costs_
(
row
,
col
));
}
void
Maximize
(
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>*
assignments
);
void
Minimize
(
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>*
assignments
);
void
OptimizationInit
();
void
OptimizationClear
();
/* Print the matrix to stdout (for debugging.) */
void
PrintMatrix
();
private:
enum
class
Mark
{
NONE
,
PRIME
,
STAR
};
/* Convert the final cost matrix into a set of assignments of agents to tasks.
* Return the assignment in a vector of pair, the same as Minimize() and
* Maximize() */
void
FindAssignments
(
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>*
assignments
);
/* Is the cell (row, col) starred? */
bool
IsStarred
(
const
size_t
row
,
const
size_t
col
)
const
{
return
marks_
(
row
,
col
)
==
Mark
::
STAR
;
}
/* Mark cell (row, col) with a star */
void
Star
(
const
size_t
row
,
const
size_t
col
)
{
marks_
(
row
,
col
)
=
Mark
::
STAR
;
++
stars_in_col_
[
col
];
}
/* Remove a star from cell (row, col) */
void
Unstar
(
const
size_t
row
,
const
size_t
col
)
{
marks_
(
row
,
col
)
=
Mark
::
NONE
;
--
stars_in_col_
[
col
];
}
/* Find a column in row 'row' containing a star, or return
* kHungarianOptimizerColNotFound if no such column exists. */
int
FindStarInRow
(
const
size_t
row
)
const
;
/* Find a row in column 'col' containing a star, or return
* kHungarianOptimizerRowNotFound if no such row exists. */
int
FindStarInCol
(
const
size_t
col
)
const
;
/* Is cell (row, col) marked with a prime? */
bool
IsPrimed
(
const
size_t
row
,
const
size_t
col
)
const
{
return
marks_
(
row
,
col
)
==
Mark
::
PRIME
;
}
/* Mark cell (row, col) with a prime. */
void
Prime
(
const
size_t
row
,
const
size_t
col
)
{
marks_
(
row
,
col
)
=
Mark
::
PRIME
;
}
/* Find a column in row containing a prime, or return
* kHungarianOptimizerColNotFound if no such column exists. */
int
FindPrimeInRow
(
const
size_t
row
)
const
;
/* Remove the prime marks_ from every cell in the matrix. */
void
ClearPrimes
();
/* Does column col contain a star? */
bool
ColContainsStar
(
const
size_t
col
)
const
{
return
stars_in_col_
[
col
]
>
0
;
}
/* Is row 'row' covered? */
bool
RowCovered
(
const
size_t
row
)
const
{
return
rows_covered_
[
row
];
}
/* Cover row 'row'. */
void
CoverRow
(
const
size_t
row
)
{
rows_covered_
[
row
]
=
true
;
}
/* Uncover row 'row'. */
void
UncoverRow
(
const
size_t
row
)
{
rows_covered_
[
row
]
=
false
;
}
/* Is column col covered? */
bool
ColCovered
(
const
size_t
col
)
const
{
return
cols_covered_
[
col
];
}
/* Cover column col. */
void
CoverCol
(
const
size_t
col
)
{
cols_covered_
[
col
]
=
true
;
}
/* Uncover column col. */
void
UncoverCol
(
const
size_t
col
)
{
cols_covered_
[
col
]
=
false
;
}
/* Uncover ever row and column in the matrix. */
void
ClearCovers
();
/* Find the smallest uncovered cell in the matrix. */
T
FindSmallestUncovered
();
/* Find an uncovered zero and store its coordinates in (zeroRow_, zeroCol_)
* and return true, or return false if no such cell exists. */
bool
FindZero
(
size_t
*
zero_row
,
size_t
*
zero_col
);
/* Run the Munkres algorithm! */
void
DoMunkres
();
void
CheckStar
();
/* Step 1:
* For each row of the matrix, find the smallest element and subtract it from
* every element in its row. Go to Step 2. */
void
ReduceRows
();
/* Step 2:
* Find a zero (Z) in the matrix. If there is no starred zero in its row or
* column, star Z. Repeat for every element in the matrix. Go to Step 3.
* Note: profiling shows this method to use 9.2% of the CPU - the next
* slowest step takes 0.6%. It is hard to find a way further speed it up. */
void
StarZeroes
();
/* Step 3:
* Cover each column containing a starred zero. If all columns are covered,
* the starred zeros describe a complete set of unique assignments.
* In this case, terminate the algorithm. Otherwise, go to Step 4. */
void
CoverStarredZeroes
();
/* Step 4:
* Find a noncovered zero and prime it. If there is no starred zero in the
* row containing this primed zero, Go to Step 5. Otherwise, cover this row
* and uncover the column containing the starred zero. Continue in this manner
* until there are no uncovered zeros left, then go to Step 6. */
void
PrimeZeroes
();
/* Step 5:
* Construct a series of alternating primed and starred zeros as follows.
* Let Z0 represent the uncovered primed zero found in Step 4. Let Z1 denote
* the starred zero in the column of Z0 (if any). Let Z2 denote the primed
* zero in the row of Z1 (there will always be one). Continue until the
* series terminates at a primed zero that has no starred zero in its column.
* Unstar each starred zero of the series, star each primed zero of the
* series, erase all primes and uncover every line in the matrix. Return to
* Step 3. */
void
MakeAugmentingPath
();
/* Step 6:
* Add the smallest uncovered value in the matrix to every element of each
* covered row, and subtract it from every element of each uncovered column.
* Return to Step 4 without altering any stars, primes, or covered lines. */
void
AugmentPath
();
/* the max optimization size set to control memory */
int
max_optimization_size_
=
1000
;
/* status of optimization initialization */
bool
optimization_initialized_
=
false
;
/* the size of the problem, i.e. std::max(#agents, #tasks). */
int
matrix_size_
=
0
;
/* the expanded cost matrix. */
SecureMat
<
T
>
costs_
;
/* the greatest cost in the initial cost matrix. */
T
max_cost_
{
0
};
/* which rows and columns are currently covered. */
std
::
vector
<
bool
>
rows_covered_
;
std
::
vector
<
bool
>
cols_covered_
;
/* the marks (star/prime/none) on each element of the cost matrix. */
SecureMat
<
Mark
>
marks_
;
/* the number of stars in each column - used to speed up coverStarredZeroes.*/
std
::
vector
<
int
>
stars_in_col_
;
/* representation of a path_ through the matrix - used in Step 5. */
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>
assignments_
;
/* the locations of a zero found in Step 4. */
int
zero_col_
=
0
;
int
zero_row_
=
0
;
/* the width_ and height_ of the initial (non-expanded) cost matrix. */
int
width_
=
0
;
int
height_
=
0
;
/* The current state of the algorithm */
std
::
function
<
void
()
>
fn_state_
=
nullptr
;
std
::
vector
<
size_t
>
uncov_col_
;
std
::
vector
<
size_t
>
uncov_row_
;
};
// class HungarianOptimizer
template
<
typename
T
>
HungarianOptimizer
<
T
>::
HungarianOptimizer
()
:
HungarianOptimizer
(
1000
)
{}
template
<
typename
T
>
HungarianOptimizer
<
T
>::
HungarianOptimizer
(
const
int
max_optimization_size
)
:
max_optimization_size_
(
max_optimization_size
)
{
costs_
.
Reserve
(
max_optimization_size
,
max_optimization_size
);
stars_in_col_
.
reserve
(
max_optimization_size
);
rows_covered_
.
reserve
(
max_optimization_size
);
cols_covered_
.
reserve
(
max_optimization_size
);
assignments_
.
reserve
(
max_optimization_size
);
uncov_row_
.
reserve
(
max_optimization_size
);
uncov_col_
.
reserve
(
max_optimization_size
);
}
/* Find an assignment which maximizes the overall costs.
* Return an array of pairs of integers. Each pair (i, j) corresponds to
* assigning agent i to task j. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
Maximize
(
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>*
assignments
)
{
OptimizationInit
();
/* operate maximizing problem as a minimizing one via substrating original
* cost from max_cost_ */
for
(
size_t
row
=
0
;
row
<
height_
;
++
row
)
{
for
(
size_t
col
=
0
;
col
<
width_
;
++
col
)
{
costs_
(
row
,
col
)
=
max_cost_
-
costs_
(
row
,
col
);
}
}
Minimize
(
assignments
);
}
/* Find an assignment which minimizes the overall costs.
* Return an array of pairs of integers. Each pair (i, j) corresponds to
* assinging agent i to task j. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
Minimize
(
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>*
assignments
)
{
OptimizationInit
();
DoMunkres
();
FindAssignments
(
assignments
);
OptimizationClear
();
}
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
OptimizationInit
()
{
if
(
optimization_initialized_
)
{
return
;
}
width_
=
costs_
.
width
();
if
(
width_
>
0
)
{
height_
=
costs_
.
height
();
}
else
{
height_
=
0
;
}
matrix_size_
=
std
::
max
(
height_
,
width_
);
max_cost_
=
0
;
/* generate the expanded cost matrix by adding extra 0s in order to make a
* square matrix. Meanwhile, find the max cost in the matrix. It may be used
* later, if we want to maximizing rather than minimizing the overall costs.*/
costs_
.
Resize
(
matrix_size_
,
matrix_size_
);
for
(
size_t
row
=
0
;
row
<
matrix_size_
;
++
row
)
{
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
if
((
row
>=
height_
)
||
(
col
>=
width_
))
{
costs_
(
row
,
col
)
=
0
;
}
else
{
max_cost_
=
std
::
max
(
max_cost_
,
costs_
(
row
,
col
));
}
}
}
/* initially, none of the cells of the matrix are marked. */
marks_
.
Resize
(
matrix_size_
,
matrix_size_
);
for
(
size_t
row
=
0
;
row
<
matrix_size_
;
++
row
)
{
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
marks_
(
row
,
col
)
=
Mark
::
NONE
;
}
}
stars_in_col_
.
assign
(
matrix_size_
,
0
);
rows_covered_
.
assign
(
matrix_size_
,
false
);
cols_covered_
.
assign
(
matrix_size_
,
false
);
assignments_
.
resize
(
matrix_size_
*
2
);
optimization_initialized_
=
true
;
}
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
OptimizationClear
()
{
optimization_initialized_
=
false
;
}
/* Convert the final costs matrix into a set of assignments of agents to tasks.
* Return an array of pairs of integers, the same as the return values of
* Minimize() and Maximize() */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
FindAssignments
(
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>*
assignments
)
{
assignments
->
clear
();
for
(
size_t
row
=
0
;
row
<
height_
;
++
row
)
{
for
(
size_t
col
=
0
;
col
<
width_
;
++
col
)
{
if
(
IsStarred
(
row
,
col
))
{
assignments
->
push_back
(
std
::
make_pair
(
row
,
col
));
break
;
}
}
}
}
/* Find a column in row 'row' containing a star, or return
* kHungarianOptimizerColNotFound if no such column exists. */
template
<
typename
T
>
int
HungarianOptimizer
<
T
>::
FindStarInRow
(
const
size_t
row
)
const
{
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
if
(
IsStarred
(
row
,
col
))
{
return
col
;
}
}
return
kHungarianOptimizerColNotFound
;
}
/* Find a row in column 'col' containing a star, or return
* kHungarianOptimizerRowNotFound if no such row exists. */
template
<
typename
T
>
int
HungarianOptimizer
<
T
>::
FindStarInCol
(
const
size_t
col
)
const
{
if
(
!
ColContainsStar
(
col
))
{
return
kHungarianOptimizerRowNotFound
;
}
for
(
size_t
row
=
0
;
row
<
matrix_size_
;
++
row
)
{
if
(
IsStarred
(
row
,
col
))
{
return
row
;
}
}
// NOT REACHED
return
kHungarianOptimizerRowNotFound
;
}
/* Find a column in row containing a prime, or return
* kHungarianOptimizerColNotFound if no such column exists. */
template
<
typename
T
>
int
HungarianOptimizer
<
T
>::
FindPrimeInRow
(
const
size_t
row
)
const
{
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
if
(
IsPrimed
(
row
,
col
))
{
return
col
;
}
}
return
kHungarianOptimizerColNotFound
;
}
/* Remove the prime marks from every cell in the matrix. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
ClearPrimes
()
{
for
(
size_t
row
=
0
;
row
<
matrix_size_
;
++
row
)
{
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
if
(
IsPrimed
(
row
,
col
))
{
marks_
(
row
,
col
)
=
Mark
::
NONE
;
}
}
}
}
/* Uncover every row and column in the matrix. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
ClearCovers
()
{
for
(
size_t
x
=
0
;
x
<
matrix_size_
;
x
++
)
{
UncoverRow
(
x
);
UncoverCol
(
x
);
}
}
/* Find the smallest uncovered cell in the matrix. */
template
<
typename
T
>
T
HungarianOptimizer
<
T
>::
FindSmallestUncovered
()
{
T
minval
=
std
::
numeric_limits
<
T
>::
max
();
uncov_col_
.
clear
();
uncov_row_
.
clear
();
for
(
size_t
i
=
0
;
i
<
matrix_size_
;
++
i
)
{
if
(
!
RowCovered
(
i
))
{
uncov_row_
.
push_back
(
i
);
}
if
(
!
ColCovered
(
i
))
{
uncov_col_
.
push_back
(
i
);
}
}
for
(
size_t
row
=
0
;
row
<
uncov_row_
.
size
();
++
row
)
{
for
(
size_t
col
=
0
;
col
<
uncov_col_
.
size
();
++
col
)
{
minval
=
std
::
min
(
minval
,
costs_
(
uncov_row_
[
row
],
uncov_col_
[
col
]));
}
}
return
minval
;
}
/* Find an uncovered zero and store its coordinates in (zeroRow, zeroCol)
* and return true, or return false if no such cell exists. */
template
<
typename
T
>
bool
HungarianOptimizer
<
T
>::
FindZero
(
size_t
*
zero_row
,
size_t
*
zero_col
)
{
uncov_col_
.
clear
();
uncov_row_
.
clear
();
for
(
int
i
=
0
;
i
<
matrix_size_
;
++
i
)
{
if
(
!
RowCovered
(
i
))
{
uncov_row_
.
push_back
(
i
);
}
if
(
!
ColCovered
(
i
))
{
uncov_col_
.
push_back
(
i
);
}
}
if
(
uncov_row_
.
empty
()
||
uncov_col_
.
empty
())
{
return
false
;
}
for
(
size_t
i
=
0
;
i
<
uncov_row_
.
size
();
++
i
)
{
for
(
size_t
j
=
0
;
j
<
uncov_col_
.
size
();
++
j
)
{
if
(
costs_
(
uncov_row_
[
i
],
uncov_col_
[
j
])
==
0
)
{
*
zero_row
=
uncov_row_
[
i
];
*
zero_col
=
uncov_col_
[
j
];
return
true
;
}
}
}
return
false
;
}
/* Print the matrix to stdout (for debugging). */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
PrintMatrix
()
{
for
(
size_t
row
=
0
;
row
<
matrix_size_
;
++
row
)
{
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
printf
(
"%g "
,
costs_
(
row
,
col
));
if
(
IsStarred
(
row
,
col
))
{
printf
(
"*"
);
}
if
(
IsPrimed
(
row
,
col
))
{
printf
(
"'"
);
}
}
printf
(
"
\n
"
);
}
}
/* Run the Munkres algorithm */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
DoMunkres
()
{
int
max_num_iter
=
1000
;
int
num_iter
=
0
;
fn_state_
=
std
::
bind
(
&
HungarianOptimizer
::
ReduceRows
,
this
);
while
(
fn_state_
!=
nullptr
&&
num_iter
<
max_num_iter
)
{
fn_state_
();
++
num_iter
;
}
if
(
num_iter
>=
max_num_iter
)
{
CheckStar
();
}
}
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
CheckStar
()
{
for
(
size_t
row
=
0
;
row
<
height_
;
++
row
)
{
int
star_col
=
-
1
;
bool
is_single
=
true
;
for
(
int
col
=
0
;
col
<
width_
;
++
col
)
{
if
(
IsStarred
(
row
,
col
))
{
if
(
star_col
==
-
1
)
{
star_col
=
col
;
}
else
{
is_single
=
false
;
break
;
}
}
}
if
(
!
is_single
)
{
for
(
int
col
=
0
;
col
<
width_
;
++
col
)
{
Unstar
(
row
,
col
);
}
}
}
}
/* Step 1:
* For each row of the matrix, find the smallest element and substract it
* from every element in its row. Then, go to Step 2. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
ReduceRows
()
{
for
(
size_t
row
=
0
;
row
<
matrix_size_
;
++
row
)
{
T
min_cost
=
costs_
(
row
,
0
);
for
(
size_t
col
=
1
;
col
<
matrix_size_
;
++
col
)
{
min_cost
=
std
::
min
(
min_cost
,
costs_
(
row
,
col
));
}
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
costs_
(
row
,
col
)
-=
min_cost
;
}
}
fn_state_
=
std
::
bind
(
&
HungarianOptimizer
::
StarZeroes
,
this
);
}
/* Step 2:
* Find a zero Z in the matrix. If there is no starred zero in its row
* or column, star Z. Repeat for every element in the matrix. Then, go to
* Step3. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
StarZeroes
()
{
/* since no rows or columns are covered on entry to this step, we use the
* covers as a quick way of making which rows & columns have stars in them */
for
(
size_t
row
=
0
;
row
<
matrix_size_
;
++
row
)
{
if
(
RowCovered
(
row
))
{
continue
;
}
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
if
(
ColCovered
(
col
))
{
continue
;
}
if
(
costs_
(
row
,
col
)
==
0
)
{
Star
(
row
,
col
);
CoverRow
(
row
);
CoverCol
(
col
);
break
;
}
}
}
ClearCovers
();
fn_state_
=
std
::
bind
(
&
HungarianOptimizer
::
CoverStarredZeroes
,
this
);
}
/* Step 3:
* Cover each column containing a starred zero. If all columns are covered,
* the starred zeros describe a complete set of unique assignments. In this
* case, terminate the algorithm. Otherwise, go to Step 4. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
CoverStarredZeroes
()
{
size_t
num_covered
=
0
;
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
if
(
ColContainsStar
(
col
))
{
CoverCol
(
col
);
num_covered
++
;
}
}
if
(
num_covered
>=
matrix_size_
)
{
fn_state_
=
nullptr
;
return
;
}
fn_state_
=
std
::
bind
(
&
HungarianOptimizer
::
PrimeZeroes
,
this
);
}
/* Step 4:
* Find a noncovered zero and prime it. If there is no starred zero in the
* row containing this primed zero, go to Step 5. Otherwise, cover this row
* and uncover the column containing the starred zero. Continue in this manner
* until there are no uncovered zeros left, then go to Step 6. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
PrimeZeroes
()
{
// this loop is guaranteed to terminate in at most matrix_size_ iterations,
// as FindZero() returns a location only if there is at least one uncovered
// zero in the matrix. Each iteration, either one row is covered or the
// loop terminates. Since there are matrix_size_ rows, after that many
// iterations there are no uncovered cells and hence no uncovered zeroes,
// so the loop terminates.
for
(;;)
{
size_t
zero_row
=
0
;
size_t
zero_col
=
0
;
if
(
!
FindZero
(
&
zero_row
,
&
zero_col
))
{
// No uncovered zeroes.
fn_state_
=
std
::
bind
(
&
HungarianOptimizer
::
AugmentPath
,
this
);
return
;
}
Prime
(
zero_row
,
zero_col
);
size_t
star_col
=
FindStarInRow
(
zero_row
);
if
(
star_col
!=
kHungarianOptimizerColNotFound
)
{
CoverRow
(
zero_row
);
UncoverCol
(
star_col
);
}
else
{
std
::
pair
<
size_t
,
size_t
>
first_assignment
=
std
::
make_pair
(
zero_row
,
zero_col
);
assignments_
[
0
]
=
first_assignment
;
fn_state_
=
std
::
bind
(
&
HungarianOptimizer
::
MakeAugmentingPath
,
this
);
return
;
}
}
}
/* Step 5:
* Construct a series of alternating primed and starred zeros as follows.
* Let Z0 represent the uncovered primed zero found in Step 4. Let Z1 denote
* the starred zero in the column of Z0 (if any). Let Z2 denote the primed
* zero in the row of Z1 (there will always be one). Continue until the
* series terminates at a primed zero that has no starred zero in its column.
* unstar each starred zero of the series, star each primed zero of the
* series, erase all primes and uncover every line in the matrix. Return to
* Step 3. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
MakeAugmentingPath
()
{
bool
done
=
false
;
size_t
count
=
0
;
/* note: this loop is guaranteed to terminate within matrix_size_ iterations
* because:
* 1) on entry to this step, there is at least 1 column with no starred zero
* (otherwise we would have terminated the algorithm already.)
* 2) each row containing a star also contains exactly one primed zero.
* 4) each column contains at most one starred zero.
*
* Since the path_ we construct visits primed and starred zeroes alternately,
* and terminates if we reach a primed zero in a column with no star, our
* path_ must either contain matrix_size_ or fewer stars (in which case the
* loop iterates fewer than matrix_size_ times), or it contains more. In
* that case, because (1) implies that there are fewer than matrix_size_
* stars, we must have visited at least one star more than once. Consider
* the first such star that we visit more than once; it must have been reached
* immediately after visiting a prime in the same row. By (2), this prime
* is unique and so must have also been visited more than once.
* Therefore, that prime must be in the same column as a star that has been
* visited more than once, contradicting the assumption that we chose the
* first multiply visited star, or it must be in the same column as more
* than one star, contradicting (3). Therefore, we never visit any star
* more than once and the loop terminates within matrix_size_ iterations. */
while
(
!
done
)
{
/* first construct the alternating path... */
size_t
row
=
FindStarInCol
(
assignments_
[
count
].
second
);
if
(
row
!=
kHungarianOptimizerRowNotFound
)
{
count
++
;
assignments_
[
count
].
first
=
row
;
assignments_
[
count
].
second
=
assignments_
[
count
-
1
].
second
;
}
else
{
done
=
true
;
}
if
(
!
done
)
{
size_t
col
=
FindPrimeInRow
(
assignments_
[
count
].
first
);
count
++
;
assignments_
[
count
].
first
=
assignments_
[
count
-
1
].
first
;
assignments_
[
count
].
second
=
col
;
}
}
/* then, modify it. */
for
(
size_t
i
=
0
;
i
<=
count
;
++
i
)
{
size_t
row
=
assignments_
[
i
].
first
;
size_t
col
=
assignments_
[
i
].
second
;
if
(
IsStarred
(
row
,
col
))
{
Unstar
(
row
,
col
);
}
else
{
Star
(
row
,
col
);
}
}
ClearCovers
();
ClearPrimes
();
fn_state_
=
std
::
bind
(
&
HungarianOptimizer
::
CoverStarredZeroes
,
this
);
}
/* Step 6:
* Add the smallest uncovered value in the matrix to every element of each
* covered row, and subtract it from every element of each uncovered column.
* Return to Step 4 without altering any stars, primes, or covered lines. */
template
<
typename
T
>
void
HungarianOptimizer
<
T
>::
AugmentPath
()
{
T
minval
=
FindSmallestUncovered
();
for
(
size_t
row
=
0
;
row
<
matrix_size_
;
++
row
)
{
if
(
RowCovered
(
row
))
{
for
(
size_t
c
=
0
;
c
<
matrix_size_
;
++
c
)
{
costs_
(
row
,
c
)
+=
minval
;
}
}
}
for
(
size_t
col
=
0
;
col
<
matrix_size_
;
++
col
)
{
if
(
!
ColCovered
(
col
))
{
for
(
size_t
r
=
0
;
r
<
matrix_size_
;
++
r
)
{
costs_
(
r
,
col
)
-=
minval
;
}
}
}
fn_state_
=
std
::
bind
(
&
HungarianOptimizer
::
PrimeZeroes
,
this
);
}
}
// namespace common
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_COMMON_GRAPH_HUNGARIAN_OPTIMIZER_H_
modules/perception/common/graph/hungarian_optimizer_test.cc
0 → 100644
浏览文件 @
b22476e6
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/perception/common/graph/hungarian_optimizer.h"
#include "Eigen/Core"
#include "gtest/gtest.h"
namespace
apollo
{
namespace
perception
{
namespace
common
{
class
HungarianOptimizerTest
:
public
testing
::
Test
{
public:
HungarianOptimizerTest
()
:
optimizer_
(
nullptr
)
{}
~
HungarianOptimizerTest
()
{}
protected:
void
SetUp
()
{
optimizer_
=
new
HungarianOptimizer
<
float
>
();
}
void
TearDown
()
{
delete
optimizer_
;
optimizer_
=
nullptr
;
}
public:
HungarianOptimizer
<
float
>*
optimizer_
;
};
// class HungarianOptimizerTest
TEST_F
(
HungarianOptimizerTest
,
test_Minimize
)
{
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>
assignments
;
optimizer_
->
costs
()
->
Reserve
(
1000
,
1000
);
/* case 1: most basic one
* costs:
* 0.1, 1.0
* 1.0, 0.1
* matches:
* (0->0, 1->1) */
optimizer_
->
costs
()
->
Resize
(
2
,
2
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
0.1
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
1.0
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
1.0
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
0.1
;
optimizer_
->
Minimize
(
&
assignments
);
optimizer_
->
PrintMatrix
();
EXPECT_EQ
(
2
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
0
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
1
,
assignments
[
1
].
second
);
/* case 2: special one with all 0s
* costs:
* 0.0, 0.0
* 0.0, 0.0
* matches:
* (0->0, 1->1) */
optimizer_
->
costs
()
->
Resize
(
2
,
2
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
0.0
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
0.0
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
0.0
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
0.0
;
optimizer_
->
Minimize
(
&
assignments
);
EXPECT_EQ
(
2
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
0
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
1
,
assignments
[
1
].
second
);
/* case 3: special one with all the same values
* costs:
* 3.0, 3.0
* 3.0, 3.0
* matches:
* (0->0, 1->1) */
optimizer_
->
costs
()
->
Resize
(
2
,
2
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
3.0
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
3.0
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
3.0
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
3.0
;
optimizer_
->
Minimize
(
&
assignments
);
EXPECT_EQ
(
2
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
0
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
1
,
assignments
[
1
].
second
);
/* case 4: further one with more cols
* costs:
* 4.7, 3.8, 1.0, 2.0
* 4.1, 3.0, 2.0, 3.0
* 1.0, 2.0, 4.7, 4.9
* matches:
* (0->2, 1->1, 2->0) */
optimizer_
->
costs
()
->
Resize
(
3
,
4
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
4.7
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
3.8
;
(
*
optimizer_
->
costs
())(
0
,
2
)
=
1.0
;
(
*
optimizer_
->
costs
())(
0
,
3
)
=
2.0
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
4.1
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
3.0
;
(
*
optimizer_
->
costs
())(
1
,
2
)
=
2.0
;
(
*
optimizer_
->
costs
())(
1
,
3
)
=
3.0
;
(
*
optimizer_
->
costs
())(
2
,
0
)
=
1.0
;
(
*
optimizer_
->
costs
())(
2
,
1
)
=
2.0
;
(
*
optimizer_
->
costs
())(
2
,
2
)
=
4.7
;
(
*
optimizer_
->
costs
())(
2
,
3
)
=
4.9
;
optimizer_
->
Minimize
(
&
assignments
);
EXPECT_EQ
(
3
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
2
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
1
,
assignments
[
1
].
second
);
EXPECT_EQ
(
2
,
assignments
[
2
].
first
);
EXPECT_EQ
(
0
,
assignments
[
2
].
second
);
/* case 5: further one with more rows
* costs:
* 4.7, 3.8, 1.0
* 4.1, 3.0, 2.0
* 1.0, 2.0, 4.7
* 3.2, 2.1, 0.5
* matches:
* (0->2, 2->0, 3->1) */
optimizer_
->
costs
()
->
Resize
(
4
,
3
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
4.7
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
3.8
;
(
*
optimizer_
->
costs
())(
0
,
2
)
=
1.0
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
4.1
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
3.0
;
(
*
optimizer_
->
costs
())(
1
,
2
)
=
2.0
;
(
*
optimizer_
->
costs
())(
2
,
0
)
=
1.0
;
(
*
optimizer_
->
costs
())(
2
,
1
)
=
2.0
;
(
*
optimizer_
->
costs
())(
2
,
2
)
=
4.7
;
(
*
optimizer_
->
costs
())(
3
,
0
)
=
3.2
;
(
*
optimizer_
->
costs
())(
3
,
1
)
=
2.1
;
(
*
optimizer_
->
costs
())(
3
,
2
)
=
0.5
;
optimizer_
->
Minimize
(
&
assignments
);
EXPECT_EQ
(
3
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
2
,
assignments
[
0
].
second
);
EXPECT_EQ
(
2
,
assignments
[
1
].
first
);
EXPECT_EQ
(
0
,
assignments
[
1
].
second
);
EXPECT_EQ
(
3
,
assignments
[
2
].
first
);
EXPECT_EQ
(
1
,
assignments
[
2
].
second
);
/* case 6: empty one */
optimizer_
->
costs
()
->
Resize
(
0
,
0
);
optimizer_
->
Minimize
(
&
assignments
);
EXPECT_EQ
(
0
,
assignments
.
size
());
}
TEST_F
(
HungarianOptimizerTest
,
test_Maximize
)
{
std
::
vector
<
std
::
pair
<
size_t
,
size_t
>>
assignments
;
optimizer_
->
costs
()
->
Reserve
(
1000
,
1000
);
/* case 1: most basic one
* costs:
* 0.1, 1.0
* 1.0, 0.1
* matches:
* (0->1, 1->0) */
optimizer_
->
costs
()
->
Resize
(
2
,
2
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
0.1
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
1.0
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
1.0
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
0.1
;
optimizer_
->
Maximize
(
&
assignments
);
EXPECT_EQ
(
2
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
1
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
0
,
assignments
[
1
].
second
);
/* case 2: special one with all 0s
* costs:
* 0.0, 0.0
* 0.0, 0.0
* matches:
* (0->0, 1->1) */
optimizer_
->
costs
()
->
Resize
(
2
,
2
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
0.0
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
0.0
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
0.0
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
0.0
;
optimizer_
->
Maximize
(
&
assignments
);
EXPECT_EQ
(
2
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
0
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
1
,
assignments
[
1
].
second
);
/* case 3: special one with all the same values
* costs:
* 3.0, 3.0
* 3.0, 3.0
* matches:
* (0->0, 1->1) */
optimizer_
->
costs
()
->
Resize
(
2
,
2
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
3.0
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
3.0
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
3.0
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
3.0
;
optimizer_
->
Maximize
(
&
assignments
);
EXPECT_EQ
(
2
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
0
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
1
,
assignments
[
1
].
second
);
/* case 4: further one with more cols
* costs:
* 4.7, 3.8, 1.0, 2.0
* 4.1, 3.0, 2.0, 3.0
* 1.0, 2.0, 4.7, 4.9
* matches:
* (0->1, 1->0, 2->3) */
optimizer_
->
costs
()
->
Resize
(
3
,
4
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
4.7
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
3.8
;
(
*
optimizer_
->
costs
())(
0
,
2
)
=
1.0
;
(
*
optimizer_
->
costs
())(
0
,
3
)
=
2.0
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
4.1
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
3.0
;
(
*
optimizer_
->
costs
())(
1
,
2
)
=
2.0
;
(
*
optimizer_
->
costs
())(
1
,
3
)
=
3.0
;
(
*
optimizer_
->
costs
())(
2
,
0
)
=
1.0
;
(
*
optimizer_
->
costs
())(
2
,
1
)
=
2.0
;
(
*
optimizer_
->
costs
())(
2
,
2
)
=
4.7
;
(
*
optimizer_
->
costs
())(
2
,
3
)
=
4.9
;
optimizer_
->
Maximize
(
&
assignments
);
EXPECT_EQ
(
3
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
1
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
0
,
assignments
[
1
].
second
);
EXPECT_EQ
(
2
,
assignments
[
2
].
first
);
EXPECT_EQ
(
3
,
assignments
[
2
].
second
);
/* case 5: further one with more rows
* costs:
* 4.7, 3.8, 1.0
* 4.1, 3.0, 2.0
* 1.0, 2.0, 4.7
* 3.2, 2.1, 0.5
* matches:
* (0->1, 1->0, 2->2) */
optimizer_
->
costs
()
->
Resize
(
4
,
3
);
(
*
optimizer_
->
costs
())(
0
,
0
)
=
4.7
;
(
*
optimizer_
->
costs
())(
0
,
1
)
=
3.8
;
(
*
optimizer_
->
costs
())(
0
,
2
)
=
1.0
;
(
*
optimizer_
->
costs
())(
1
,
0
)
=
4.1
;
(
*
optimizer_
->
costs
())(
1
,
1
)
=
3.0
;
(
*
optimizer_
->
costs
())(
1
,
2
)
=
2.0
;
(
*
optimizer_
->
costs
())(
2
,
0
)
=
1.0
;
(
*
optimizer_
->
costs
())(
2
,
1
)
=
2.0
;
(
*
optimizer_
->
costs
())(
2
,
2
)
=
4.7
;
(
*
optimizer_
->
costs
())(
3
,
0
)
=
3.2
;
(
*
optimizer_
->
costs
())(
3
,
1
)
=
2.1
;
(
*
optimizer_
->
costs
())(
3
,
2
)
=
0.5
;
optimizer_
->
Maximize
(
&
assignments
);
EXPECT_EQ
(
3
,
assignments
.
size
());
EXPECT_EQ
(
0
,
assignments
[
0
].
first
);
EXPECT_EQ
(
1
,
assignments
[
0
].
second
);
EXPECT_EQ
(
1
,
assignments
[
1
].
first
);
EXPECT_EQ
(
0
,
assignments
[
1
].
second
);
EXPECT_EQ
(
2
,
assignments
[
2
].
first
);
EXPECT_EQ
(
2
,
assignments
[
2
].
second
);
/* case 6: empty one */
optimizer_
->
costs
()
->
Resize
(
0
,
0
);
optimizer_
->
Maximize
(
&
assignments
);
EXPECT_EQ
(
0
,
assignments
.
size
());
}
}
// namespace common
}
// namespace perception
}
// namespace apollo
modules/perception/common/graph/secure_matrix.h
0 → 100644
浏览文件 @
b22476e6
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_COMMON_GRAPH_SECURE_MATRIX_H_
#define MODULES_PERCEPTION_COMMON_GRAPH_SECURE_MATRIX_H_
#include <memory>
#include "Eigen/Dense"
namespace
apollo
{
namespace
perception
{
namespace
common
{
template
<
typename
T
>
class
SecureMat
{
public:
SecureMat
()
:
height_
(
0
),
width_
(
0
)
{
Reserve
(
max_height_
,
max_width_
);
}
size_t
height
()
{
return
height_
;
}
size_t
width
()
{
return
width_
;
}
/* @brief: reserve memory of SecureMat
* @params[IN] reserve_height: height of reserve memory
* @params[IN] reserve_width: width of reserve memory
* @return nothing */
void
Reserve
(
const
size_t
reserve_height
,
const
size_t
reserve_width
)
{
max_height_
=
(
reserve_height
>
max_height_
)
?
reserve_height
:
max_height_
;
max_width_
=
(
reserve_width
>
max_width_
)
?
reserve_width
:
max_width_
;
mat_
.
resize
(
max_height_
,
max_width_
);
}
/* @brief: resize memory of SecureMat
* @params[IN] resize_height: height of resize memory
* @params[IN] resize_width: width of resize memory
* @return nothing */
void
Resize
(
const
size_t
resize_height
,
const
size_t
resize_width
)
{
height_
=
resize_height
;
width_
=
resize_width
;
if
(
resize_height
<=
max_height_
&&
resize_width
<=
max_width_
)
{
return
;
}
max_height_
=
(
resize_height
>
max_height_
)
?
resize_height
:
max_height_
;
max_width_
=
(
resize_width
>
max_width_
)
?
resize_width
:
max_width_
;
mat_
.
resize
(
max_height_
,
max_width_
);
}
void
ToString
(
std
::
ostream
*
out_stream
)
{
std
::
ostream
&
stream
=
*
out_stream
;
for
(
size_t
row
=
0
;
row
<
height_
;
++
row
)
{
for
(
size_t
col
=
0
;
col
<
width_
;
++
col
)
{
stream
<<
mat_
(
row
,
col
)
<<
"
\t
"
;
}
stream
<<
"
\n
"
;
}
}
inline
const
T
&
operator
()(
const
size_t
row
,
const
size_t
col
)
const
{
return
mat_
(
row
,
col
);
}
inline
T
&
operator
()(
const
size_t
row
,
const
size_t
col
)
{
return
mat_
(
row
,
col
);
}
private:
Eigen
::
Matrix
<
T
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
mat_
;
size_t
max_height_
=
1000
;
size_t
max_width_
=
1000
;
size_t
height_
=
0
;
size_t
width_
=
0
;
};
// class SecureMat
}
// namespace common
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_COMMON_GRAPH_SECURE_MATRIX_H_
modules/perception/common/graph/secure_matrix_test.cc
0 → 100644
浏览文件 @
b22476e6
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/perception/common/graph/secure_matrix.h"
#include "Eigen/Core"
#include "gtest/gtest.h"
namespace
apollo
{
namespace
perception
{
namespace
common
{
TEST
(
SecureMatTest
,
test_reserve_mat
)
{
SecureMat
<
float
>
test_mat
;
test_mat
.
Reserve
(
100
,
100
);
test_mat
.
Resize
(
10
,
20
);
EXPECT_EQ
(
10
,
test_mat
.
height
());
EXPECT_EQ
(
20
,
test_mat
.
width
());
test_mat
.
Reserve
(
1200
,
1200
);
EXPECT_EQ
(
10
,
test_mat
.
height
());
EXPECT_EQ
(
20
,
test_mat
.
width
());
}
TEST
(
SecureMatTest
,
test_resize_mat
)
{
SecureMat
<
float
>
test_mat
;
test_mat
.
Reserve
(
100
,
100
);
test_mat
.
Resize
(
3
,
2
);
EXPECT_EQ
(
3
,
test_mat
.
height
());
EXPECT_EQ
(
2
,
test_mat
.
width
());
test_mat
.
Resize
(
500
,
1500
);
EXPECT_EQ
(
500
,
test_mat
.
height
());
EXPECT_EQ
(
1500
,
test_mat
.
width
());
test_mat
.
Resize
(
2000
,
1500
);
EXPECT_EQ
(
2000
,
test_mat
.
height
());
EXPECT_EQ
(
1500
,
test_mat
.
width
());
}
TEST
(
SecureMatTest
,
test_fill_mat
)
{
SecureMat
<
float
>
test_mat
;
test_mat
.
Reserve
(
10
,
10
);
test_mat
.
Resize
(
2
,
2
);
test_mat
(
0
,
0
)
=
1
;
test_mat
(
0
,
1
)
=
2
;
test_mat
(
1
,
0
)
=
3
;
test_mat
(
1
,
1
)
=
4
;
test_mat
.
ToString
(
&
std
::
cout
);
EXPECT_EQ
(
1
,
test_mat
(
0
,
0
));
EXPECT_EQ
(
2
,
test_mat
(
0
,
1
));
EXPECT_EQ
(
3
,
test_mat
(
1
,
0
));
EXPECT_EQ
(
4
,
test_mat
(
1
,
1
));
}
}
// namespace common
}
// namespace perception
}
// namespace apollo
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录