Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
C
csm
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Model registry
Operate
Environments
Terraform modules
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
csm
Commits
bb90ff04
Commit
bb90ff04
authored
15 years ago
by
blackcoder
Committed by
BlackCoder
15 years ago
Browse files
Options
Downloads
Patches
Plain Diff
- A prototype cpp wrapper
parent
1e0097f5
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
sm/CMakeLists.txt
+1
-1
1 addition, 1 deletion
sm/CMakeLists.txt
sm/csmpp/CMakeLists.txt
+3
-5
3 additions, 5 deletions
sm/csmpp/CMakeLists.txt
sm/csmpp/csmpp.cpp
+58
-3
58 additions, 3 deletions
sm/csmpp/csmpp.cpp
sm/csmpp/csmpp.h
+293
-1
293 additions, 1 deletion
sm/csmpp/csmpp.h
with
355 additions
and
10 deletions
sm/CMakeLists.txt
+
1
−
1
View file @
bb90ff04
...
@@ -71,7 +71,7 @@ SUBDIRS(lib/json-c)
...
@@ -71,7 +71,7 @@ SUBDIRS(lib/json-c)
SUBDIRS
(
lib/egsl
)
SUBDIRS
(
lib/egsl
)
SUBDIRS
(
lib/gpc
)
SUBDIRS
(
lib/gpc
)
SUBDIRS
(
csm
)
SUBDIRS
(
csm
)
SUBDIRS
(
csmpp
)
...
...
This diff is collapsed.
Click to expand it.
sm/csmpp/CMakeLists.txt
+
3
−
5
View file @
bb90ff04
...
@@ -5,7 +5,7 @@ SET(csmpp_version 0.1)
...
@@ -5,7 +5,7 @@ SET(csmpp_version 0.1)
# Require we have pkgconfig installed
# Require we have pkgconfig installed
find_package
(
PkgConfig REQUIRED
)
find_package
(
PkgConfig REQUIRED
)
# Tell pkgconfig to look for CSM
# Tell pkgconfig to look for CSM
pkg_check_modules
(
CSM REQUIRED csm
)
#
pkg_check_modules(CSM REQUIRED csm)
# Look for CSM
# Look for CSM
IF
(
${
CSM_FOUND
}
)
IF
(
${
CSM_FOUND
}
)
...
@@ -13,18 +13,16 @@ IF(${CSM_FOUND})
...
@@ -13,18 +13,16 @@ IF(${CSM_FOUND})
MESSAGE
(
"CSM_LIBRARIES:
${
CSM_LIBRARIES
}
"
)
MESSAGE
(
"CSM_LIBRARIES:
${
CSM_LIBRARIES
}
"
)
MESSAGE
(
"CSM_INCLUDE_DIRS:
${
CSM_INCLUDE_DIRS
}
"
)
MESSAGE
(
"CSM_INCLUDE_DIRS:
${
CSM_INCLUDE_DIRS
}
"
)
MESSAGE
(
"CSM_LDFLAGS: "
${
CSM_LDFLAGS
}
)
MESSAGE
(
"CSM_LDFLAGS: "
${
CSM_LDFLAGS
}
)
INCLUDE_DIRECTORIES
(
${
CSM_INCLUDE_DIRS
}
)
# important!
INCLUDE_DIRECTORIES
(
${
CSM_INCLUDE_DIRS
}
)
# important!
LINK_DIRECTORIES
(
${
CSM_LIBRARY_DIRS
}
)
# important!
LINK_DIRECTORIES
(
${
CSM_LIBRARY_DIRS
}
)
# important!
ELSE
(
${
CSM_FOUND
}
)
ELSE
(
${
CSM_FOUND
}
)
MESSAGE
(
FATAL_ERROR
"CSM not found. Check that the environment variable PKG_CONFIG_PATH includes the path containing the file 'csm.pc'."
)
#
MESSAGE(FATAL_ERROR "CSM not found. Check that the environment variable PKG_CONFIG_PATH includes the path containing the file 'csm.pc'.")
ENDIF
(
${
CSM_FOUND
}
)
ENDIF
(
${
CSM_FOUND
}
)
# Name of generated libraries
# Name of generated libraries
SET
(
dynamic_lib csmpp
)
SET
(
dynamic_lib csmpp
)
SET
(
static_lib csmpp-static
)
SET
(
static_lib csmpp-static
)
SET
(
csmpp_sources csmpp.cpp
)
SET
(
csmpp_sources csmpp.cpp
csmpp.h
)
SET
(
csmpp_c_flags
""
)
SET
(
csmpp_c_flags
""
)
SET
(
csmpp_link_flags
${
CSM_LDFLAGS
}
)
SET
(
csmpp_link_flags
${
CSM_LDFLAGS
}
)
...
...
This diff is collapsed.
Click to expand it.
sm/csmpp/csmpp.cpp
+
58
−
3
View file @
bb90ff04
#include
"csmpp.h"
#include
"csmpp.h"
CanonicalScanMatcher
::
CanonicalScanMatcher
(
SMParameters
parameters
)
:
params
(
parameters
),
showDebug
(
0
),
recoverFromError
(
0
),
matchingAlgorithm
(
ICP
)
{
void
test_function
()
{
}
}
CanonicalScanMatcher
::
CanonicalScanMatcher
()
:
\ No newline at end of file
showDebug
(
0
),
recoverFromError
(
0
),
matchingAlgorithm
(
ICP
)
{
}
CanonicalScanMatcher
::~
CanonicalScanMatcher
()
{
}
bool
CanonicalScanMatcher
::
scanMatch
(
LDP
refScan
,
LDP
secondScan
)
{
params
.
setLaserRef
(
refScan
);
params
.
setLaserSen
(
secondScan
);
switch
(
matchingAlgorithm
)
{
case
(
ICP
):
sm_icp
(
params
.
getParams
(),
&
matchingResult
);
break
;
case
(
GPM
):
sm_gpm
(
params
.
getParams
(),
&
matchingResult
);
break
;
case
(
HSM
):
sm_hsm
(
params
.
getParams
(),
&
matchingResult
);
break
;
default:
sm_error
(
"Unknown algorithm to run: %d.
\n
"
,
matchingAlgorithm
);
return
false
;
}
return
true
;
}
void
CanonicalScanMatcher
::
setShowDebug
(
bool
state
)
{
this
->
showDebug
=
state
;
}
void
CanonicalScanMatcher
::
setSMParameters
(
SMParameters
parameters
)
{
this
->
params
=
parameters
;
}
void
CanonicalScanMatcher
::
setRecoverFromError
(
bool
state
)
{
this
->
recoverFromError
=
state
;
}
This diff is collapsed.
Click to expand it.
sm/csmpp/csmpp.h
+
293
−
1
View file @
bb90ff04
#ifndef CSM_WRAPPER
#define CSM_WRAPPER
void
test_function
();
#include
<iostream>
\ No newline at end of file
#include
<csm/csm_all.h>
class
SMParameters
{
public:
SMParameters
()
{
// Default Values
this
->
smParameters
.
max_angular_correction_deg
=
90
;
this
->
smParameters
.
max_linear_correction
=
2
;
this
->
smParameters
.
max_iterations
=
1000
;
this
->
smParameters
.
epsilon_xy
=
0.0001
;
this
->
smParameters
.
epsilon_theta
=
0.0001
;
this
->
smParameters
.
max_correspondence_dist
=
2
;
this
->
smParameters
.
sigma
=
0.01
;
this
->
smParameters
.
use_corr_tricks
=
1
;
this
->
smParameters
.
restart
=
1
;
this
->
smParameters
.
restart_threshold_mean_error
=
0.01
;
this
->
smParameters
.
restart_dt
=
0.01
;
this
->
smParameters
.
restart_dtheta
=
0.0261799
;
this
->
smParameters
.
clustering_threshold
=
0.05
;
this
->
smParameters
.
orientation_neighbourhood
=
3
;
this
->
smParameters
.
use_point_to_line_distance
=
1
;
this
->
smParameters
.
do_alpha_test
=
0
;
this
->
smParameters
.
do_alpha_test_thresholdDeg
=
20
;
this
->
smParameters
.
outliers_maxPerc
=
0.95
;
this
->
smParameters
.
outliers_adaptive_order
=
0.7
;
this
->
smParameters
.
outliers_adaptive_mult
=
2
;
this
->
smParameters
.
do_visibility_test
=
0
;
this
->
smParameters
.
outliers_remove_doubles
=
1
;
this
->
smParameters
.
do_compute_covariance
=
0
;
this
->
smParameters
.
debug_verify_tricks
=
0
;
this
->
smParameters
.
gpm_theta_bin_size_deg
=
5
;
this
->
smParameters
.
gpm_extend_range_deg
=
15
;
this
->
smParameters
.
gpm_interval
=
1
;
this
->
smParameters
.
min_reading
=
0
;
this
->
smParameters
.
max_reading
=
1000
;
this
->
smParameters
.
use_ml_weights
=
0
;
this
->
smParameters
.
use_sigma_weights
=
0
;
this
->
smParameters
.
hsm
.
linear_cell_size
=
0.03
;
this
->
smParameters
.
hsm
.
angular_cell_size_deg
=
1
;
this
->
smParameters
.
hsm
.
num_angular_hypotheses
=
8
;
this
->
smParameters
.
hsm
.
xc_directions_min_distance_deg
=
10
;
this
->
smParameters
.
hsm
.
xc_ndirections
=
3
;
this
->
smParameters
.
hsm
.
angular_hyp_min_distance_deg
=
10
;
this
->
smParameters
.
hsm
.
linear_xc_max_npeaks
=
5
;
this
->
smParameters
.
hsm
.
linear_xc_peaks_min_distance
=
5
;
}
~
SMParameters
(){}
sm_params
*
getParams
()
{
return
&
this
->
smParameters
;
}
/*! Where to start */
void
setfirstGuess
(
double
firtGuess
[
3
])
{
this
->
smParameters
.
first_guess
[
0
]
=
firtGuess
[
0
];
this
->
smParameters
.
first_guess
[
1
]
=
firtGuess
[
1
];
this
->
smParameters
.
first_guess
[
2
]
=
firtGuess
[
2
];
}
void
setLaserRef
(
LDP
laserRef
)
{
this
->
smParameters
.
laser_ref
=
laserRef
;
}
void
setLaserSen
(
LDP
laserSen
)
{
this
->
smParameters
.
laser_sens
=
laserSen
;
}
/*! Maximum angular displacement between scans (deg)*/
void
setMaxAngularCorrectionDeg
(
double
maxAng
)
{
this
->
smParameters
.
max_angular_correction_deg
=
maxAng
;
}
/*! Maximum translation between scans (m) */
void
setMaxLinearCorrection
(
double
maxLinear
)
{
this
->
smParameters
.
max_linear_correction
=
maxLinear
;
}
/*! When to stop */
void
setMaxIterations
(
int
maxIt
)
{
this
->
smParameters
.
max_iterations
=
maxIt
;
}
/*! A threshold for stopping. */
void
setEpsilonXY
(
double
eps
)
{
this
->
smParameters
.
epsilon_xy
=
eps
;
}
/*! A threshold for stopping. */
void
setEpsilonTheta
(
double
epsTheta
)
{
this
->
smParameters
.
epsilon_theta
=
epsTheta
;
}
/*! Maximum distance for a correspondence to be valid */
void
setMaxCorrespondenceDist
(
double
maxCorrDist
)
{
this
->
smParameters
.
max_correspondence_dist
=
maxCorrDist
;
}
/*! Use smart tricks for finding correspondences. Only influences speed; not convergence. */
void
setUseCorrTricks
(
bool
useTricks
)
{
this
->
smParameters
.
use_corr_tricks
=
useTricks
;
}
/*! Restart if error under threshold (0 or 1)*/
void
setRestart
(
bool
restart
)
{
this
->
smParameters
.
restart
=
restart
;
}
/*! Threshold for restarting */
void
setRestartThresholdMeanError
(
double
restartME
)
{
this
->
smParameters
.
restart_threshold_mean_error
=
restartME
;
}
/*! Displacement for restarting */
void
setRestartDT
(
double
restardt
)
{
this
->
smParameters
.
restart_dt
=
restardt
;
}
/*! Displacement for restarting */
void
setRestartDTheta
(
double
restartdtTheta
)
{
this
->
smParameters
.
restart_dtheta
=
restartdtTheta
;
}
/*
Functions concerning discarding correspondences.
THESE ARE MAGIC NUMBERS -- and they need to be tuned.
*/
/*!
Percentage of correspondences to consider: if 0.9,
always discard the top 10% of correspondences with more error
*/
void
setOutliersMaxPerc
(
double
maxPerc
)
{
this
->
smParameters
.
outliers_maxPerc
=
maxPerc
;
}
/*!
Parameters describing a simple adaptive algorithm for discarding.
1) Order the errors.
2) Choose the percentile according to outliers_adaptive_order.
(if it is 0.7, get the 70% percentile)
3) Define an adaptive threshold multiplying outliers_adaptive_mult
with the value of the error at the chosen percentile.
4) Discard correspondences over the threshold.
This is useful to be conservative; yet remove the biggest errors.
*/
void
setOutliersAdaptiveOrder
(
double
adaptiveOrder
)
/* 0.7 */
{
this
->
smParameters
.
outliers_adaptive_order
=
adaptiveOrder
;
}
void
setOutliersAdaptiveMult
(
double
adaptiveMult
)
/* 2 */
{
this
->
smParameters
.
outliers_adaptive_mult
=
adaptiveMult
;
}
/*! Do not allow two different correspondences to share a point */
void
setOutliersRemoveDoubles
(
bool
removeDoubles
)
{
this
->
smParameters
.
outliers_remove_doubles
=
removeDoubles
;
}
/* Functions that compute and use point orientation for defining matches. */
/*! For now, a very simple max-distance clustering algorithm is used */
void
setClusteringThreshold
(
double
clusteringThresh
)
{
this
->
smParameters
.
clustering_threshold
=
clusteringThresh
;
}
/*! Number of neighbour rays used to estimate the orientation.*/
void
setOrientationNeighbourhood
(
int
orientationNeighbourhood
)
{
this
->
smParameters
.
orientation_neighbourhood
=
orientationNeighbourhood
;
}
/*! Discard correspondences based on the angles */
void
setDoAlphaTest
(
bool
doAlphaTest
)
{
this
->
smParameters
.
do_alpha_test
=
doAlphaTest
;
}
void
setDoAlphaTestThresholdDeg
(
int
alphaTestThresh
)
{
this
->
smParameters
.
do_alpha_test_thresholdDeg
=
alphaTestThresh
;
}
void
setDoVisibilityTest
(
bool
doVisibility
)
{
this
->
smParameters
.
do_visibility_test
=
doVisibility
;
}
/*! If 1, use PlICP; if 0, use vanilla ICP. */
void
setUsePoint2LineDistance
(
bool
usePoint2Line
)
{
this
->
smParameters
.
use_point_to_line_distance
=
usePoint2Line
;
}
/*!
If 1, the field "true_alpha" is used to compute the incidence
beta, and the factor (1/cos^2(beta)) used to weight the impact
of each correspondence. This works fabolously if doing localization,
that is the first scan has no noise.
If "true_alpha" is not available, it uses "alpha".
*/
void
setUseMLWeights
(
bool
useMLWeights
)
{
this
->
smParameters
.
use_ml_weights
=
useMLWeights
;
}
/*! If 1, the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
void
setUseSigmaWeights
(
bool
useSigmaWeights
)
{
this
->
smParameters
.
use_sigma_weights
=
useSigmaWeights
;
}
/*!
Use the method in http://purl.org/censi/2006/icpcov to compute
the matching covariance.
*/
void
setDoComputeCovariance
(
bool
computeCovariance
)
{
this
->
smParameters
.
do_compute_covariance
=
computeCovariance
;
}
/*! Checks that find_correspondences_tricks give the right answer */
void
setDebugVerifyTricks
(
bool
debugVerifyTricks
)
{
this
->
smParameters
.
debug_verify_tricks
=
debugVerifyTricks
;
}
/*! Pose of sensor with respect to robot: used for computing
the first estimate given the odometry. */
void
setLaserPose
(
double
laserPose
[
3
])
{
this
->
smParameters
.
laser
[
0
]
=
laserPose
[
0
]
;
this
->
smParameters
.
laser
[
1
]
=
laserPose
[
1
]
;
this
->
smParameters
.
laser
[
2
]
=
laserPose
[
2
]
;
}
/*! Noise in the scan */
void
setSigma
(
double
sigma
)
{
this
->
smParameters
.
sigma
=
sigma
;
}
/*! mark as invalid ( = don't use ) rays outside of this interval */
void
setMinReading
(
double
minReading
)
{
this
->
smParameters
.
min_reading
=
minReading
;
}
void
setMaxReading
(
double
maxReading
)
{
this
->
smParameters
.
max_reading
=
maxReading
;
}
/*! Parameters specific to GPM (unfinished :-/ ) */
void
setGpmThetaBinSizeDeg
(
double
gpmBinSize
)
{
this
->
smParameters
.
gpm_theta_bin_size_deg
=
gpmBinSize
;
}
void
setGpmExtendRangeDeg
(
double
extendRange
)
{
this
->
smParameters
.
gpm_extend_range_deg
=
extendRange
;
}
void
setGpmInterval
(
int
interval
)
{
this
->
smParameters
.
gpm_interval
=
interval
;
}
/*! Parameter specific to HSM (unfinished :-/ ) */
void
setHSM
(
struct
hsm_params
hsmParams
)
{
this
->
smParameters
.
hsm
=
hsmParams
;
}
private
:
sm_params
smParameters
;
};
class
CanonicalScanMatcher
{
enum
MatchingAlgorithims
{
ICP
,
GPM
,
HSM
};
public
:
CanonicalScanMatcher
();
CanonicalScanMatcher
(
SMParameters
parameters
);
~
CanonicalScanMatcher
();
void
setSMParameters
(
SMParameters
parameters
);
void
setShowDebug
(
bool
);
void
setRecoverFromError
(
bool
);
bool
scanMatch
(
LDP
refScan
,
LDP
secondScan
);
private
:
int
matchingAlgorithm
;
bool
showDebug
;
bool
recoverFromError
;
SMParameters
params
;
sm_result
matchingResult
;
};
#endif
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