Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
N
Navigation
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Container Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Software
ArmarX
Skills
Navigation
Merge requests
!55
Couldn't fetch the linked file.
Draft: Implement human grouping
Code
Review changes
Check out branch
Download
Patches
Plain diff
Open
Draft: Implement human grouping
implement-human-groups
into
dev
Overview
0
Commits
63
Pipelines
1
Changes
2
Open
Timo Weberruß
requested to merge
implement-human-groups
into
dev
2 years ago
Overview
0
Commits
63
Pipelines
1
Changes
2
Expand
Closes
#34
Edited
2 years ago
by
Timo Weberruß
0
0
Merge request reports
Viewing commit
57987085
Prev
Next
Show latest version
2 files
+
44
−
2
Inline
Compare changes
Side-by-side
Inline
Show whitespace changes
Show one file at a time
Files
2
Search (e.g. *.vue) (Ctrl+P)
57987085
Implement OrientationDistance
· 57987085
Timo Weberruß
authored
2 years ago
source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
+
39
−
2
Options
@@ -2,16 +2,53 @@
namespace
armarx
::
navigation
::
components
::
dynamic_scene_provider
{
OrientationDistance
::
OrientationDistance
(
double
max
,
double
min
)
{
this
->
max
=
max
;
this
->
min
=
min
;
}
double
OrientationDistance
::
computeDistance
(
Human
&
h1
,
Human
&
h2
)
{
// ranges from 0 to 1
double
factor
=
getOrientationFactor
(
h1
,
h2
);
OrientationDistance
::
OrientationDistance
(
double
max
,
double
min
)
return
min
+
factor
*
(
max
-
min
);
}
double
OrientationDistance
::
getOrientationFactor
(
Human
&
h1
,
Human
&
h2
)
{
double
lineOrientation
=
getLineOrientation
(
h2
,
h1
);
double
angleH1
=
Eigen
::
Rotation2Dd
(
h1
.
pose
.
linear
()).
angle
();
double
angleH2
=
Eigen
::
Rotation2Dd
(
h2
.
pose
.
linear
()).
angle
();
// assuming the angles to be in the interval [0, 2pi]
double
deviationFirst
=
std
::
abs
(
normOrientation
(
angleH1
-
lineOrientation
));
double
deviationSecond
=
std
::
abs
(
normOrientation
(
angleH2
-
(
lineOrientation
+
1
)));
// ranges from 0 (looking directly at each other) to 1 (back to back)
return
(
deviationFirst
+
deviationSecond
)
/
2
;
}
double
OrientationDistance
::
computeDistance
(
Human
&
h
1
,
Human
&
h
2
)
double
OrientationDistance
::
getLineOrientation
(
Human
&
h
2
,
Human
&
h
1
)
{
double
dx
=
(
h1
.
pose
.
translation
()
-
h2
.
pose
.
translation
()).
x
();
double
dy
=
(
h1
.
pose
.
translation
()
-
h2
.
pose
.
translation
()).
y
();
double
lineOrientation
=
atan2
(
dy
,
dx
);
return
lineOrientation
;
}
// scales orientation to (-1, 1]
double
OrientationDistance
::
normOrientation
(
double
orientation
)
{
double
normedOrientation
=
orientation
/
3.1416
;
// brings orientation to [0, 2)
normedOrientation
=
std
::
fmod
(((
std
::
fmod
(
normedOrientation
,
2
))
+
2
),
2
);
// brings orientation to [-1, 1)
normedOrientation
-=
(
normedOrientation
>
1
)
*
2
;
return
normedOrientation
;
}
}
Loading