Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
L
laser
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
laser
Commits
98fc7ca6
Commit
98fc7ca6
authored
5 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
adapting to measurements in factors
parent
ab3eb138
No related branches found
No related tags found
3 merge requests
!30
Release after RAL
,
!29
After 2nd RAL submission
,
!19
Resolve "Copying measurement and sqrtinfo in FactorBase (multi-threading)"
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/laser/factor/factor_point_2d.h
+20
-30
20 additions, 30 deletions
include/laser/factor/factor_point_2d.h
include/laser/factor/factor_point_to_line_2d.h
+20
-29
20 additions, 29 deletions
include/laser/factor/factor_point_to_line_2d.h
with
40 additions
and
59 deletions
include/laser/factor/factor_point_2d.h
+
20
−
30
View file @
98fc7ca6
...
@@ -5,6 +5,7 @@
...
@@ -5,6 +5,7 @@
#include
"core/factor/factor_autodiff.h"
#include
"core/factor/factor_autodiff.h"
#include
"laser/feature/feature_polyline_2d.h"
#include
"laser/feature/feature_polyline_2d.h"
#include
"laser/landmark/landmark_polyline_2d.h"
#include
"laser/landmark/landmark_polyline_2d.h"
#include
"core/math/covariance.h"
namespace
wolf
{
namespace
wolf
{
...
@@ -19,9 +20,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
...
@@ -19,9 +20,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
unsigned
int
feature_point_id_
;
unsigned
int
feature_point_id_
;
int
landmark_point_id_
;
int
landmark_point_id_
;
StateBlockPtr
point_state_ptr_
;
StateBlockPtr
point_state_ptr_
;
Eigen
::
VectorXd
measurement_
;
///< the measurement vector
Eigen
::
MatrixXd
measurement_covariance_
;
///< the measurement covariance matrix
Eigen
::
MatrixXd
measurement_sqrt_information_
;
///< the squared root information matrix
public:
public:
...
@@ -44,19 +42,32 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
...
@@ -44,19 +42,32 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
_lmk_ptr
->
getPointStateBlock
(
_lmk_point_id
)),
_lmk_ptr
->
getPointStateBlock
(
_lmk_point_id
)),
feature_point_id_
(
_ftr_point_id
),
feature_point_id_
(
_ftr_point_id
),
landmark_point_id_
(
_lmk_point_id
),
landmark_point_id_
(
_lmk_point_id
),
point_state_ptr_
(
_lmk_ptr
->
getPointStateBlock
(
_lmk_point_id
)),
point_state_ptr_
(
_lmk_ptr
->
getPointStateBlock
(
_lmk_point_id
))
measurement_
(
_ftr_ptr
->
getPoints
().
col
(
_ftr_point_id
)),
measurement_covariance_
(
_ftr_ptr
->
getPointsCov
().
middleCols
(
_ftr_point_id
*
2
,
2
))
{
{
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
//std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
//std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
Eigen
::
LLT
<
Eigen
::
MatrixXd
>
lltOfA
(
measurement_covariance_
);
// compute the Cholesky decomposition of A
Eigen
::
MatrixXd
measurement_sqrt_covariance
=
lltOfA
.
matrixU
();
measurement_sqrt_information_
=
measurement_sqrt_covariance
.
inverse
().
transpose
();
// retrieve factor U in the decomposition
}
}
virtual
~
FactorPoint2d
()
=
default
;
virtual
~
FactorPoint2d
()
=
default
;
virtual
void
updateMeasurementAndSquareRootInformationUpper
()
override
{
if
(
not
getFeature
())
return
;
// cast
auto
ftr
=
std
::
static_pointer_cast
<
FeaturePolyline2d
>
(
getFeature
());
// measurement
measurement_
=
ftr
->
getPoints
().
col
(
feature_point_id_
);
// covariance (ensuring symmetry)
Eigen
::
Matrix2d
measurement_covariance
=
ftr
->
getPointsCov
().
middleCols
(
feature_point_id_
*
2
,
2
).
selfadjointView
<
Eigen
::
Upper
>
();
// square root information upper matrix
measurement_sqrt_information_upper_
=
computeSqrtUpper
(
measurement_covariance
.
inverse
());
}
virtual
std
::
string
getTopology
()
const
override
virtual
std
::
string
getTopology
()
const
override
{
{
return
std
::
string
(
"LMK"
);
return
std
::
string
(
"LMK"
);
...
@@ -103,27 +114,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
...
@@ -103,27 +114,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
*/
*/
template
<
typename
T
>
template
<
typename
T
>
bool
operator
()(
const
T
*
const
_robotP
,
const
T
*
const
_robotO
,
const
T
*
const
_landmarkOriginP
,
const
T
*
const
_landmarkOriginO
,
const
T
*
const
_landmarkPoint
,
T
*
_residuals
)
const
;
bool
operator
()(
const
T
*
const
_robotP
,
const
T
*
const
_robotO
,
const
T
*
const
_landmarkOriginP
,
const
T
*
const
_landmarkOriginO
,
const
T
*
const
_landmarkPoint
,
T
*
_residuals
)
const
;
/** \brief Returns a reference to the feature measurement
**/
virtual
const
Eigen
::
VectorXd
&
getMeasurement
()
const
override
{
return
measurement_
;
}
/** \brief Returns a reference to the feature measurement covariance
**/
virtual
const
Eigen
::
MatrixXd
&
getMeasurementCovariance
()
const
override
{
return
measurement_covariance_
;
}
/** \brief Returns a reference to the feature measurement square root information
**/
virtual
const
Eigen
::
MatrixXd
&
getMeasurementSquareRootInformationUpper
()
const
override
{
return
measurement_sqrt_information_
;
}
};
};
template
<
typename
T
>
template
<
typename
T
>
...
...
This diff is collapsed.
Click to expand it.
include/laser/factor/factor_point_to_line_2d.h
+
20
−
29
View file @
98fc7ca6
...
@@ -5,6 +5,7 @@
...
@@ -5,6 +5,7 @@
#include
"core/factor/factor_autodiff.h"
#include
"core/factor/factor_autodiff.h"
#include
"laser/feature/feature_polyline_2d.h"
#include
"laser/feature/feature_polyline_2d.h"
#include
"laser/landmark/landmark_polyline_2d.h"
#include
"laser/landmark/landmark_polyline_2d.h"
#include
"core/math/covariance.h"
namespace
wolf
{
namespace
wolf
{
...
@@ -19,9 +20,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
...
@@ -19,9 +20,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
unsigned
int
feature_point_id_
;
unsigned
int
feature_point_id_
;
StateBlockPtr
point_state_ptr_
;
StateBlockPtr
point_state_ptr_
;
StateBlockPtr
point_aux_state_ptr_
;
StateBlockPtr
point_aux_state_ptr_
;
Eigen
::
VectorXd
measurement_
;
///< the measurement vector
Eigen
::
MatrixXd
measurement_covariance_
;
///< the measurement covariance matrix
Eigen
::
MatrixXd
measurement_covariance_
;
///< the measurement covariance matrix
Eigen
::
MatrixXd
measurement_sqrt_information_
;
///< the squared root information matrix
public:
public:
...
@@ -48,20 +47,33 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
...
@@ -48,20 +47,33 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
landmark_point_aux_id_
(
_lmk_point_aux_id
),
landmark_point_aux_id_
(
_lmk_point_aux_id
),
feature_point_id_
(
_ftr_point_id
),
feature_point_id_
(
_ftr_point_id
),
point_state_ptr_
(
_lmk_ptr
->
getPointStateBlock
(
_lmk_point_id
)),
point_state_ptr_
(
_lmk_ptr
->
getPointStateBlock
(
_lmk_point_id
)),
point_aux_state_ptr_
(
_lmk_ptr
->
getPointStateBlock
(
_lmk_point_aux_id
)),
point_aux_state_ptr_
(
_lmk_ptr
->
getPointStateBlock
(
_lmk_point_aux_id
))
measurement_
(
_ftr_ptr
->
getPoints
().
col
(
_ftr_point_id
)),
measurement_covariance_
(
_ftr_ptr
->
getPointsCov
().
middleCols
(
_ftr_point_id
*
2
,
2
))
{
{
//std::cout << "FactorPointToLine2d" << std::endl;
//std::cout << "FactorPointToLine2d" << std::endl;
//std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl;
//std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl;
assert
(
_lmk_ptr
->
isValidId
(
landmark_point_id_
)
&&
_lmk_ptr
->
isValidId
(
landmark_point_aux_id_
));
assert
(
_lmk_ptr
->
isValidId
(
landmark_point_id_
)
&&
_lmk_ptr
->
isValidId
(
landmark_point_aux_id_
));
Eigen
::
LLT
<
Eigen
::
MatrixXd
>
lltOfA
(
measurement_covariance_
);
// compute the Cholesky decomposition of A
Eigen
::
MatrixXd
measurement_sqrt_covariance
=
lltOfA
.
matrixU
();
measurement_sqrt_information_
=
measurement_sqrt_covariance
.
inverse
().
transpose
();
// retrieve factor U in the decomposition
}
}
virtual
~
FactorPointToLine2d
()
=
default
;
virtual
~
FactorPointToLine2d
()
=
default
;
virtual
void
updateMeasurementAndSquareRootInformationUpper
()
override
{
if
(
not
getFeature
())
return
;
// cast
auto
ftr
=
std
::
static_pointer_cast
<
FeaturePolyline2d
>
(
getFeature
());
// measurement
measurement_
=
ftr
->
getPoints
().
col
(
feature_point_id_
);
// covariance (ensuring symmetry)
measurement_covariance_
=
ftr
->
getPointsCov
().
middleCols
(
feature_point_id_
*
2
,
2
).
selfadjointView
<
Eigen
::
Upper
>
();
// square root information upper matrix
measurement_sqrt_information_upper_
=
computeSqrtUpper
(
measurement_covariance_
.
inverse
());
}
virtual
std
::
string
getTopology
()
const
override
virtual
std
::
string
getTopology
()
const
override
{
{
return
std
::
string
(
"GEOM"
);
return
std
::
string
(
"GEOM"
);
...
@@ -99,27 +111,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
...
@@ -99,27 +111,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
template
<
typename
T
>
template
<
typename
T
>
bool
operator
()(
const
T
*
const
_robotP
,
const
T
*
const
_robotO
,
const
T
*
const
_landmarkOriginPosition
,
const
T
*
const
_landmarkOriginOrientation
,
const
T
*
const
_landmarkPoint
,
const
T
*
const
_landmarkPointAux
,
T
*
_residuals
)
const
;
bool
operator
()(
const
T
*
const
_robotP
,
const
T
*
const
_robotO
,
const
T
*
const
_landmarkOriginPosition
,
const
T
*
const
_landmarkOriginOrientation
,
const
T
*
const
_landmarkPoint
,
const
T
*
const
_landmarkPointAux
,
T
*
_residuals
)
const
;
/** \brief Returns a reference to the feature measurement
**/
virtual
const
Eigen
::
VectorXd
&
getMeasurement
()
const
override
{
return
measurement_
;
}
/** \brief Returns a reference to the feature measurement covariance
**/
virtual
const
Eigen
::
MatrixXd
&
getMeasurementCovariance
()
const
override
{
return
measurement_covariance_
;
}
/** \brief Returns a reference to the feature measurement square root information
**/
virtual
const
Eigen
::
MatrixXd
&
getMeasurementSquareRootInformationUpper
()
const
override
{
return
measurement_sqrt_information_
;
}
};
};
template
<
typename
T
>
template
<
typename
T
>
...
...
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