Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
wolf
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
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
mobile_robotics
wolf_projects
wolf_lib
wolf
Merge requests
!419
Resolve "Processor motion model"
Code
Review changes
Check out branch
Download
Patches
Plain diff
Merged
Resolve "Processor motion model"
407-processor-motion-model
into
devel
Overview
0
Commits
19
Pipelines
16
Changes
6
Merged
Joan Vallvé Navarro
requested to merge
407-processor-motion-model
into
devel
3 years ago
Overview
0
Commits
19
Pipelines
16
Changes
6
Expand
Closes
#407 (closed)
Edited
3 years ago
by
Joan Vallvé Navarro
0
0
Merge request reports
Compare
devel
version 14
b2cb547f
3 years ago
version 13
7a0a5a09
3 years ago
version 12
3a1b1bbe
3 years ago
version 11
1dd7523f
3 years ago
version 10
a20db862
3 years ago
version 9
c5bafa69
3 years ago
version 8
4682adb7
3 years ago
version 7
da46d8d5
3 years ago
version 6
e798de7a
3 years ago
version 5
60fd7d8b
3 years ago
version 4
d114f33b
3 years ago
version 3
da42fd62
3 years ago
version 2
8b22178a
3 years ago
version 1
451aba0a
3 years ago
devel (base)
and
version 2
latest version
02ca05a3
19 commits,
3 years ago
version 14
b2cb547f
17 commits,
3 years ago
version 13
7a0a5a09
16 commits,
3 years ago
version 12
3a1b1bbe
15 commits,
3 years ago
version 11
1dd7523f
13 commits,
3 years ago
version 10
a20db862
11 commits,
3 years ago
version 9
c5bafa69
10 commits,
3 years ago
version 8
4682adb7
9 commits,
3 years ago
version 7
da46d8d5
8 commits,
3 years ago
version 6
e798de7a
7 commits,
3 years ago
version 5
60fd7d8b
6 commits,
3 years ago
version 4
d114f33b
5 commits,
3 years ago
version 3
da42fd62
3 commits,
3 years ago
version 2
8b22178a
2 commits,
3 years ago
version 1
451aba0a
1 commit,
3 years ago
6 files
+
377
−
0
Inline
Compare changes
Side-by-side
Inline
Show whitespace changes
Show one file at a time
Files
6
Search (e.g. *.vue) (Ctrl+P)
include/core/factor/factor_velocity_direction_3d.h
0 → 100644
+
75
−
0
Options
#ifndef FACTOR_VELOCITY_DIRECTION_3D_H_
#define FACTOR_VELOCITY_DIRECTION_3D_H_
//Wolf includes
#include
"core/factor/factor_autodiff.h"
#include
"core/frame/frame_base.h"
#include
"core/math/rotations.h"
//#include "ceres/jet.h"
namespace
wolf
{
WOLF_PTR_TYPEDEFS
(
FactorVelocityDirection3d
);
//class
class
FactorVelocityDirection3d
:
public
FactorAutodiff
<
FactorVelocityDirection3d
,
1
,
3
,
4
>
{
public:
FactorVelocityDirection3d
(
FeatureBasePtr
_ftr_ptr
,
const
ProcessorBasePtr
&
_processor_ptr
,
bool
_apply_loss_function
,
FactorStatus
_status
=
FAC_ACTIVE
)
:
FactorAutodiff
<
FactorVelocityDirection3d
,
1
,
3
,
4
>
(
"FactorVelocityDirection3d"
,
TOP_ABS
,
_ftr_ptr
,
nullptr
,
nullptr
,
nullptr
,
nullptr
,
_processor_ptr
,
_apply_loss_function
,
_status
,
_ftr_ptr
->
getFrame
()
->
getV
(),
_ftr_ptr
->
getFrame
()
->
getO
())
{
// std::cout << "created FactorVelocityDirection3d " << std::endl;
}
~
FactorVelocityDirection3d
()
override
=
default
;
template
<
typename
T
>
bool
operator
()(
const
T
*
const
_v
,
const
T
*
const
_o
,
T
*
_residuals
)
const
;
};
template
<
typename
T
>
inline
bool
FactorVelocityDirection3d
::
operator
()(
const
T
*
const
_v
,
const
T
*
const
_q
,
T
*
_residuals
)
const
{
Eigen
::
Map
<
const
Eigen
::
Matrix
<
T
,
3
,
1
>
>
v
(
_v
);
Eigen
::
Map
<
const
Eigen
::
Quaternion
<
T
>
>
q
(
_q
);
// std::cout << "v: " << v(0) << " "
// << v(1) << " "
// << v(2) << "\n";
// std::cout << "q: " << q.coeffs()(0) << " "
// << q.coeffs()(1) << " "
// << q.coeffs()(2) << " "
// << q.coeffs()(3) << "\n";
// velocity in local coordinates
Eigen
::
Matrix
<
T
,
3
,
1
>
v_local
=
q
.
conjugate
()
*
v
;
// std::cout << "v_local: " << v_local(0) << " "
// << v_local(1) << " "
// << v_local(2) << "\n";
// error (angle between measurement and velocity in local coordinates)
double
error
=
acos
(
v_local
.
dot
(
getMeasurement
().
cast
<
T
>
())
/
(
v_local
.
norm
()
*
getMeasurement
().
cast
<
T
>
().
norm
()));
// residual
_residuals
[
0
]
=
getMeasurementSquareRootInformationUpper
()(
0
,
0
)
*
error
;
return
true
;
}
}
// namespace wolf
#endif
Loading