add slam_gmapping

2025-06-06 16:15:07 +08:00
parent 9187b9fb85
commit a7b75c31cb
141 changed files with 15992 additions and 0 deletions
@@ -0,0 +1,108 @@
+#include "movement.h"
+#include <gmapping/utils/gvalues.h>
+
+namespace GMapping {
+
+
+FSRMovement::FSRMovement(double f, double s, double r) {
+  this->f = f;     
+  this->s = s;       
+  this->r = r;
+}
+
+FSRMovement::FSRMovement(const FSRMovement& src) {
+  *this = src;
+}
+
+FSRMovement::FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2) {
+  *this = moveBetweenPoints(pt1, pt2);
+}
+
+
+FSRMovement::FSRMovement(const FSRMovement& move1, const FSRMovement& move2) {
+  *this = composeMoves(move1, move2);
+}
+
+void FSRMovement::normalize()
+{
+  if (r >= -M_PI && r < M_PI)
+    return;
+  
+  int multiplier = (int)(r / (2*M_PI));
+  r = r - multiplier*2*M_PI;
+  if (r >= M_PI)
+    r -= 2*M_PI;
+  if (r < -M_PI)
+    r += 2*M_PI;
+}
+
+OrientedPoint FSRMovement::move(const OrientedPoint& pt) const {
+  return movePoint(pt, *this);
+}
+
+void FSRMovement::invert() {
+  *this = invertMove(*this);
+}
+
+void FSRMovement::compose(const FSRMovement& move2) {
+  *this = composeMoves(*this, move2);
+}
+
+
+FSRMovement FSRMovement::composeMoves(const FSRMovement& move1, 
+				      const FSRMovement& move2) {
+  FSRMovement comp;
+  comp.f = cos(move1.r) * move2.f - sin(move1.r) * move2.s + move1.f;
+  comp.s = sin(move1.r) * move2.f + cos(move1.r) * move2.s + move1.s;
+  comp.r = (move1.r  + move2.r);
+  comp.normalize();
+  return comp;
+}
+
+OrientedPoint FSRMovement::movePoint(const OrientedPoint& pt, const FSRMovement& move1) {
+  OrientedPoint pt2(pt);
+  pt2.x += move1.f * cos(pt.theta) - move1.s * sin(pt.theta);
+  pt2.y += move1.f * sin(pt.theta) + move1.s * cos(pt.theta);
+  pt2.theta = (move1.r + pt.theta);
+  pt2.normalize();
+  return pt2;
+}
+
+FSRMovement FSRMovement::moveBetweenPoints(const OrientedPoint& pt1, 
+					   const OrientedPoint& pt2) {
+  FSRMovement move;
+  move.f =   (pt2.y - pt1.y) * sin(pt1.theta) + (pt2.x - pt1.x) * cos(pt1.theta);
+  move.s = + (pt2.y - pt1.y) * cos(pt1.theta) - (pt2.x - pt1.x) * sin(pt1.theta);
+  move.r = (pt2.theta - pt1.theta);
+  move.normalize();
+  return move;
+  
+}
+
+FSRMovement FSRMovement::invertMove(const FSRMovement& move1) {
+  FSRMovement p_inv;
+  p_inv.f = - cos(move1.r) * move1.f - sin(move1.r) * move1.s;
+  p_inv.s =   sin(move1.r) * move1.f - cos(move1.r) * move1.s;
+  p_inv.r = (-move1.r);
+  p_inv.normalize();
+  return p_inv;
+}
+
+
+OrientedPoint FSRMovement::frameTransformation(const OrientedPoint& reference_pt_frame1, 
+					       const OrientedPoint& reference_pt_frame2,
+					       const OrientedPoint& pt_frame1) {
+  OrientedPoint zero;
+
+  FSRMovement itrans_refp1(zero, reference_pt_frame1);
+  itrans_refp1.invert();
+
+  FSRMovement trans_refp2(zero, reference_pt_frame2);
+  FSRMovement trans_pt(zero, pt_frame1);
+
+  FSRMovement tmp = composeMoves( composeMoves(trans_refp2, itrans_refp1), trans_pt);
+  return tmp.move(zero);
+}
+ 
+
+}