Skip to content
Snippets Groups Projects
Commit c3b15f1e authored by Davis, Duane T's avatar Davis, Duane T
Browse files

Added more utility functions to robot_math.py

postMultiply(matrix, vector)
homogeneousTransformMatrix(frame)
transformToNewFrame(point, frame)
parent c712fa3d
No related branches found
No related tags found
No related merge requests found
......@@ -6,6 +6,7 @@ import math
import random
import numpy
import copy
import functools
# Some potentially useful constants
......@@ -264,6 +265,27 @@ def projectSensorReturnToWorld(state, sensor_posit, rng, brg):
return ( x_return, y_return )
def homogeneousTransformMatrix(frame):
''' Generates a homogeneous transormation matrix to transform points
@param frame: (x, y, theta) of the source frame origin in the target frame
@return a 3x3 homogeneous transformation matrix as a tuple of tuples
'''
return ( (math.cos(frame[2]), -math.sin(frame[2]), frame[0]), \
(math.sin(frame[2]), math.cos(frame[2]), frame[1]), \
(0.0, 0.0, 1.0) )
def transformToNewFrame(point, frame):
''' Transpose a point in one frame into another frame
@param point: (x, y) coordinate of the point to be transformed
@param frame: (x, y, theta) of the current frame in the target frame
@return a tuple with the point transformed into the new frame
'''
h = homogeneousTransformMatrix(frame[0], frame[1], frame[2])
(newX, newY, _) = postMultiply(matrix, (point[0], point[1], 1.0))
return (newX, newY)
def lowVarianceSampler(hypotheses, weights):
''' Implements the low-variance sampling algorithm to generate an updated
set of hypotheses reflecting the probabilistic weights of the initial set
......@@ -321,6 +343,18 @@ def vector_projection(v1, v2):
# Matrix functions (a few simple ones, anyway)
def postMultiply(matrix, vector):
''' Postmultiplies an m x n by an n-size vector
@param matrix with M columns as a list of lists
@param vector with M elements as a list
'''
result = []
for row in matrix:
temp = map(lambda x, y: x*y, row, vector)
result.append(functools.reduce(lambda x, y: x+y, temp))
return result
def determinant(pt1, pt2):
''' Computes the determinant product of 2 2-vectors (Cartesian points)
@param pt1: Cartesian coordinates of the first point
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment