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
Viewing commit
4682adb7
Prev
Next
Show latest version
6 files
+
459
−
186
Inline
Compare changes
Side-by-side
Inline
Show whitespace changes
Show one file at a time
Files
6
Search (e.g. *.vue) (Ctrl+P)
4682adb7
removed constant velocity and working on gtests
· 4682adb7
Joan Vallvé Navarro
authored
3 years ago
include/core/factor/factor_velocity_direction_3d.h
→
include/core/factor/factor_velocity_
local_
direction_3d.h
+
8
−
8
Options
#ifndef FACTOR_VELOCITY_DIRECTION_3D_H_
#define FACTOR_VELOCITY_DIRECTION_3D_H_
#ifndef FACTOR_VELOCITY_
LOCAL_
DIRECTION_3D_H_
#define FACTOR_VELOCITY_
LOCAL_
DIRECTION_3D_H_
//Wolf includes
#include
"core/factor/factor_autodiff.h"
@@ -11,21 +11,21 @@
namespace
wolf
{
WOLF_PTR_TYPEDEFS
(
FactorVelocityDirection3d
);
WOLF_PTR_TYPEDEFS
(
FactorVelocity
Local
Direction3d
);
//class
class
FactorVelocityDirection3d
:
public
FactorAutodiff
<
FactorVelocityDirection3d
,
1
,
3
,
4
>
class
FactorVelocity
Local
Direction3d
:
public
FactorAutodiff
<
FactorVelocity
Local
Direction3d
,
1
,
3
,
4
>
{
protected:
double
min_vel_sq_norm_
;
// stored in squared norm for efficiency
public:
FactorVelocityDirection3d
(
FeatureBasePtr
_ftr_ptr
,
FactorVelocity
Local
Direction3d
(
FeatureBasePtr
_ftr_ptr
,
const
double
&
_min_vel_norm
,
const
ProcessorBasePtr
&
_processor_ptr
,
bool
_apply_loss_function
,
FactorStatus
_status
=
FAC_ACTIVE
)
:
FactorAutodiff
<
FactorVelocityDirection3d
,
1
,
3
,
4
>
(
"FactorVelocityDirection3d"
,
FactorAutodiff
<
FactorVelocity
Local
Direction3d
,
1
,
3
,
4
>
(
"FactorVelocityDirection3d"
,
TOP_ABS
,
_ftr_ptr
,
nullptr
,
nullptr
,
nullptr
,
nullptr
,
@@ -41,14 +41,14 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
assert
(
abs
(
_ftr_ptr
->
getMeasurement
().
norm
()
-
1.0
)
<
wolf
::
Constants
::
EPS
&&
"velocity direction measurement must be normalized"
);
}
~
FactorVelocityDirection3d
()
override
=
default
;
~
FactorVelocity
Local
Direction3d
()
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
inline
bool
FactorVelocity
Local
Direction3d
::
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
);
Loading