Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
L
Lidar Odometry
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Requirements
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Locked files
Build
Pipelines
Jobs
Pipeline schedules
Test cases
Artifacts
Deploy
Releases
Package registry
Container registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Code review analytics
Issue analytics
Insights
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
echelon
Lidar Odometry
Commits
99e91f17
Commit
99e91f17
authored
5 years ago
by
echelon
Browse files
Options
Downloads
Patches
Plain Diff
implemented spatialite approach
parent
412bc681
Branches
Branches containing commit
Tags
Tags containing commit
No related merge requests found
Changes
2
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/LidarOdometry.h
+58
-2
58 additions, 2 deletions
include/LidarOdometry.h
src/LidarOdometry.cpp
+932
-10
932 additions, 10 deletions
src/LidarOdometry.cpp
with
990 additions
and
12 deletions
include/LidarOdometry.h
+
58
−
2
View file @
99e91f17
...
...
@@ -4,6 +4,7 @@
#include
<ros/ros.h>
#include
<nodelet/nodelet.h>
#include
<tf2_ros/transform_listener.h>
#include
<sqlite3.h>
#include
<geometry_msgs/PoseWithCovarianceStamped.h>
#include
<nav_msgs/Odometry.h>
...
...
@@ -20,20 +21,75 @@ namespace fub_lidar_odometry {
class
LidarOdometry
:
public
nodelet
::
Nodelet
{
public:
typedef
std
::
map
<
long
,
std
::
map
<
long
,
std
::
pair
<
double
,
double
>
>
>
FingerprintMap
;
LidarOdometry
();
virtual
~
LidarOdometry
();
virtual
void
onInit
()
override
;
struct
{
bool
operator
()(
const
std
::
tuple
<
double
,
double
,
double
,
double
,
pole_based_localization
::
Pole
,
unsigned
int
,
double
>&
i
,
const
std
::
tuple
<
double
,
double
,
double
,
double
,
pole_based_localization
::
Pole
,
unsigned
int
,
double
>&
j
)
const
{
// squared x,y dist is enough...
double
iDist
=
std
::
pow
(
std
::
get
<
1
>
(
i
)
-
std
::
get
<
4
>
(
i
).
pose
.
position
.
x
,
2.0
)
+
std
::
pow
(
std
::
get
<
2
>
(
i
)
-
std
::
get
<
4
>
(
i
).
pose
.
position
.
y
,
2.0
);
double
jDist
=
std
::
pow
(
std
::
get
<
1
>
(
j
)
-
std
::
get
<
4
>
(
j
).
pose
.
position
.
x
,
2.0
)
+
std
::
pow
(
std
::
get
<
2
>
(
j
)
-
std
::
get
<
4
>
(
j
).
pose
.
position
.
y
,
2.0
);
unsigned
int
distCounti
=
std
::
get
<
5
>
(
i
),
distCountj
=
std
::
get
<
5
>
(
j
);
// if dist count equal sort by distance (close to far)
if
(
distCounti
==
distCountj
)
return
iDist
<=
jDist
;
return
std
::
get
<
5
>
(
i
)
>
std
::
get
<
5
>
(
j
);
}
}
distCompare
;
protected
:
//void callbackFeatures(const fub_feature_common::Features& features);
void
callbackFeatures
(
const
pole_based_localization
::
PoleArray
&
features
);
void
callbackPosition
(
const
nav_msgs
::
Odometry
&
odom
);
void
updateFingerprints
(
const
pole_based_localization
::
PoleArray
&
poles
,
FingerprintMap
&
fingerprints
,
std
::
map
<
long
,
pole_based_localization
::
Pole
>&
poleMap
)
const
;
void
cleanPolesNPrints
(
std
::
map
<
long
,
pole_based_localization
::
Pole
>
&
poleMap
,
FingerprintMap
&
fingerprints
)
const
;
void
insertPrints
(
std
::
map
<
long
,
pole_based_localization
::
Pole
>
&
poleMap
,
FingerprintMap
&
fingerprints
)
const
;
std
::
vector
<
std
::
tuple
<
pole_based_localization
::
Pole
,
std
::
vector
<
std
::
tuple
<
double
,
double
,
double
,
double
,
pole_based_localization
::
Pole
,
unsigned
int
,
double
>
>
>
>
dbAssociation
(
const
std
::
vector
<
long
>::
const_iterator
fpbegin
,
const
std
::
vector
<
long
>::
const_iterator
fpend
,
const
FingerprintMap
&
fpMap
,
const
std
::
map
<
long
,
pole_based_localization
::
Pole
>
&
poleMap
,
sqlite3
*
db
,
sqlite3_stmt
*
poleStmt
,
sqlite3_stmt
*
wallStmt
)
const
;
std
::
vector
<
std
::
pair
<
pole_based_localization
::
Pole
,
pole_based_localization
::
Pole
>
>
associate
(
const
FingerprintMap
&
fingerprints
,
const
std
::
map
<
long
,
pole_based_localization
::
Pole
>
&
poleMap
)
const
;
geometry_msgs
::
Pose
calculateOffset
(
const
std
::
vector
<
std
::
pair
<
pole_based_localization
::
Pole
,
pole_based_localization
::
Pole
>
>&
associations
)
const
;
std
::
array
<
double
,
3
>
calcVariances
(
const
std
::
vector
<
std
::
pair
<
pole_based_localization
::
Pole
,
pole_based_localization
::
Pole
>
>&
matchings
)
const
;
ros
::
Subscriber
mFeaturesSubscriber
;
ros
::
Publisher
mOdometryPublisher
;
double
calcDistSquared
(
const
geometry_msgs
::
Point
&
point1
,
const
geometry_msgs
::
Point
&
point2
)
const
;
double
calcRangeDistSquared
(
const
pole_based_localization
::
Pole
&
feat1
,
const
pole_based_localization
::
Pole
&
feat2
)
const
;
double
calcLineDist
(
const
pole_based_localization
::
Pole
&
line
,
const
geometry_msgs
::
Point
&
point
)
const
;
double
calcDist
(
const
pole_based_localization
::
Pole
&
pole1
,
const
pole_based_localization
::
Pole
&
pole2
)
const
;
double
calcDist
(
const
geometry_msgs
::
Point
&
point1
,
const
geometry_msgs
::
Point
&
point2
)
const
;
double
calcPointEdgeAngle
(
const
pole_based_localization
::
Pole
&
pole1
,
const
pole_based_localization
::
Pole
&
pole2
)
const
;
double
calcAngle
(
const
pole_based_localization
::
Pole
&
pole1
,
const
pole_based_localization
::
Pole
&
pole2
)
const
;
double
calcAngle
(
const
tf2
::
Vector3
&
direction
)
const
;
geometry_msgs
::
Point
getClosestPointOnLine
(
const
pole_based_localization
::
Pole
&
line
,
const
geometry_msgs
::
Point
&
point
)
const
;
std
::
pair
<
geometry_msgs
::
Point
,
geometry_msgs
::
Point
>
edgeToPoints
(
const
pole_based_localization
::
Pole
&
edge
)
const
;
std
::
vector
<
long
>
getPolesInRange
(
const
pole_based_localization
::
Pole
&
origin
,
const
std
::
map
<
long
,
pole_based_localization
::
Pole
>&
poleMap
,
double
range
)
const
;
bool
isEdgeType
(
const
pole_based_localization
::
Pole
&
feature
)
const
;
geometry_msgs
::
Point
map2UTM
(
const
geometry_msgs
::
Point
&
point
,
ros
::
Time
stamp
)
const
;
ros
::
Subscriber
mFeaturesSubscriber
,
mPosSubscriber
;
ros
::
Publisher
mOdometryPublisher
,
mPositionDifferencePublisher
,
mPoleMatchingPublisher
;
std
::
map
<
std
::
string
,
pole_based_localization
::
Pole
>
mLastPoles
;
//std::map<long, fub_feature_common::Pole> mLastPoles;
std
::
map
<
std
::
string
,
fub_feature_common
::
Corner
>
mLastCorners
;
std
::
map
<
std
::
string
,
fub_feature_common
::
Edge
>
mLastEdges
;
static
const
unsigned
short
mMaxDists
=
100
;
bool
mDetectWalls
=
true
;
float
mMaxDistTolerance
=
0.05
f
,
mMaxDistAbsTolerance
=
0.8
f
,
mMinDistAbsTolerance
=
0.2
;
float
mMaxAngleTolerance
=
0.02
f
;
float
mMaxWallAngleTolerance
=
0.02
f
;
float
mMaxSearchRadius
=
2.5
f
;
sqlite3
*
mDatabase
;
sqlite3_stmt
*
mStatement
,
*
mWallStatement
;
geometry_msgs
::
Point
mLastPos
;
tf2_ros
::
Buffer
mTfBuffer
;
tf2_ros
::
TransformListener
mTfListener
;
};
}
...
...
This diff is collapsed.
Click to expand it.
src/LidarOdometry.cpp
+
932
−
10
View file @
99e91f17
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