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
3a01d91b
Commit
3a01d91b
authored
5 years ago
by
Joan Solà Ortega
Browse files
Options
Downloads
Patches
Plain Diff
Odd odometry computation to ProcOdomIcp
parent
f75c1241
No related branches found
No related tags found
2 merge requests
!30
Release after RAL
,
!29
After 2nd RAL submission
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/processor/processor_odom_icp.cpp
+60
-108
60 additions, 108 deletions
src/processor/processor_odom_icp.cpp
test/gtest_processor_odom_icp.cpp
+0
-1
0 additions, 1 deletion
test/gtest_processor_odom_icp.cpp
with
60 additions
and
109 deletions
src/processor/processor_odom_icp.cpp
+
60
−
108
View file @
3a01d91b
#include
"laser/processor/processor_odom_icp.h"
#include
"core/common/wolf.h"
#include
"laser/math/laser_tools.h"
#include
<core/common/wolf.h>
using
namespace
wolf
;
using
namespace
laserscanutils
;
ProcessorOdomIcp
::
ProcessorOdomIcp
(
ParamsProcessorOdomIcpPtr
_params
)
:
ProcessorTracker
(
"ProcessorOdomIcp"
,
"PO"
,
2
,
_params
)
ProcessorTracker
(
"ProcessorOdomIcp"
,
"PO"
,
2
,
_params
),
IsMotion
(
"PO"
)
{
proc_params_
=
_params
;
...
...
@@ -53,7 +56,6 @@ ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
trf_origin_last_
.
res_covar
=
Eigen
::
Matrix3d
::
Identity
();
trf_origin_incoming_
.
res_covar
=
Eigen
::
Matrix3d
::
Identity
();
trf_last_incoming_
.
res_covar
=
Eigen
::
Matrix3d
::
Identity
();
setStateStructure
(
"PO"
);
}
ProcessorOdomIcp
::~
ProcessorOdomIcp
()
...
...
@@ -84,46 +86,12 @@ unsigned int ProcessorOdomIcp::processKnown()
CaptureLaser2dPtr
origin_ptr
=
std
::
static_pointer_cast
<
CaptureLaser2d
>
(
origin_ptr_
);
CaptureLaser2dPtr
incoming_ptr
=
std
::
static_pointer_cast
<
CaptureLaser2d
>
(
incoming_ptr_
);
// WOLF_INFO("max_correspondence_dist ", this->icp_params_.max_correspondence_dist);
// WOLF_INFO("max_iterations ", this->icp_params_.max_iterations);
// WOLF_INFO("outliers_adaptive_mult ", this->icp_params_.outliers_adaptive_mult);
// WOLF_INFO("outliers_adaptive_order ", this->icp_params_.outliers_adaptive_order);
// WOLF_INFO("outliers_maxPerc ", this->icp_params_.outliers_maxPerc);
// WOLF_INFO("use_corr_tricks ", this->icp_params_.use_corr_tricks);
// WOLF_INFO("use_point_to_line_distance ", this->icp_params_.use_point_to_line_distance);
// WOLF_INFO("angle_max_ ", this->laser_scan_params_.angle_max_);
// WOLF_INFO("angle_min_ ", this->laser_scan_params_.angle_min_);
// WOLF_INFO("angle_std_dev_ ", this->laser_scan_params_.angle_std_dev_);
// WOLF_INFO("angle_step_ ", this->laser_scan_params_.angle_step_);
// WOLF_INFO("range_max_ ", this->laser_scan_params_.range_max_);
// WOLF_INFO("range_min_ ", this->laser_scan_params_.range_min_);
// WOLF_INFO("range_std_dev_ ", this->laser_scan_params_.range_std_dev_);
// WOLF_INFO("scan_time_ ", this->laser_scan_params_.scan_time_);
// WOLF_INFO("ODOM Icp: Aligning scans ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
// WOLF_INFO("Laser scan params\n");
// this->laser_scan_params_.print();
// WOLF_INFO("Icp params\n");
// WOLF_INFO("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist,
// "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance, "\n max_iterations: ", this->icp_params_.max_iterations,
// "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult,
// "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order, "\n outliers_maxPerc: ", this->icp_params_.outliers_maxPerc,
// "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
trf_origin_incoming_
=
icp_tools_ptr_
->
align
(
incoming_ptr
->
getScan
(),
origin_ptr
->
getScan
(),
this
->
laser_scan_params_
,
this
->
icp_params_
,
this
->
trf_origin_last_
.
res_transf
);
// Check order
// double score_normalized
// ((double)trf_origin_incoming_.nvalid / (double)min(incoming_ptr->getScan().ranges_raw_.size(), origin_ptr->getScan().ranges_raw_.size()));
// double mean_error = trf_origin_incoming_.error / trf_origin_incoming_.nvalid;
// WOLF_INFO("DBG ------------------------------");
// WOLF_INFO("DBG valid? ", trf_origin_incoming_.valid, " m_er ", mean_error, " ", score_normalized * 100, "% own_id: ", incoming_ptr->id(),
// " other_id: ", origin_ptr->id());
// WOLF_INFO("DBG own_POSE: 0 0 0 other_POSE: ", this->trf_origin_last_.res_transf.transpose(),
// " Icp_guess: ", this->trf_origin_last_.res_transf.transpose(), " Icp_trf_origin_incoming_: ", trf_origin_incoming_.res_transf.transpose());
// WOLF_INFO("DBG odometry");
trf_origin_incoming_
.
valid
=
trf_origin_incoming_
.
valid
&&
trf_origin_incoming_
.
error
/
trf_origin_incoming_
.
nvalid
<
5e-2
;
}
return
0
;
...
...
@@ -134,7 +102,6 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
Eigen
::
Vector3d
t_identity
;
t_identity
<<
0
,
0
,
0
;
// XXX: Dynamic or static? JS: static is OK.
CaptureLaser2dPtr
incoming_ptr
=
std
::
static_pointer_cast
<
CaptureLaser2d
>
(
incoming_ptr_
);
CaptureLaser2dPtr
last_ptr
=
std
::
static_pointer_cast
<
CaptureLaser2d
>
(
last_ptr_
);
...
...
@@ -144,28 +111,8 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
this
->
icp_params_
,
t_identity
);
// WOLF_INFO("ODOM Icp: Aligning scans ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
// WOLF_INFO("Laser scan params\n");
// this->laser_scan_params_.print();
// WOLF_INFO("Icp params\n");
// WOLF_INFO("\n max_correspondence_dist: ", this->icp_params_.max_correspondence_dist, "\n use_point_to_line_distance: ", this->icp_params_.use_point_to_line_distance, "\n max_iterations: ", this->icp_params_.max_iterations,
// "\n outliers_adaptive_mult: ", this->icp_params_.outliers_adaptive_mult, "\n outliers_adaptive_order: ", this->icp_params_.outliers_adaptive_order, "\n outliers_maxPerc: ",
// this->icp_params_.outliers_maxPerc, "\n use corr tricks: ", this->icp_params_.use_corr_tricks);
trf_last_incoming_
=
icp_tools_ptr_
->
align
(
incoming_ptr
->
getScan
(),
last_ptr
->
getScan
(),
this
->
laser_scan_params_
,
this
->
icp_params_
,
t_identity
);
//TEST! TO BE REMOVED
// trf_last_incoming_.res_transf = t_identity;
// WOLF_TRACE("trf_last_incoming_: ", trf_last_incoming_.res_transf.transpose());
// Eigen::Matrix2s R_last;
// double alphal = trf_origin_last_.res_transf(2);
// double alphali = trf_origin_incoming_.res_transf(2) - alphal;
// R_last << cos(alphal), -sin(alphal), sin(alphal), cos(alphal);
// Eigen::Vector3s trf_last_incoming_check;
// trf_last_incoming_check.head(2) = R_last.transpose() * (trf_origin_incoming_.res_transf.head(2) - trf_origin_last_.res_transf.head(2));
// trf_last_incoming_check(2) = alphali;
// WOLF_TRACE("trf_last_incoming_check: ", trf_last_incoming_check.transpose());
trf_last_incoming_
.
valid
=
trf_last_incoming_
.
valid
&&
trf_last_incoming_
.
error
/
trf_last_incoming_
.
nvalid
<
5e-2
;
return
0
;
...
...
@@ -236,60 +183,69 @@ void ProcessorOdomIcp::advanceDerived()
// overwrite last frame's state
Isometry2d
w_T_ro
=
Translation2d
(
origin_ptr_
->
getFrame
()
->
getState
().
at
(
'P'
))
*
Rotation2Dd
(
origin_ptr_
->
getFrame
()
->
getState
().
at
(
'O'
)(
0
));
Isometry2d
ro_T_so
=
Translation2d
(
origin_ptr_
->
getSensorP
()
->
getState
())
*
Rotation2Dd
(
origin_ptr_
->
getSensorO
()
->
getState
()(
0
));
Isometry2d
&
ri_T_si
=
ro_T_so
;
// A reference just to have nice names without computing overhead
auto
w_T_ro
=
laser
::
trf2isometry
(
origin_ptr_
->
getFrame
()
->
getP
()
->
getState
(),
origin_ptr_
->
getFrame
()
->
getO
()
->
getState
());
auto
ro_T_so
=
laser
::
trf2isometry
(
origin_ptr_
->
getSensorP
()
->
getState
(),
origin_ptr_
->
getSensorO
()
->
getState
());
const
auto
&
ri_T_si
=
ro_T_so
;
// A reference just to have nice names without computing overhead
// incoming
Isometry2d
so_T_si
=
Translation2d
(
trf_origin_incoming_
.
res_transf
.
head
(
2
))
*
Rotation2Dd
(
trf_origin_incoming_
.
res_transf
(
2
)
);
auto
so_T_si
=
laser
::
trf2isometry
(
trf_origin_incoming_
.
res_transf
);
Isometry2d
w_T_ri
=
w_T_ro
*
ro_T_so
*
so_T_si
*
ri_T_si
.
inverse
();
Vector3d
x_inc
;
x_inc
<<
w_T_ri
.
translation
()
,
Rotation2Dd
(
w_T_ri
.
rotation
()).
angle
();
// Put the state of incoming in last
last_ptr_
->
getFrame
()
->
setState
(
x_inc
,
"PO"
,
{
2
,
1
});
// computing odometry
auto
so_T_sl
=
laser
::
trf2isometry
(
trf_origin_last_
.
res_transf
);
Isometry2d
sl_T_si
=
so_T_sl
.
inverse
()
*
so_T_si
;
odometry_
[
'P'
]
=
sl_T_si
.
translation
();
odometry_
[
'O'
]
=
Vector1d
(
Rotation2Dd
(
sl_T_si
.
rotation
()).
angle
());
// advance transforms
trf_origin_last_
=
trf_origin_incoming_
;
}
void
ProcessorOdomIcp
::
resetDerived
()
{
// WOLF_INFO("NDEBUG origin ", origin_ptr_->id(), " last ", last_ptr_->id(), " error ", trf_origin_last_.error, " merror ", trf_origin_last_.error/(double) trf_origin_last_.nvalid, " nvalid ", trf_origin_last_.nvalid, " sqrt diag. ", trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose(), " trnsf ", trf_origin_last_.res_transf.transpose());
if
(
origin_ptr_
!=
nullptr
and
last_ptr_
!=
nullptr
)
{
// std::cout << "NDEBUG origin " << std::setw(4) << origin_ptr_->id()
// << " last " << std::setw(4) << last_ptr_->id()
// << " is valid? " << std::setw(4) << trf_origin_last_.valid
// << " error " << std::setw(8) << trf_origin_last_.error
// << " merror " << std::setw(10) << trf_origin_last_.error / (double)trf_origin_last_.nvalid
// << " nvalid " << std::setw(3) << trf_origin_last_.nvalid
// << " sqrt diag. " << std::setw(10) << trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose()
// << " trnsf " << std::setw(10) << trf_origin_last_.res_transf.transpose() << std::endl;
}
// Using processing_step_ to ensure that origin, last and incoming are different
if
(
processing_step_
!=
FIRST_TIME_WITH_PACK
&&
processing_step_
!=
FIRST_TIME_WITHOUT_PACK
)
{
// Notation explanation:
// x1_R_x2: Rotation from frame x1 to frame x2
// x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
Isometry2d
w_T_ro
=
Translation2d
(
origin_ptr_
->
getFrame
()
->
getP
()
->
getState
())
*
Rotation2Dd
(
origin_ptr_
->
getFrame
()
->
getO
()
->
getState
()(
0
));
Isometry2d
ro_T_so
=
Translation2d
(
origin_ptr_
->
getSensorP
()
->
getState
())
*
Rotation2Dd
(
origin_ptr_
->
getSensorO
()
->
getState
()(
0
));
Isometry2d
&
ri_T_si
=
ro_T_so
;
// A reference just to have nice names without computing overhead
// incoming
Isometry2d
so_T_si
=
Translation2d
(
trf_origin_incoming_
.
res_transf
.
head
(
2
))
*
Rotation2Dd
(
trf_origin_incoming_
.
res_transf
(
2
));
Isometry2d
w_T_ri
=
w_T_ro
*
ro_T_so
*
so_T_si
*
ri_T_si
.
inverse
();
Vector3d
x_current
;
x_current
<<
w_T_ri
.
translation
(),
Rotation2Dd
(
w_T_ri
.
rotation
()).
angle
();
// last
last_ptr_
->
getFrame
()
->
setState
(
x_current
,
"PO"
,
{
2
,
1
});
// incoming_ptr_->getFrame()->setState(x_current);
trf_origin_last_
=
trf_last_incoming_
;
// WOLF_INFO("NDEBUG origin ", origin_ptr_->id(), " last ", last_ptr_->id(), " error ", trf_origin_last_.error, " merror ", trf_origin_last_.error/(double) trf_origin_last_.nvalid, " nvalid ", trf_origin_last_.nvalid, " sqrt diag. ", trf_origin_last_.res_covar.diagonal().cwiseSqrt().transpose(), " trnsf ", trf_origin_last_.res_transf.transpose());
if
(
origin_ptr_
!=
nullptr
and
last_ptr_
!=
nullptr
)
{
//
}
// Using processing_step_ to ensure that origin, last and incoming are different
if
(
processing_step_
!=
FIRST_TIME_WITH_PACK
&&
processing_step_
!=
FIRST_TIME_WITHOUT_PACK
)
{
// Notation explanation:
// x1_R_x2: Rotation from frame x1 to frame x2
// x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
auto
w_T_ro
=
laser
::
trf2isometry
(
origin_ptr_
->
getFrame
()
->
getP
()
->
getState
(),
origin_ptr_
->
getFrame
()
->
getO
()
->
getState
());
auto
ro_T_so
=
laser
::
trf2isometry
(
origin_ptr_
->
getSensorP
()
->
getState
(),
origin_ptr_
->
getSensorO
()
->
getState
());
const
auto
&
ri_T_si
=
ro_T_so
;
// A reference just to have nice names without computing overhead
// incoming
auto
so_T_si
=
laser
::
trf2isometry
(
trf_origin_incoming_
.
res_transf
);
Isometry2d
w_T_ri
=
w_T_ro
*
ro_T_so
*
so_T_si
*
ri_T_si
.
inverse
();
Vector3d
x_current
;
x_current
<<
w_T_ri
.
translation
(),
Rotation2Dd
(
w_T_ri
.
rotation
()).
angle
();
// last
last_ptr_
->
getFrame
()
->
setState
(
x_current
,
"PO"
,
{
2
,
1
});
// incoming_ptr_->getFrame()->setState(x_current);
// computing odometry
auto
so_T_sl
=
laser
::
trf2isometry
(
trf_origin_last_
.
res_transf
);
Isometry2d
sl_T_si
=
so_T_sl
.
inverse
()
*
so_T_si
;
odometry_
[
'P'
]
=
sl_T_si
.
translation
();
odometry_
[
'O'
]
=
Vector1d
(
Rotation2Dd
(
sl_T_si
.
rotation
()).
angle
());
// advance transforms
trf_origin_last_
=
trf_last_incoming_
;
}
}
...
...
@@ -321,21 +277,17 @@ FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature)
VectorComposite
ProcessorOdomIcp
::
getState
(
const
StateStructure
&
_structure
)
const
{
Isometry2d
w_T_ro
=
Translation2d
(
origin_ptr_
->
getFrame
()
->
getP
()
->
getState
()
)
*
Rotation2Dd
(
origin_ptr_
->
getFrame
()
->
getO
()
->
getState
()
(
0
)
);
Isometry2d
ro_T_so
=
Translation2d
(
origin_ptr_
->
getSensorP
()
->
getState
()
)
*
Rotation2Dd
(
origin_ptr_
->
getSensorO
()
->
getState
()
(
0
)
);
Isometry2d
&
ri
_T_s
i
=
ro_T_so
;
// A reference just to have nice names without computing overhead
auto
w_T_ro
=
laser
::
trf2isometry
(
origin_ptr_
->
getFrame
()
->
getP
()
->
getState
()
,
origin_ptr_
->
getFrame
()
->
getO
()
->
getState
());
auto
ro_T_so
=
laser
::
trf2isometry
(
origin_ptr_
->
getSensorP
()
->
getState
()
,
origin_ptr_
->
getSensorO
()
->
getState
());
const
auto
&
rl
_T_s
l
=
ro_T_so
;
// A reference just to have nice names without computing overhead
// incoming
Isometry2d
so_T_s
i
=
Translation2d
(
trf_origin_last_
.
res_transf
.
head
(
2
))
*
Rotation2Dd
(
trf_origin_last_
.
res_transf
(
2
)
);
Isometry2d
w_T_r
i
=
w_T_ro
*
ro_T_so
*
so_T_s
i
*
r
i
_T_s
i
.
inverse
();
auto
so_T_s
l
=
laser
::
trf2isometry
(
trf_origin_last_
.
res_transf
);
Isometry2d
w_T_r
l
=
w_T_ro
*
ro_T_so
*
so_T_s
l
*
r
l
_T_s
l
.
inverse
();
Vector3d
x_current
;
VectorComposite
state
(
"PO"
,
{
w_T_ri
.
translation
(),
Vector1d
(
Rotation2Dd
(
w_T_ri
.
rotation
()).
angle
())});
// Eigen::Vector3d state = Vector3d(0, 0, 0);
// state.head(2) << w_T_ri.translation();
// state(2) = Rotation2Dd(w_T_ri.rotation()).angle();
VectorComposite
state
(
"PO"
,
{
w_T_rl
.
translation
(),
Vector1d
(
Rotation2Dd
(
w_T_rl
.
rotation
()).
angle
())});
return
state
;
}
...
...
This diff is collapsed.
Click to expand it.
test/gtest_processor_odom_icp.cpp
+
0
−
1
View file @
3a01d91b
...
...
@@ -161,7 +161,6 @@ TEST_F(ProcessorOdomIcp_Test, solve)
for
(
auto
F
:
problem
->
getTrajectory
()
->
getFrameList
())
{
if
(
F
->
isKey
())
// F->setState(0.5 * Vector3d::Random());
F
->
perturb
(
0.5
);
}
...
...
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