movement.cpp
Go to the documentation of this file.
00001 #include "movement.h"
00002 #include <gmapping/utils/gvalues.h>
00003 
00004 namespace GMapping {
00005 
00006 
00007 FSRMovement::FSRMovement(double f, double s, double r) {
00008   this->f = f;     
00009   this->s = s;       
00010   this->r = r;
00011 }
00012 
00013 FSRMovement::FSRMovement(const FSRMovement& src) {
00014   *this = src;
00015 }
00016 
00017 FSRMovement::FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2) {
00018   *this = moveBetweenPoints(pt1, pt2);
00019 }
00020 
00021 
00022 FSRMovement::FSRMovement(const FSRMovement& move1, const FSRMovement& move2) {
00023   *this = composeMoves(move1, move2);
00024 }
00025 
00026 void FSRMovement::normalize()
00027 {
00028   if (r >= -M_PI && r < M_PI)
00029     return;
00030   
00031   int multiplier = (int)(r / (2*M_PI));
00032   r = r - multiplier*2*M_PI;
00033   if (r >= M_PI)
00034     r -= 2*M_PI;
00035   if (r < -M_PI)
00036     r += 2*M_PI;
00037 }
00038 
00039 OrientedPoint FSRMovement::move(const OrientedPoint& pt) const {
00040   return movePoint(pt, *this);
00041 }
00042 
00043 void FSRMovement::invert() {
00044   *this = invertMove(*this);
00045 }
00046 
00047 void FSRMovement::compose(const FSRMovement& move2) {
00048   *this = composeMoves(*this, move2);
00049 }
00050 
00051 
00052 FSRMovement FSRMovement::composeMoves(const FSRMovement& move1, 
00053                                       const FSRMovement& move2) {
00054   FSRMovement comp;
00055   comp.f = cos(move1.r) * move2.f - sin(move1.r) * move2.s + move1.f;
00056   comp.s = sin(move1.r) * move2.f + cos(move1.r) * move2.s + move1.s;
00057   comp.r = (move1.r  + move2.r);
00058   comp.normalize();
00059   return comp;
00060 }
00061 
00062 OrientedPoint FSRMovement::movePoint(const OrientedPoint& pt, const FSRMovement& move1) {
00063   OrientedPoint pt2(pt);
00064   pt2.x += move1.f * cos(pt.theta) - move1.s * sin(pt.theta);
00065   pt2.y += move1.f * sin(pt.theta) + move1.s * cos(pt.theta);
00066   pt2.theta = (move1.r + pt.theta);
00067   pt2.normalize();
00068   return pt2;
00069 }
00070 
00071 FSRMovement FSRMovement::moveBetweenPoints(const OrientedPoint& pt1, 
00072                                            const OrientedPoint& pt2) {
00073   FSRMovement move;
00074   move.f =   (pt2.y - pt1.y) * sin(pt1.theta) + (pt2.x - pt1.x) * cos(pt1.theta);
00075   move.s = + (pt2.y - pt1.y) * cos(pt1.theta) - (pt2.x - pt1.x) * sin(pt1.theta);
00076   move.r = (pt2.theta - pt1.theta);
00077   move.normalize();
00078   return move;
00079   
00080 }
00081 
00082 FSRMovement FSRMovement::invertMove(const FSRMovement& move1) {
00083   FSRMovement p_inv;
00084   p_inv.f = - cos(move1.r) * move1.f - sin(move1.r) * move1.s;
00085   p_inv.s =   sin(move1.r) * move1.f - cos(move1.r) * move1.s;
00086   p_inv.r = (-move1.r);
00087   p_inv.normalize();
00088   return p_inv;
00089 }
00090 
00091 
00092 OrientedPoint FSRMovement::frameTransformation(const OrientedPoint& reference_pt_frame1, 
00093                                                const OrientedPoint& reference_pt_frame2,
00094                                                const OrientedPoint& pt_frame1) {
00095   OrientedPoint zero;
00096 
00097   FSRMovement itrans_refp1(zero, reference_pt_frame1);
00098   itrans_refp1.invert();
00099 
00100   FSRMovement trans_refp2(zero, reference_pt_frame2);
00101   FSRMovement trans_pt(zero, pt_frame1);
00102 
00103   FSRMovement tmp = composeMoves( composeMoves(trans_refp2, itrans_refp1), trans_pt);
00104   return tmp.move(zero);
00105 }
00106  
00107 
00108 }


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13