Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
laser_scan_utils
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
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
labrobotica
algorithms
laser_scan_utils
Commits
3999cba3
Commit
3999cba3
authored
5 years ago
by
Joaquim Casals Buñuel
Browse files
Options
Downloads
Patches
Plain Diff
Removed duplicate align code, now there is only different headers no duplicated code.
parent
27408130
No related branches found
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/icp.cpp
+1
-64
1 addition, 64 deletions
src/icp.cpp
with
1 addition
and
64 deletions
src/icp.cpp
+
1
−
64
View file @
3999cba3
...
@@ -128,70 +128,7 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
...
@@ -128,70 +128,7 @@ icpOutput ICP::align(const LaserScan &_current_ls, const LaserScan &_ref_ls, con
//Legacy code
//Legacy code
icpOutput
ICP
::
align
(
const
LaserScan
&
_current_ls
,
const
LaserScan
&
_ref_ls
,
const
LaserScanParams
&
scan_params
,
const
icpParams
&
icp_params
,
Eigen
::
Vector3s
&
_transf_guess
)
icpOutput
ICP
::
align
(
const
LaserScan
&
_current_ls
,
const
LaserScan
&
_ref_ls
,
const
LaserScanParams
&
scan_params
,
const
icpParams
&
icp_params
,
Eigen
::
Vector3s
&
_transf_guess
)
{
{
// Uncomment to enable debug messages from the CSM library
return
align
(
_current_ls
,
_ref_ls
,
scan_params
,
scan_params
,
icp_params
,
_transf_guess
);
// sm_debug_write(true);
LDWrapper
current
=
LDWrapper
(
_current_ls
,
scan_params
);
LDWrapper
ref
=
LDWrapper
(
_ref_ls
,
scan_params
);
int
num_rays
=
_current_ls
.
ranges_raw_
.
size
();
sm_params
csm_input
{};
sm_result
csm_output
{};
csm_input
.
min_reading
=
scan_params
.
range_min_
;
csm_input
.
max_reading
=
scan_params
.
range_max_
;
csm_input
.
sigma
=
scan_params
.
range_std_dev_
;
csm_input
.
laser_ref
=
ref
.
laser_data
;
csm_input
.
laser_sens
=
current
.
laser_data
;
csm_input
.
first_guess
[
0
]
=
_transf_guess
(
0
);
csm_input
.
first_guess
[
1
]
=
_transf_guess
(
1
);
csm_input
.
first_guess
[
2
]
=
_transf_guess
(
2
);
csm_input
.
use_point_to_line_distance
=
icp_params
.
use_point_to_line_distance
;
csm_input
.
max_correspondence_dist
=
icp_params
.
max_correspondence_dist
;
csm_input
.
max_iterations
=
icp_params
.
max_iterations
;
csm_input
.
use_corr_tricks
=
icp_params
.
use_corr_tricks
;
csm_input
.
outliers_maxPerc
=
icp_params
.
outliers_maxPerc
;
csm_input
.
outliers_adaptive_order
=
icp_params
.
outliers_adaptive_order
;
csm_input
.
outliers_adaptive_mult
=
icp_params
.
outliers_adaptive_mult
;
csm_input
.
do_compute_covariance
=
1
;
sm_icp
(
&
csm_input
,
&
csm_output
);
icpOutput
result
{};
result
.
nvalid
=
csm_output
.
nvalid
;
result
.
valid
=
csm_output
.
valid
;
result
.
error
=
csm_output
.
error
;
if
(
result
.
valid
==
1
)
{
result
.
res_transf
(
0
)
=
csm_output
.
x
[
0
];
result
.
res_transf
(
1
)
=
csm_output
.
x
[
1
];
result
.
res_transf
(
2
)
=
csm_output
.
x
[
2
];
for
(
int
i
=
0
;
i
<
3
;
++
i
)
for
(
int
j
=
0
;
j
<
3
;
++
j
)
result
.
res_covar
(
i
,
j
)
=
//gsl_matrix_get(csm_output.cov_x_m, i, j); // NOT COMPILING
csm_output
.
cov_x_m
->
data
[
i
*
csm_output
.
cov_x_m
->
tda
+
j
];
// This does the same
}
else
{
std
::
cout
<<
"ICP valid != 1, providing first guess transformation and identity covariance
\n
"
;
result
.
res_transf
=
_transf_guess
;
result
.
res_covar
=
Eigen
::
Matrix3s
::
Identity
();
}
// std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
// std::cout << "Number of iterations: " << csm_output.iterations << '\n';
// std::cout << "Error: " << csm_output.error << '\n';
return
result
;
}
}
void
ICP
::
printLaserData
(
LDP
&
laser_data
)
void
ICP
::
printLaserData
(
LDP
&
laser_data
)
...
...
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