Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
B
bodydynamics
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Model registry
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor 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
plugins
bodydynamics
Commits
ff9c9aca
Commit
ff9c9aca
authored
3 years ago
by
Mederic Fourmy
Browse files
Options
Downloads
Patches
Plain Diff
[skip-ci] Beginning to code a zero vel kin factor
parent
c202064e
No related branches found
No related tags found
2 merge requests
!23
After cmake and const refactor
,
!11
Resolve "Turn ProcessorPointFeetNomove into a processor tracker"
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
include/bodydynamics/factor/factor_point_feet_zero_velocity.h
+113
-0
113 additions, 0 deletions
...ude/bodydynamics/factor/factor_point_feet_zero_velocity.h
with
113 additions
and
0 deletions
include/bodydynamics/factor/factor_point_feet_zero_velocity.h
0 → 100644
+
113
−
0
View file @
ff9c9aca
/**
* \file factor_point_feet_nomove.h
*
* Created on: Feb 22, 2020
* \author: mfourmy
*/
#ifndef FACTOR_POINT_FEET_ZERO_VELOCITY_H_
#define FACTOR_POINT_FEET_ZERO_VELOCITY_H_
#include
"core/factor/factor_autodiff.h"
namespace
wolf
{
WOLF_PTR_TYPEDEFS
(
FactorPointFeetNomove
);
class
FactorPointFeetNomove
:
public
FactorAutodiff
<
FactorPointFeetNomove
,
3
,
3
,
4
,
3
>
{
public:
FactorPointFeetNomove
(
const
FeatureBasePtr
&
_ftr_current_ptr
,
const
StateBlockPtr
&
_bias_imu
,
const
ProcessorBasePtr
&
_processor_ptr
,
bool
_apply_loss_function
,
FactorStatus
_status
=
FAC_ACTIVE
);
~
FactorPointFeetNomove
()
override
{
/* nothing */
}
/*
Factor state blocks:
_pb: W_p_WB -> base position in world frame
_qb: W_q_B -> base orientation in world frame
_pbm: W_p_WB -> base position in world frame, previous KF
_qbm: W_q_B -> base orientation in world frame, previous KF
*/
template
<
typename
T
>
bool
operator
()
(
const
T
*
const
_vb
,
const
T
*
const
_qb
,
const
T
*
const
_bi
,
T
*
_res
)
const
;
// Vector9d residual() const;
// double cost() const;
};
}
/* namespace wolf */
namespace
wolf
{
FactorPointFeetNomove
::
FactorPointFeetNomove
(
const
FeatureBasePtr
&
_ftr_current_ptr
,
const
StateBlockPtr
&
_bias_imu
,
const
ProcessorBasePtr
&
_processor_ptr
,
bool
_apply_loss_function
,
FactorStatus
_status
)
:
FactorAutodiff
(
"FactorPointFeetNomove"
,
TOP_GEOM
,
_ftr_current_ptr
,
nullptr
,
// _frame_other_ptr
nullptr
,
// _capture_other_ptr
nullptr
,
// _feature_other_ptr
nullptr
,
// _landmark_other_ptr
_processor_ptr
,
_apply_loss_function
,
_status
,
_ftr_current_ptr
->
getFrame
()
->
getV
(),
_ftr_current_ptr
->
getFrame
()
->
getO
(),
_bias_imu
)
{
}
template
<
typename
T
>
bool
FactorPointFeetNomove
::
operator
()
(
const
T
*
const
_vb
,
const
T
*
const
_qb
,
const
T
*
const
_bi
,
T
*
_res
)
const
{
Map
<
Matrix
<
T
,
3
,
1
>
>
res
(
_res
);
// State variables instanciation
Map
<
const
Matrix
<
T
,
3
,
1
>
>
vb
(
_vb
);
Map
<
const
Quaternion
<
T
>
>
qb
(
_qb
);
Map
<
const
Matrix
<
T
,
3
,
1
>
>
bw
(
_pbm
+
3
);
// gyro bias
Map
<
const
Quaternion
<
T
>
>
qbm
(
_qbm
);
// Measurements retrieval
Eigen
::
Matrix
<
T
,
6
,
1
>
meas
=
getMeasurement
().
cast
<
T
>
();
Eigen
::
Matrix
<
T
,
3
,
1
>
b_v_bl
=
meas
.
head
<
3
>
();
Eigen
::
Matrix
<
T
,
3
,
1
>
i_omg_i
=
meas
.
segment
<
3
>
(
3
);
Eigen
::
Matrix
<
T
,
3
,
1
>
b_p_bl
=
meas
.
tail
<
3
>
();
Matrix
<
T
,
3
,
1
>
err
=
qb
.
conjugate
()
*
vb
+
(
i_omg_i
-
bw
).
cross
(
b_p_bl
)
+
b_v_bl
;
res
=
getMeasurementSquareRootInformationUpper
()
*
err
;
return
true
;
}
}
// namespace wolf
#endif
/* FACTOR__POINT_FEET_ZERO_VELOCITY_H_ */
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment