movement.cpp
Go to the documentation of this file.
3 
4 namespace GMapping {
5 
6 
7 FSRMovement::FSRMovement(double f, double s, double r) {
8  this->f = f;
9  this->s = s;
10  this->r = r;
11 }
12 
14  *this = src;
15 }
16 
18  *this = moveBetweenPoints(pt1, pt2);
19 }
20 
21 
22 FSRMovement::FSRMovement(const FSRMovement& move1, const FSRMovement& move2) {
23  *this = composeMoves(move1, move2);
24 }
25 
27 {
28  if (r >= -M_PI && r < M_PI)
29  return;
30 
31  int multiplier = (int)(r / (2*M_PI));
32  r = r - multiplier*2*M_PI;
33  if (r >= M_PI)
34  r -= 2*M_PI;
35  if (r < -M_PI)
36  r += 2*M_PI;
37 }
38 
40  return movePoint(pt, *this);
41 }
42 
44  *this = invertMove(*this);
45 }
46 
47 void FSRMovement::compose(const FSRMovement& move2) {
48  *this = composeMoves(*this, move2);
49 }
50 
51 
53  const FSRMovement& move2) {
54  FSRMovement comp;
55  comp.f = cos(move1.r) * move2.f - sin(move1.r) * move2.s + move1.f;
56  comp.s = sin(move1.r) * move2.f + cos(move1.r) * move2.s + move1.s;
57  comp.r = (move1.r + move2.r);
58  comp.normalize();
59  return comp;
60 }
61 
63  OrientedPoint pt2(pt);
64  pt2.x += move1.f * cos(pt.theta) - move1.s * sin(pt.theta);
65  pt2.y += move1.f * sin(pt.theta) + move1.s * cos(pt.theta);
66  pt2.theta = (move1.r + pt.theta);
67  pt2.normalize();
68  return pt2;
69 }
70 
72  const OrientedPoint& pt2) {
74  move.f = (pt2.y - pt1.y) * sin(pt1.theta) + (pt2.x - pt1.x) * cos(pt1.theta);
75  move.s = + (pt2.y - pt1.y) * cos(pt1.theta) - (pt2.x - pt1.x) * sin(pt1.theta);
76  move.r = (pt2.theta - pt1.theta);
77  move.normalize();
78  return move;
79 
80 }
81 
83  FSRMovement p_inv;
84  p_inv.f = - cos(move1.r) * move1.f - sin(move1.r) * move1.s;
85  p_inv.s = sin(move1.r) * move1.f - cos(move1.r) * move1.s;
86  p_inv.r = (-move1.r);
87  p_inv.normalize();
88  return p_inv;
89 }
90 
91 
93  const OrientedPoint& reference_pt_frame2,
94  const OrientedPoint& pt_frame1) {
95  OrientedPoint zero;
96 
97  FSRMovement itrans_refp1(zero, reference_pt_frame1);
98  itrans_refp1.invert();
99 
100  FSRMovement trans_refp2(zero, reference_pt_frame2);
101  FSRMovement trans_pt(zero, pt_frame1);
102 
103  FSRMovement tmp = composeMoves( composeMoves(trans_refp2, itrans_refp1), trans_pt);
104  return tmp.move(zero);
105 }
106 
107 
108 }
static FSRMovement invertMove(const FSRMovement &move1)
Definition: movement.cpp:82
OrientedPoint move(const OrientedPoint &pt) const
Definition: movement.cpp:39
static FSRMovement composeMoves(const FSRMovement &move1, const FSRMovement &move2)
Definition: movement.cpp:52
void compose(const FSRMovement &move2)
Definition: movement.cpp:47
static OrientedPoint frameTransformation(const OrientedPoint &reference_pt_frame1, const OrientedPoint &reference_pt_frame2, const OrientedPoint &pt_frame1)
Definition: movement.cpp:92
static FSRMovement moveBetweenPoints(const OrientedPoint &pt1, const OrientedPoint &pt2)
Definition: movement.cpp:71
static OrientedPoint movePoint(const OrientedPoint &pt, const FSRMovement &move1)
Definition: movement.cpp:62
FSRMovement(double f=0.0, double s=0.0, double r=0.0)
Definition: movement.cpp:7


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20