Use "pose change" instead of "velocities" for (dxy_mm, dtheta_degrees, dt_seconds)

.gitignore

@@ -3,10 +3,11 @@ examples/*.pyc
 examples/*.pgm
 examples/*.png
 examples/*.map
+examples/log2pgm
 python/breezyslam/*.pyc
 python/breezyslam/__pycache__
 python/build
-cpp/*.o
+*.o
-cpp/*.so
+*.so
 *.swp
 

cpp/Makefile

@@ -50,11 +50,11 @@ libbreezyslam.$(LIBEXT): algorithms.o  Scan.o Map.o WheeledRobot.o \
                         coreslam.o coreslam_$(ARCH).o random.o ziggurat.o \
           -o libbreezyslam.$(LIBEXT) -lm
 
-algorithms.o: algorithms.cpp algorithms.hpp Laser.hpp Position.hpp Map.hpp Scan.hpp Velocities.hpp \
+algorithms.o: algorithms.cpp algorithms.hpp Laser.hpp Position.hpp Map.hpp Scan.hpp PoseChange.hpp \
                WheeledRobot.hpp ../c/coreslam.h 
 	g++ -O3 -I../c -c -Wall $(CFLAGS) algorithms.cpp
 
-Scan.o: Scan.cpp Scan.hpp Velocities.hpp Laser.hpp ../c/coreslam.h
+Scan.o: Scan.cpp Scan.hpp PoseChange.hpp Laser.hpp ../c/coreslam.h
 	g++ -O3 -I../c -c -Wall $(CFLAGS) Scan.cpp
 
 Map.o: Map.cpp Map.hpp Position.hpp Scan.hpp ../c/coreslam.h

cpp/PoseChange.hpp

@@ -1,6 +1,6 @@
 /**
 * 
-* Velocities.hpp - C++ header for Velocities class
+* PoseChange.hpp - C++ header for PoseChange class
 *
 * Copyright (C) 2014 Simon D. Levy
 
@@ -27,18 +27,18 @@ using namespace std;
 
 
 /**
-* A class representing the forward and angular velocities of a robot.
+* A class representing the forward and angular poseChange of a robot.
 */
-class Velocities 
+class PoseChange 
 {    
     friend class Scan;
     
 public:
     
 /**
-* Creates a new Velocities object with specified velocities.
+* Creates a new PoseChange object with specified poseChange.
 */
-Velocities(double dxy_mm, double dtheta_degrees, double dtSeconds)
+PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds)
 {
     this->dxy_mm = dxy_mm;
     this->dtheta_degrees = dtheta_degrees;
@@ -46,9 +46,9 @@ Velocities(double dxy_mm, double dtheta_degrees, double dtSeconds)
 }
 
 /**
-* Creates a new Velocities object with zero velocities.
+* Creates a new PoseChange object with zero poseChange.
 */
-Velocities(void)
+PoseChange(void)
 {
     this->dxy_mm = 0;
     this->dtheta_degrees = 0;
@@ -58,10 +58,10 @@ Velocities(void)
 
 
 /**
-* Updates this Velocities object.
+* Updates this PoseChange object.
 * @param dxy_mm new forward distance traveled in millimeters
 * @param dtheta_degrees new angular rotation in degrees
-* @param dtSeconds time in seconds since last velocities
+* @param dtSeconds time in seconds since last poseChange
 */
 void update(double dxy_mm, double dtheta_degrees, double dtSeconds)
 {
@@ -72,14 +72,14 @@ void update(double dxy_mm, double dtheta_degrees, double dtSeconds)
     this->dtheta_degrees = dtheta_degrees * velocity_factor;
 }
 
-friend ostream& operator<< (ostream & out, Velocities & velocities)
+friend ostream& operator<< (ostream & out, PoseChange & poseChange)
 {
     char str[100];
     
     sprintf(str, "<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
-        velocities.dxy_mm, 
+        poseChange.dxy_mm, 
-        velocities.dtheta_degrees,
+        poseChange.dtheta_degrees,
-        velocities.dt_seconds);
+        poseChange.dt_seconds);
     
     out << str;
     

cpp/Scan.cpp

@@ -31,7 +31,7 @@ using namespace std;
 
 #include "coreslam.h"
 
-#include "Velocities.hpp"
+#include "PoseChange.hpp"
 #include "Laser.hpp"
 #include "Scan.hpp"
 
@@ -76,22 +76,22 @@ void
 Scan::update(
     int * scanvals_mm, 
     double hole_width_millimeters,
-    Velocities & velocities)
+    PoseChange & poseChange)
 {
     scan_update(
         this->scan,
         scanvals_mm,
         hole_width_millimeters,
-        velocities.dxy_mm,
+        poseChange.dxy_mm,
-        velocities.dtheta_degrees);
+        poseChange.dtheta_degrees);
 }
 void 
 Scan::update(
     int * scanvals_mm, 
     double hole_width_millimeters)
 {
-    Velocities zeroVelocities;
+    PoseChange zeroPoseChange;
-    this->update(scanvals_mm, hole_width_millimeters, zeroVelocities);
+    this->update(scanvals_mm, hole_width_millimeters, zeroPoseChange);
 }
 
 ostream& operator<< (ostream & out, Scan & scan)

