练习：Omega and Xi
# -----------
# User Instructions
#
# Write a function, doit, that takes as its input an
# initial robot position, move1, and move2. This
# function should compute the Omega and Xi matrices
# discussed in lecture and should RETURN the mu vector
# (which is the product of Omega.inverse() and Xi).
#
# Please enter your code at the bottom.
from math import *
import random
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#
#
#===============================================================
# ------------------------------------------------
#
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
#
class matrix:
# implements basic operations of a matrix class
# ------------
#
# initialization - can be called with an initial matrix
#
def __init__(self, value = [[]]):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
# ------------
#
# makes matrix of a certain size and sets each element to zero
#
def zero(self, dimx, dimy = 0):
if dimy == 0:
dimy = dimx
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
# ------------
#
# makes matrix of a certain (square) size and turns matrix into identity matrix
#
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0.0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1.0
# ------------
#
# prints out values of matrix
#
def show(self, txt = ''):
for i in range(len(self.value)):
print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']'
print ' '
# ------------
#
# defines elmement-wise matrix addition. Both matrices must be of equal dimensions
#
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimension to add"
else:
# add if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
# ------------
#
# defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
#
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimension to subtract"
else:
# subtract if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
# ------------
#
# defines multiplication. Both matrices must be of fitting dimensions
#
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# multiply if correct dimensions
res = matrix()
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
# ------------
#
# returns a matrix transpose
#
def transpose(self):
# compute transpose
res = matrix()
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[ 1, 2, 3, 4, 5],
# [ 6, 7, 8, 9, 10],
# [11, 12, 13, 14, 15]])
#
# l.take([0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 3, 4],
# [11, 13, 14]]
#
#
# take is used to remove rows and columns from existing matrices
# list1/list2 define a sequence of rows/columns that shall be taken
# is no list2 is provided, then list2 is set to list1 (good for symmetric matrices)
#
def take(self, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in take()"
res = matrix()
res.zero(len(list1), len(list2))
for i in range(len(list1)):
for j in range(len(list2)):
res.value[i][j] = self.value[list1[i]][list2[j]]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[1, 2, 3],
# [4, 5, 6]])
#
# l.expand(3, 5, [0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 0, 2, 3, 0],
# [0, 0, 0, 0, 0],
# [4, 0, 5, 6, 0]]
#
# expand is used to introduce new rows and columns into an existing matrix
# list1/list2 are the new indexes of row/columns in which the matrix
# elements are being mapped. Elements for rows and columns
# that are not listed in list1/list2
# will be initialized by 0.0.
#
def expand(self, dimx, dimy, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in expand()"
res = matrix()
res.zero(dimx, dimy)
for i in range(len(list1)):
for j in range(len(list2)):
res.value[list1[i]][list2[j]] = self.value[i][j]
return res
# ------------
#
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def Cholesky(self, ztol= 1.0e-5):
res = matrix()
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
if abs(S) < ztol:
S = 0.0
try:
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
except:
raise ValueError, "Zero diagonal"
return res
# ------------
#
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def CholeskyInverse(self):
res = matrix()
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/ tjj**2 - S/ tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = \
-sum([self.value[i][k]*res.value[k][j] for k in \
range(i+1,self.dimx)])/self.value[i][i]
return res
# ------------
#
# comutes and returns the inverse of a square matrix
#
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
# ------------
#
# prints matrix (needs work!)
#
def __repr__(self):
return repr(self.value)
# ######################################################################
# ######################################################################
# ######################################################################
"""
For the following example, you would call doit(-3, 5, 3):
3 robot positions
initially: -3
moves by 5
moves by 3
which should return a mu of:
[[-3.0],
[2.0],
[5.0]]
"""
def doit(initial_pos, move1, move2):
#
#
# Add your code here.
#
#
Omega = matrix([[1,0,0],[0,0,0],[0,0,0]])
Xi = matrix([[initial_pos],[0],[0]])
Omega += matrix([[1,-1,0],[-1,1,0],[0,0,0]])
Xi += matrix([[-move1],[move1],[0]])
Omega += matrix([[0,0,0],[0,1,-1],[0,-1,1]])
Xi += matrix([[0],[-move2],[move2]])
Omega.show('Omega: ')
Xi.show('xi: ')
mu = Omega.inverse()*Xi
mu.show('result: ')
return mu
doit(-3, 5, 3)
练习20：Expand
# -----------
# User Instructions
#
# Modify your doit function to incorporate 3
# distance measurements to a landmark(Z0, Z1, Z2).
# You should use the provided expand function to
# allow your Omega and Xi matrices to accomodate
# the new information.
#
# Each landmark measurement should modify 4
# values in your Omega matrix and 2 in your
# Xi vector.
#
# Add your code at line 356.
from math import *
import random
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#
#
#===============================================================
# ------------------------------------------------
#
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
#
class matrix:
# implements basic operations of a matrix class
# ------------
#
# initialization - can be called with an initial matrix
#
def __init__(self, value = [[]]):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
# ------------
#
# makes matrix of a certain size and sets each element to zero
#
def zero(self, dimx, dimy = 0):
if dimy == 0:
dimy = dimx
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
# ------------
#
# makes matrix of a certain (square) size and turns matrix into identity matrix
#
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0.0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1.0
# ------------
#
# prints out values of matrix
#
def show(self, txt = ''):
for i in range(len(self.value)):
print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']'
print ' '
# ------------
#
# defines elmement-wise matrix addition. Both matrices must be of equal dimensions
#
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimension to add"
else:
# add if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
# ------------
#
# defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
#
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimension to subtract"
else:
# subtract if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
# ------------
#
# defines multiplication. Both matrices must be of fitting dimensions
#
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# multiply if correct dimensions
res = matrix()
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
# ------------
#
# returns a matrix transpose
#
def transpose(self):
# compute transpose
res = matrix()
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[ 1, 2, 3, 4, 5],
# [ 6, 7, 8, 9, 10],
# [11, 12, 13, 14, 15]])
#
# l.take([0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 3, 4],
# [11, 13, 14]]
#
#
# take is used to remove rows and columns from existing matrices
# list1/list2 define a sequence of rows/columns that shall be taken
# is no list2 is provided, then list2 is set to list1 (good for symmetric matrices)
#
def take(self, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in take()"
res = matrix()
res.zero(len(list1), len(list2))
for i in range(len(list1)):
for j in range(len(list2)):
res.value[i][j] = self.value[list1[i]][list2[j]]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[1, 2, 3],
# [4, 5, 6]])
#
# l.expand(3, 5, [0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 0, 2, 3, 0],
# [0, 0, 0, 0, 0],
# [4, 0, 5, 6, 0]]
#
# expand is used to introduce new rows and columns into an existing matrix
# list1/list2 are the new indexes of row/columns in which the matrix
# elements are being mapped. Elements for rows and columns
# that are not listed in list1/list2
# will be initialized by 0.0.
#
def expand(self, dimx, dimy, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in expand()"
res = matrix()
res.zero(dimx, dimy)
for i in range(len(list1)):
for j in range(len(list2)):
res.value[list1[i]][list2[j]] = self.value[i][j]
return res
# ------------
#
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def Cholesky(self, ztol= 1.0e-5):
res = matrix()
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
if abs(S) < ztol:
S = 0.0
try:
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
except:
raise ValueError, "Zero diagonal"
return res
# ------------
#
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def CholeskyInverse(self):
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
res = matrix()
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/ tjj**2 - S/ tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = \
-sum([self.value[i][k]*res.value[k][j] for k in \
range(i+1,self.dimx)])/self.value[i][i]
return res
# ------------
#
# comutes and returns the inverse of a square matrix
#
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
# ------------
#
# prints matrix (needs work!)
#
def __repr__(self):
return repr(self.value)
# ######################################################################
# ######################################################################
# ######################################################################
"""
For the following example, you would call doit(-3, 5, 3, 10, 5, 2):
3 robot positions
initially: -3 (measure landmark to be 10 away)
moves by 5 (measure landmark to be 5 away)
moves by 3 (measure landmark to be 2 away)
which should return a mu of:
[[-3.0],
[2.0],
[5.0],
[7.0]]
"""
def doit(initial_pos, move1, move2, Z0, Z1, Z2):
Omega = matrix([[1.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]])
Xi = matrix([[initial_pos],
[0.0],
[0.0]])
Omega += matrix([[1.0, -1.0, 0.0],
[-1.0, 1.0, 0.0],
[0.0, 0.0, 0.0]])
Xi += matrix([[-move1],
[move1],
[0.0]])
Omega += matrix([[0.0, 0.0, 0.0],
[0.0, 1.0, -1.0],
[0.0, -1.0, 1.0]])
Xi += matrix([[0.0],
[-move2],
[move2]])
#
#
# Add your code here.
#
#
Omega = Omega.expand(4,4,[0,1,2],[0,1,2])
Xi = Xi.expand(4,1,[0,1,2],[0])
Omega += matrix([[1.0, 0.0, 0.0,-1.0],
[0.0, 0.0, 0.0,0.0],
[0.0, 0.0, 0.0,0.0],
[-1.0, 0.0, 0.0,1.0]])
Xi += matrix([[-Z0],
[0.0],
[0.0],
[Z0]])
Omega += matrix([[0.0, 0.0, 0.0,0.0],
[0.0, 1.0, 0.0,-1.0],
[0.0, 0.0, 0.0,0.0],
[0.0, -1.0, 0.0,1.0]])
Xi += matrix([[0.0],
[-Z1],
[0.0],
[Z1]])
Omega += matrix([[0.0, 0.0, 0.0,0.0],
[0.0, 0.0, 0.0,0.0],
[0.0, 0.0, 1.0,-1.0],
[0.0, 0.0, -1.0,1.0]])
Xi += matrix([[0.0],
[0.0],
[-Z2],
[Z2]])
Omega.show('Omega: ')
Xi.show('Xi: ')
mu = Omega.inverse() * Xi
mu.show('Mu: ')
return mu
doit(-3, 5, 3, 10, 5, 2)