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
Commits
fce48793
"demos/solver/test_ccolamd.cpp" did not exist on "bd467fb1961b4d3a411adb31f3e2f23c058fbfde"
Commit
fce48793
authored
7 years ago
by
Joan Solà Ortega
Browse files
Options
Downloads
Patches
Plain Diff
Add getCovariance() in Frames and Lmks
parent
a6c0d7f4
No related branches found
No related tags found
No related merge requests found
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
src/frame_base.cpp
+12
-0
12 additions, 0 deletions
src/frame_base.cpp
src/frame_base.h
+2
-0
2 additions, 0 deletions
src/frame_base.h
src/landmark_base.h
+13
-2
13 additions, 2 deletions
src/landmark_base.h
src/problem.cpp
+4
-9
4 additions, 9 deletions
src/problem.cpp
src/problem.h
+4
-4
4 additions, 4 deletions
src/problem.h
with
35 additions
and
15 deletions
src/frame_base.cpp
+
12
−
0
View file @
fce48793
...
@@ -224,6 +224,18 @@ unsigned int FrameBase::getSize() const
...
@@ -224,6 +224,18 @@ unsigned int FrameBase::getSize() const
return
size
;
return
size
;
}
}
bool
FrameBase
::
getCovariance
(
Eigen
::
MatrixXs
&
_cov
)
const
{
return
getProblem
()
->
getFrameCovariance
(
shared_from_this
(),
_cov
);
}
Eigen
::
MatrixXs
FrameBase
::
getCovariance
()
const
{
return
getProblem
()
->
getFrameCovariance
(
shared_from_this
());
}
FrameBasePtr
FrameBase
::
getPreviousFrame
()
const
FrameBasePtr
FrameBase
::
getPreviousFrame
()
const
{
{
//std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
//std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
...
...
This diff is collapsed.
Click to expand it.
src/frame_base.h
+
2
−
0
View file @
fce48793
...
@@ -109,6 +109,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
...
@@ -109,6 +109,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
Eigen
::
VectorXs
getState
()
const
;
Eigen
::
VectorXs
getState
()
const
;
void
getState
(
Eigen
::
VectorXs
&
_state
)
const
;
void
getState
(
Eigen
::
VectorXs
&
_state
)
const
;
unsigned
int
getSize
()
const
;
unsigned
int
getSize
()
const
;
bool
getCovariance
(
Eigen
::
MatrixXs
&
_cov
)
const
;
Eigen
::
MatrixXs
getCovariance
()
const
;
// Wolf tree access ---------------------------------------------------
// Wolf tree access ---------------------------------------------------
public:
public:
...
...
This diff is collapsed.
Click to expand it.
src/landmark_base.h
+
13
−
2
View file @
fce48793
...
@@ -68,13 +68,14 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
...
@@ -68,13 +68,14 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
void
setStateBlockPtr
(
unsigned
int
_i
,
StateBlockPtr
_sb_ptr
);
void
setStateBlockPtr
(
unsigned
int
_i
,
StateBlockPtr
_sb_ptr
);
StateBlockPtr
getPPtr
()
const
;
StateBlockPtr
getPPtr
()
const
;
StateBlockPtr
getOPtr
()
const
;
StateBlockPtr
getOPtr
()
const
;
StateBlockPtr
getVPtr
()
const
;
void
setPPtr
(
const
StateBlockPtr
_p_ptr
);
void
setPPtr
(
const
StateBlockPtr
_p_ptr
);
void
setOPtr
(
const
StateBlockPtr
_o_ptr
);
void
setOPtr
(
const
StateBlockPtr
_o_ptr
);
void
setVPtr
(
const
StateBlockPtr
_v_ptr
);
virtual
void
registerNewStateBlocks
();
virtual
void
registerNewStateBlocks
();
Eigen
::
VectorXs
getState
()
const
;
Eigen
::
VectorXs
getState
()
const
;
void
getState
(
Eigen
::
VectorXs
&
_state
)
const
;
void
getState
(
Eigen
::
VectorXs
&
_state
)
const
;
bool
getCovariance
(
Eigen
::
MatrixXs
&
_cov
)
const
;
Eigen
::
MatrixXs
getCovariance
()
const
;
protected:
protected:
virtual
void
removeStateBlocks
();
virtual
void
removeStateBlocks
();
...
@@ -104,6 +105,16 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
...
@@ -104,6 +105,16 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
namespace
wolf
{
namespace
wolf
{
inline
bool
LandmarkBase
::
getCovariance
(
Eigen
::
MatrixXs
&
_cov
)
const
{
return
getProblem
()
->
getLandmarkCovariance
(
shared_from_this
(),
_cov
);
}
inline
Eigen
::
MatrixXs
LandmarkBase
::
getCovariance
()
const
{
return
getProblem
()
->
getLandmarkCovariance
(
shared_from_this
());
}
inline
MapBasePtr
LandmarkBase
::
getMapPtr
()
inline
MapBasePtr
LandmarkBase
::
getMapPtr
()
{
{
return
map_ptr_
.
lock
();
return
map_ptr_
.
lock
();
...
...
This diff is collapsed.
Click to expand it.
src/problem.cpp
+
4
−
9
View file @
fce48793
...
@@ -512,13 +512,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, co
...
@@ -512,13 +512,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, co
return
getCovarianceBlock
(
_state
,
_state
,
_cov
,
_row_and_col
,
_row_and_col
);
return
getCovarianceBlock
(
_state
,
_state
,
_cov
,
_row_and_col
,
_row_and_col
);
}
}
bool
Problem
::
getFrameCovariance
(
FrameBasePtr
_frame_ptr
,
Eigen
::
MatrixXs
&
_covariance
)
bool
Problem
::
getFrameCovariance
(
FrameBase
Const
Ptr
_frame_ptr
,
Eigen
::
MatrixXs
&
_covariance
)
{
{
// return getCovarianceBlock(_frame_ptr->getPPtr(), _frame_ptr->getPPtr(), _covariance, 0, 0 ) &&
// getCovarianceBlock(_frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _covariance, 0, _frame_ptr->getPPtr()->getSize()) &&
// getCovarianceBlock(_frame_ptr->getOPtr(), _frame_ptr->getPPtr(), _covariance, _frame_ptr->getPPtr()->getSize(), 0 ) &&
// getCovarianceBlock(_frame_ptr->getOPtr(), _frame_ptr->getOPtr(), _covariance, _frame_ptr->getPPtr()->getSize(), _frame_ptr->getPPtr()->getSize());
bool
success
(
true
);
bool
success
(
true
);
int
i
=
0
,
j
=
0
;
int
i
=
0
,
j
=
0
;
...
@@ -543,7 +538,7 @@ bool Problem::getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _cova
...
@@ -543,7 +538,7 @@ bool Problem::getFrameCovariance(FrameBasePtr _frame_ptr, Eigen::MatrixXs& _cova
return
success
;
return
success
;
}
}
Eigen
::
MatrixXs
Problem
::
getFrameCovariance
(
FrameBasePtr
_frame_ptr
)
Eigen
::
MatrixXs
Problem
::
getFrameCovariance
(
FrameBase
Const
Ptr
_frame_ptr
)
{
{
Size
sz
=
0
;
Size
sz
=
0
;
for
(
const
auto
&
sb
:
_frame_ptr
->
getStateBlockVec
())
for
(
const
auto
&
sb
:
_frame_ptr
->
getStateBlockVec
())
...
@@ -562,7 +557,7 @@ Eigen::MatrixXs Problem::getLastKeyFrameCovariance()
...
@@ -562,7 +557,7 @@ Eigen::MatrixXs Problem::getLastKeyFrameCovariance()
return
getFrameCovariance
(
frm
);
return
getFrameCovariance
(
frm
);
}
}
bool
Problem
::
getLandmarkCovariance
(
LandmarkBasePtr
_landmark_ptr
,
Eigen
::
MatrixXs
&
_covariance
)
bool
Problem
::
getLandmarkCovariance
(
LandmarkBase
Const
Ptr
_landmark_ptr
,
Eigen
::
MatrixXs
&
_covariance
)
{
{
bool
success
(
true
);
bool
success
(
true
);
int
i
=
0
,
j
=
0
;
int
i
=
0
,
j
=
0
;
...
@@ -588,7 +583,7 @@ bool Problem::getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::Matrix
...
@@ -588,7 +583,7 @@ bool Problem::getLandmarkCovariance(LandmarkBasePtr _landmark_ptr, Eigen::Matrix
return
success
;
return
success
;
}
}
Eigen
::
MatrixXs
Problem
::
getLandmarkCovariance
(
LandmarkBasePtr
_landmark_ptr
)
Eigen
::
MatrixXs
Problem
::
getLandmarkCovariance
(
LandmarkBase
Const
Ptr
_landmark_ptr
)
{
{
Size
sz
=
0
;
Size
sz
=
0
;
for
(
const
auto
&
sb
:
_landmark_ptr
->
getStateBlockVec
())
for
(
const
auto
&
sb
:
_landmark_ptr
->
getStateBlockVec
())
...
...
This diff is collapsed.
Click to expand it.
src/problem.h
+
4
−
4
View file @
fce48793
...
@@ -252,11 +252,11 @@ class Problem : public std::enable_shared_from_this<Problem>
...
@@ -252,11 +252,11 @@ class Problem : public std::enable_shared_from_this<Problem>
bool
getCovarianceBlock
(
StateBlockPtr
_state1
,
StateBlockPtr
_state2
,
Eigen
::
MatrixXs
&
_cov
,
const
int
_row
=
0
,
const
int
_col
=
0
);
bool
getCovarianceBlock
(
StateBlockPtr
_state1
,
StateBlockPtr
_state2
,
Eigen
::
MatrixXs
&
_cov
,
const
int
_row
=
0
,
const
int
_col
=
0
);
bool
getCovarianceBlock
(
std
::
map
<
StateBlockPtr
,
unsigned
int
>
_sb_2_idx
,
Eigen
::
MatrixXs
&
_cov
);
bool
getCovarianceBlock
(
std
::
map
<
StateBlockPtr
,
unsigned
int
>
_sb_2_idx
,
Eigen
::
MatrixXs
&
_cov
);
bool
getCovarianceBlock
(
StateBlockPtr
_state
,
Eigen
::
MatrixXs
&
_cov
,
const
int
_row_and_col
=
0
);
bool
getCovarianceBlock
(
StateBlockPtr
_state
,
Eigen
::
MatrixXs
&
_cov
,
const
int
_row_and_col
=
0
);
bool
getFrameCovariance
(
FrameBasePtr
_frame_ptr
,
Eigen
::
MatrixXs
&
_covariance
);
bool
getFrameCovariance
(
FrameBase
Const
Ptr
_frame_ptr
,
Eigen
::
MatrixXs
&
_covariance
);
Eigen
::
MatrixXs
getFrameCovariance
(
FrameBasePtr
_frame_ptr
);
Eigen
::
MatrixXs
getFrameCovariance
(
FrameBase
Const
Ptr
_frame_ptr
);
Eigen
::
MatrixXs
getLastKeyFrameCovariance
();
Eigen
::
MatrixXs
getLastKeyFrameCovariance
();
bool
getLandmarkCovariance
(
LandmarkBasePtr
_landmark_ptr
,
Eigen
::
MatrixXs
&
_covariance
);
bool
getLandmarkCovariance
(
LandmarkBase
Const
Ptr
_landmark_ptr
,
Eigen
::
MatrixXs
&
_covariance
);
Eigen
::
MatrixXs
getLandmarkCovariance
(
LandmarkBasePtr
_landmark_ptr
);
Eigen
::
MatrixXs
getLandmarkCovariance
(
LandmarkBase
Const
Ptr
_landmark_ptr
);
// Solver management ----------------------------------------
// Solver management ----------------------------------------
...
...
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