cpp/Scan.hpp

@@ -27,7 +27,7 @@
 #include <iostream>
 using namespace std; 
 
-class Velocities;
+class PoseChange;
 class Laser;
 
 
@@ -81,14 +81,14 @@ update(
 * Updates this Scan object with new values from a Lidar scan.
 * @param scanvals_mm scanned Lidar distance values in millimeters
 * @param hole_width_millimeters hole width in millimeters
-* @param velocities forward velocity and angular velocity of robot at scan time
+* @param poseChange forward velocity and angular velocity of robot at scan time
 * 
 */
 void 
 update(
     int * scanvals_mm, 
     double hole_width_millimeters,
-    Velocities & velocities);
+    PoseChange & poseChange);
 
 friend ostream& operator<< (ostream & out, Scan & scan);
 

cpp/WheeledRobot.cpp

@@ -28,15 +28,15 @@
 using namespace std; 
 
 #include "WheeledRobot.hpp"
-#include "Velocities.hpp"
+#include "PoseChange.hpp"
 
-Velocities 
+PoseChange 
-WheeledRobot::computeVelocities(
+WheeledRobot::computePoseChange(
     double timestamp, 
     double left_wheel_odometry, 
     double right_wheel_odometry)
 {                
-    Velocities velocities;
+    PoseChange poseChange;
     
     double timestamp_seconds_curr, left_wheel_degrees_curr, right_wheel_degrees_curr;
     
@@ -48,12 +48,12 @@ WheeledRobot::computeVelocities(
         double left_diff_degrees = left_wheel_degrees_curr - this->left_wheel_degrees_prev;
         double right_diff_degrees = right_wheel_degrees_curr - this->right_wheel_degrees_prev;
         
-        velocities.dxy_mm =  this->wheel_radius_mm * (radians(left_diff_degrees) + radians(right_diff_degrees));
+        poseChange.dxy_mm =  this->wheel_radius_mm * (radians(left_diff_degrees) + radians(right_diff_degrees));
         
-        velocities.dtheta_degrees = this->wheel_radius_mm / this->half_axle_length_mm *
+        poseChange.dtheta_degrees = this->wheel_radius_mm / this->half_axle_length_mm *
         (right_diff_degrees - left_diff_degrees);
         
-        velocities.dt_seconds = timestamp_seconds_curr - timestamp_seconds_prev;
+        poseChange.dt_seconds = timestamp_seconds_curr - timestamp_seconds_prev;
     }
     
     // Store current odometry for next time
@@ -61,5 +61,5 @@ WheeledRobot::computeVelocities(
     this->left_wheel_degrees_prev = left_wheel_degrees_curr;
     this->right_wheel_degrees_prev = right_wheel_degrees_curr;      
     
-    return velocities;
+    return poseChange;
 }

cpp/WheeledRobot.hpp

@@ -27,11 +27,11 @@
 #include <vector>
 using namespace std;
 
-class Velocities;
+class PoseChange;
 
 /**
 * An abstract class for wheeled robots.  Supports computation of forward and angular 
-* velocities based on odometry. Your subclass should should implement the
+* poseChange based on odometry. Your subclass should should implement the
 * extractOdometry method.
 */
 class WheeledRobot 
@@ -57,15 +57,15 @@ WheeledRobot(double wheel_radius_mm, double half_axle_length_mm)
 }
 
 /**
-* Computes forward and angular velocities based on odometry.
+* Computes forward and angular poseChange based on odometry.
 * @param timestamp time stamp, in whatever units your robot uses       
 * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses       
 * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
-* @return velocities object representing velocities for these odometry values
+* @return poseChange object representing poseChange for these odometry values
 */
 
-Velocities
+PoseChange
-computeVelocities(
+computePoseChange(
     double timestamp, 
     double left_wheel_odometry, 
     double right_wheel_odometry);

cpp/algorithms.cpp

@@ -24,7 +24,7 @@ along with this code.  If not, see <http://www.gnu.org/licenses/>.
 #include "Map.hpp"
 #include "Laser.hpp"
 #include "Scan.hpp"
-#include "Velocities.hpp"
+#include "PoseChange.hpp"
 #include "WheeledRobot.hpp"
 
 #include "algorithms.hpp"
@@ -64,8 +64,8 @@ CoreSLAM::CoreSLAM(
     // Store laser for later
     this->laser = new Laser(laser);
     
-    // Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
+    // Initialize poseChange (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
-    this->velocities = new Velocities();
+    this->poseChange = new PoseChange();
 
     // Initialize a scan for computing distance to map, and one for updating map
     this->scan_for_mapbuild = this->scan_create(3);
@@ -80,30 +80,30 @@ CoreSLAM::~CoreSLAM(void)
     delete this->map;
     delete this->scan_for_distance;
     delete this->scan_for_mapbuild;
-    delete this->velocities;
+    delete this->poseChange;
 }
 
 
-void CoreSLAM::update(int * scan_mm, Velocities & velocities)
+void CoreSLAM::update(int * scan_mm, PoseChange & poseChange)
 {             
     // Build a scan for computing distance to map, and one for updating map
     this->scan_update(this->scan_for_mapbuild, scan_mm);
     this->scan_update(this->scan_for_distance, scan_mm);
     
-    // Update velocities
+    // Update poseChange
-    this->velocities->update(velocities.dxy_mm, 
+    this->poseChange->update(poseChange.dxy_mm, 
-                             velocities.dtheta_degrees,  
+                             poseChange.dtheta_degrees,  
-                             velocities.dt_seconds);
+                             poseChange.dt_seconds);
                              
     // Implementing class updates map and pointcloud
-    this->updateMapAndPointcloud(velocities);
+    this->updateMapAndPointcloud(poseChange);
 }   
 
 void CoreSLAM::update(int * scan_mm) 
 {
-    Velocities zero_velocities;   
+    PoseChange zero_poseChange;   
     
-    this->update(scan_mm, zero_velocities);
+    this->update(scan_mm, zero_poseChange);
 }
 
 
@@ -121,7 +121,7 @@ Scan * CoreSLAM::scan_create(int span)
 void 
 CoreSLAM::scan_update(Scan * scan, int * scan_mm)
 {
-    scan->update(scan_mm, this->hole_width_mm, *this->velocities);
+    scan->update(scan_mm, this->hole_width_mm, *this->poseChange);
 }
 
 SinglePositionSLAM::SinglePositionSLAM(Laser & laser, int map_size_pixels, double map_size_meters) :
@@ -133,15 +133,15 @@ CoreSLAM(laser, map_size_pixels, map_size_meters)
     
 // SinglePositionSLAM class -------------------------------------------------------------------------------------------
 
-void SinglePositionSLAM::updateMapAndPointcloud(Velocities & velocities)
+void SinglePositionSLAM::updateMapAndPointcloud(PoseChange & poseChange)
 {    
     // Start at current position 
     Position start_pos = this->position;
 
-    // Add effect of velocities
+    // Add effect of poseChange
-    start_pos.x_mm      += velocities.dxy_mm * this->costheta(),
+    start_pos.x_mm      += poseChange.dxy_mm * this->costheta(),
-    start_pos.y_mm      += velocities.dxy_mm *  this->sintheta(),
+    start_pos.y_mm      += poseChange.dxy_mm *  this->sintheta(),
-    start_pos.theta_degrees += velocities.dtheta_degrees;
+    start_pos.theta_degrees += poseChange.dtheta_degrees;
     
     // Add offset from laser
     start_pos.x_mm += this->laser->offset_mm * this->costheta();

cpp/algorithms.hpp

@@ -47,7 +47,7 @@ class Laser;
 *     </pre>
 *    Implementing classes should provide the method
 *
-*      void updateMapAndPointcloud(int * scan_mm, Velocities & velocities)
+*      void updateMapAndPointcloud(int * scan_mm, PoseChange & poseChange)
 *
 *    to update the map and point-cloud (particle cloud).
 *
@@ -74,17 +74,17 @@ public:
     
    /**
     * Updates the scan and odometry, and calls the the implementing class's updateMapAndPointcloud method with
-    * the specified velocities.
+    * the specified poseChange.
     * 
     * @param scan_mm Lidar scan values, whose count is specified in the <tt>scan_size</tt> 
     * attribute of the Laser object passed to the CoreSlam constructor
-    * @param velocities velocities for odometry
+    * @param poseChange poseChange for odometry
     */
-    void update(int * scan_mm, Velocities & velocities);
+    void update(int * scan_mm, PoseChange & poseChange);
 
 
     /**
-    * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities
+    * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero poseChange
     * (no odometry).
     * @param scan_mm Lidar scan values, whose count is specified in the <tt>scan_size</tt> 
     * attribute of the Laser object passed to the CoreSlam constructor
@@ -139,15 +139,15 @@ protected:
     Scan * scan_for_distance;    
     
     /**
-    * The current velocities from odometry
+    * The current poseChange from odometry
     */
-    class Velocities * velocities;
+    class PoseChange * poseChange;
         
     /**
     * Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update()
-    * @param velocities velocities for odometry
+    * @param poseChange poseChange for odometry
     */
-    virtual void updateMapAndPointcloud(Velocities & velocities) = 0;
+    virtual void updateMapAndPointcloud(PoseChange & poseChange) = 0;
 
 private:
             
@@ -191,9 +191,9 @@ protected:
     
     /**
     * Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update()
-    * @param velocities velocities for odometry
+    * @param poseChange poseChange for odometry
     */
-    void updateMapAndPointcloud(Velocities & velocities);
+    void updateMapAndPointcloud(PoseChange & poseChange);
     
     /**
     * Returns a new position based on searching from a starting position. Called automatically by

examples/Log2PGM.java

@@ -35,7 +35,7 @@ import edu.wlu.cs.levy.breezyslam.components.Map;
 import edu.wlu.cs.levy.breezyslam.components.Scan;
 import edu.wlu.cs.levy.breezyslam.components.Position;
 import edu.wlu.cs.levy.breezyslam.components.URG04LX;
-import edu.wlu.cs.levy.breezyslam.components.Velocities;
+import edu.wlu.cs.levy.breezyslam.components.PoseChange;
 
 import edu.wlu.cs.levy.breezyslam.robots.WheeledRobot;
 
@@ -226,8 +226,8 @@ public class Log2PGM
             if (this.use_odometry)
             {
                 long [] odometry = odometries.elementAt(scanno);
-                Velocities velocities = this.robot.computeVelocities(odometry[0], odometry[1], odometry[2]);
+                PoseChange poseChange = this.robot.computePoseChange(odometry[0], odometry[1], odometry[2]);
-                slam.update(scan, velocities);            
+                slam.update(scan, poseChange);            
             }
             else
             {

examples/MinesRover.m

@@ -45,9 +45,9 @@ classdef MinesRover < WheeledRobot
             
         end
         
-        function [velocities, obj] = computeVelocities(obj, odometry)
+        function [poseChange, obj] = computePoseChange(obj, odometry)
             
-            [velocities, obj] = computeVelocities@WheeledRobot(obj, odometry(1), odometry(2), odometry(3));
+            [poseChange, obj] = computePoseChange@WheeledRobot(obj, odometry(1), odometry(2), odometry(3));
             
         end
         

examples/log2pgm.cpp

@@ -58,7 +58,7 @@ using namespace std;
 #include "Position.hpp"
 #include "Laser.hpp"
 #include "WheeledRobot.hpp"
-#include "Velocities.hpp"
+#include "PoseChange.hpp"
 #include "algorithms.hpp"
 
 
@@ -161,9 +161,9 @@ public:
     {
     }
     
-    Velocities computeVelocities(long * odometry, Velocities & velocities)
+    PoseChange computePoseChange(long * odometry, PoseChange & poseChange)
     {  
-        return WheeledRobot::computeVelocities(
+        return WheeledRobot::computePoseChange(
             odometry[0], 
             odometry[1], 
             odometry[2]);
@@ -350,8 +350,8 @@ int main( int argc, const char** argv )
         // Update with/out odometry
         if (use_odometry)
         {
-            Velocities velocities = robot.computeVelocities(odometries[scanno], velocities);
+            PoseChange poseChange = robot.computePoseChange(odometries[scanno], poseChange);
-            slam->update(lidar, velocities);            
+            slam->update(lidar, poseChange);            
         }
         else
         {

examples/log2pgm.py

@@ -94,8 +94,8 @@ def main():
     
         if use_odometry:
                   
-            # Convert odometry to velocities
+            # Convert odometry to pose change (dxy_mm, dtheta_degrees, dt_seconds)
-            velocities = robot.computeVelocities(odometries[scanno])
+            velocities = robot.computePoseChange(odometries[scanno])
                                  
             # Update SLAM with lidar and velocities
             slam.update(lidars[scanno], velocities)

examples/log2pkl.py

@@ -77,7 +77,7 @@ def main():
         if use_odometry:
                   
             # Convert odometry to velocities
-            velocities = robot.computeVelocities(odometries[scanno])
+            velocities = robot.computePoseChange(odometries[scanno])
                                  
             # Update SLAM with lidar and velocities
             slam.update(lidars[scanno], velocities)

examples/log2png.py

@@ -96,7 +96,7 @@ def main():
         if use_odometry:
                   
             # Convert odometry to velocities
-            velocities = robot.computeVelocities(odometries[scanno])
+            velocities = robot.computePoseChange(odometries[scanno])
                                  
             # Update SLAM with lidar and velocities
             slam.update(lidars[scanno], velocities)

examples/logdemo.m

@@ -82,7 +82,7 @@ for scanno = 1:size(scans, 1)
     if use_odometry
         
         % Convert odometry to velocities
-        [velocities,robot] = robot.computeVelocities(odometries(scanno, :));
+        [velocities,robot] = robot.computePoseChange(odometries(scanno, :));
         
         % Update SLAM with lidar and velocities
         slam = slam.update(scans(scanno,:), velocities);

examples/logmovie.py

@@ -67,7 +67,7 @@ def threadfunc(robot, slam, timestamps, lidars, odometries, mapbytes, pose):
         else:
         
             # Convert odometry to velocities
-            velocities = robot.computeVelocities(odometries[scanno])
+            velocities = robot.computePoseChange(odometries[scanno])
 
             # Update SLAM with lidar and velocities
             slam.update(lidars[scanno], velocities)

examples/mines.py

@@ -99,9 +99,9 @@ class Rover(WheeledVehicle):
         
         return '<%s ticks_per_cycle=%d>' % (WheeledVehicle.__str__(self), self.ticks_per_cycle)
         
-    def computeVelocities(self, odometry):
+    def computePoseChange(self, odometry):
         
-        return WheeledVehicle.computeVelocities(self, odometry[0], odometry[1], odometry[2])
+        return WheeledVehicle.computePoseChange(self, odometry[0], odometry[1], odometry[2])
 
     def extractOdometry(self, timestamp, leftWheel, rightWheel):
                 

java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java

@@ -23,12 +23,12 @@
 
 package edu.wlu.cs.levy.breezyslam.robots;
 
-import edu.wlu.cs.levy.breezyslam.components.Velocities;
+import edu.wlu.cs.levy.breezyslam.components.PoseChange;
 
 
 /**
  * An abstract class for wheeled robots.  Supports computation of forward and angular 
- * velocities based on odometry. Your subclass should should implement the
+ * poseChange based on odometry. Your subclass should should implement the
  * extractOdometry method.
  */
 public abstract class WheeledRobot 
@@ -59,14 +59,14 @@ public abstract class WheeledRobot
     }
 
     /**
-     * Computes forward and angular velocities based on odometry.
+     * Computes forward and angular poseChange based on odometry.
      * @param timestamp time stamp, in whatever units your robot uses       
      * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses       
      * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
-     * @return velocities object representing velocities for these odometry values
+     * @return poseChange object representing poseChange for these odometry values
      */
 
-    public Velocities computeVelocities( double timestamp, double left_wheel_odometry, double right_wheel_odometry)
+    public PoseChange computePoseChange( double timestamp, double left_wheel_odometry, double right_wheel_odometry)
     {
         WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry);
 
@@ -91,7 +91,7 @@ public abstract class WheeledRobot
         this.left_wheel_degrees_prev = odometry.left_wheel_degrees;
         this.right_wheel_degrees_prev = odometry.right_wheel_degrees;      
 
-        return new Velocities(dxy_mm, dtheta_degrees, dt_seconds);
+        return new PoseChange(dxy_mm, dtheta_degrees, dt_seconds);
     }
 
     /**

matlab/WheeledRobot.m

@@ -62,8 +62,8 @@ classdef WheeledRobot
     
     methods (Access = 'public')
         
-        function [velocities, obj] = computeVelocities(obj, timestamp, leftWheelOdometry, rightWheelOdometry)
+        function [poseChange, obj] = computePoseChange(obj, timestamp, leftWheelOdometry, rightWheelOdometry)
-            % Computes forward and angular velocities based on odometry.
+            % Computes forward and angular poseChange based on odometry.
             %
             % Parameters:
             
@@ -104,7 +104,7 @@ classdef WheeledRobot
             obj.rightWheelDegreesPrev = rightWheelDegreesCurr;
             
             % Return linear velocity, angular velocity, time difference
-            velocities = [dxyMillimeters, dthetaDegrees, dtSeconds];
+            poseChange = [dxyMillimeters, dthetaDegrees, dtSeconds];
             
         end
     end    

python/breezyslam/algorithms.py

@@ -57,7 +57,7 @@ class CoreSLAM(object):
     
     Implementing classes should provide the method
     
-      _updateMapAndPointcloud(scan_mm, velocities, should_update_map)
+      _updateMapAndPointcloud(scan_mm, dxy_mm, dtheta_degrees, should_update_map)
     
     to update the point-cloud (particle cloud) and map (if should_update_map true)
     '''
@@ -80,7 +80,7 @@ class CoreSLAM(object):
         # Store laser for later
         self.laser = laser
         
-        # Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
+        # Initialize velocities (dxyMillimeters/dt, dthetaDegrees/dt) for scan update
         self.velocities = (0, 0)
         
         # Initialize a scan for computing distance to map, and one for updating map
@@ -97,22 +97,22 @@ class CoreSLAM(object):
          
         scan_mm is a list of Lidar scan values, whose count is specified in the scan_size 
         attribute of the Laser object passed to the CoreSlam constructor
-        velocities is a tuple of velocities (dxy_mm, dtheta_degrees, dt_seconds) for odometry
+        velocities is a tuple of velocities (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry
         should_update_map flags for whether you want to update the map
         '''
 
+        # Convert (dxy,dtheta,dt) to (dxy/dt, dtheta/dt) for scan update
+        velocity_factor = (1 / velocities[2])  if (velocities[2] > 0) else 0 # units => units/sec
+        dxy_mm_dt = velocities[0] * velocity_factor  
+        dtheta_degrees_dt = velocities[1] * velocity_factor
+        self.velocities = (dxy_mm_dt, dtheta_degrees_dt)
+
         # Build a scan for computing distance to map, and one for updating map 
         self._scan_update(self.scan_for_mapbuild, scans_mm)
         self._scan_update(self.scan_for_distance, scans_mm)
 
-        # Convert (dxy,dtheta,dt) to (dxy/dt, dtheta/dt) for scan update
-        velocity_factor = (1 / velocities[2])  if (velocities[2] > 0) else 0 # units => units/sec
-        new_dxy_mm = velocities[0] * velocity_factor  
-        new_dtheta_degrees = velocities[1] * velocity_factor
-        self.velocities = (new_dxy_mm, new_dtheta_degrees)
-
         # Implementing class updates map and pointcloud
-        self._updateMapAndPointcloud(self.velocities, should_update_map)
+        self._updateMapAndPointcloud(velocities[0], velocities[1], should_update_map)
         
     def getmap(self, mapbytes):
         '''
@@ -166,7 +166,7 @@ class SinglePositionSLAM(CoreSLAM):
         init_coord_mm = 500 * map_size_meters # center of map
         self.position =  pybreezyslam.Position(init_coord_mm, init_coord_mm, 0)
         
-    def _updateMapAndPointcloud(self, velocities, should_update_map):
+    def _updateMapAndPointcloud(self, dxy_mm, dtheta_degrees, should_update_map):
         '''
         Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM.update()
         velocities is a tuple of the form (dxy_mm, dtheta_degrees, dt_seconds).
@@ -176,9 +176,9 @@ class SinglePositionSLAM(CoreSLAM):
         start_pos = self.position.copy()
         
         # Add effect of velocities
-        start_pos.x_mm      += velocities[0] * self._costheta()
+        start_pos.x_mm      += dxy_mm * self._costheta()
-        start_pos.y_mm      += velocities[0] * self._sintheta()
+        start_pos.y_mm      += dxy_mm * self._sintheta()
-        start_pos.theta_degrees += velocities[1]
+        start_pos.theta_degrees += dtheta_degrees
 
         # Add offset from laser
         start_pos.x_mm  += self.laser.offset_mm * self._costheta()

python/breezyslam/vehicles.py

@@ -52,9 +52,9 @@ class WheeledVehicle(object):
         
         return self.__str__()
         
-    def computeVelocities(self, timestamp, leftWheelOdometry, rightWheelOdometry):
+    def computePoseChange(self, timestamp, leftWheelOdometry, rightWheelOdometry):
         '''
-        Computes forward and angular velocities based on odometry.
+        Computes pose change based on odometry.
         
         Parameters:
         

python/pybreezyslam.c

@@ -15,29 +15,6 @@ GNU General Public License for more details.
 
 You should have received a copy of the GNU Lesser General Public License 
 along with this code.  If not, see <http://www.gnu.org/licenses/>.
-
-Change log:
-
-07-FEB-2014 : Simon D. Levy  - Initial release
-
-17-FEB-2014 : SDL - Prohibit non-positive random seed
-28-FEB-2014 : SDL - Check for null return in Robot.computeVelocities()
-01-MAR-2014 : SDL - Moved module code to in __init__.py
-14-MAR_2014 : SDL - Changed mm_per_pixel to pixels_per_meter
-                  - No more robot object passed to __init__(); velocities 
-                    sent directly into update()
-16-MAR_2014 : SDL - Changed #include to ../pybreezyslam
-23-MAR-2014 : SDL - Changed millimeters to meters
-31-MAR-2014 : SDL - Improved documetnation for Laser class
-                  - Made all units explicit
-30-APR-2014 : SDL - Migrated CoreSLAM algorithm to pure Python
-                  - Added Position, Map, Scan classes
-                  - Added distanceScanToMap method
-04-MAY-2014 : SDL - Changed back from meters to mm
-03-JUN-2014 : SDL - Made distanceScanToMap() return -1 for infinity
-23-JUL-2014 : SDL - Simplified API for Laser
-08-AUG-2014:  SDL - Moved Laser to pure Python
-13-AUG-2014:  SDL - Fixed Scan.update() bug in Python3
 */
 
 #include <Python.h>