class Transform(object):
def __init__(self, *args):
if len(args) == 0:
self.translation = np.array([0.0, 0.0, 0.0])
self.quaternion = Quaternion()
if len(args) == 7:
self.translation = np.array(args[0:3])
self.quaternion = Quaternion(args[3:7])
if len(args) == 2:
self.quaternion = Quaternion(args[0])
self.translation = np.array(args[1])
if len(args) == 1:
if isinstance(args[0], Transform):
self.quaternion = args[0].quaternion
self.translation = args[0].translation
elif isinstance(args[0], (list, tuple)):
self.translation = np.array(args[0][0:3])
self.quaternion = Quaternion(args[0][3:7])
self.quaternion.normalize()
def __mul__(self, other):
rot = self.quaternion * other.quaternion
trans = self.quaternion.transform(other.translation) + self.translation
return Transform(rot, trans)
def inverse(self):
rot = self.quaternion.conjugate()
trans = -self.quaternion.conjugate().transform(self.translation)
return Transform(rot, trans)
def transform(self, v):
res = self.quaternion.transform(v) + self.translation
return res
def __str__(self):
return "quaternion: %s,\ntranslation: %s" % (self.quaternion,
self.translation)
def __getitem__(self, i):
if i < 3:
return self.translation[i]
if i < 7:
return self.quaternion.toTuple()[i - 3]
raise IndexError("index out of range")
def __len__(self):
return 7
def toHomogeneousMatrix(self):
H = np.eye(4)
H[:3, :3] = self.quaternion.toRotationMatrix()
H[:3, 3] = self.translation
return H
def toTuple(self):
return tuple(self.translation) + self.quaternion.toTuple()
class Transform (object):
def __init__ (self, *args):
if len (args) == 0:
self.translation = np.array ([0.,0.,0.])
self.quaternion = Quaternion ()
if len (args) == 7:
self.translation = np.array (args [0:3])
self.quaternion = Quaternion (args [3:7])
if len (args) == 2:
self.quaternion = Quaternion (args [0])
self.translation = np.array (args [1])
if len (args) == 1:
if isinstance (args [0], Transform) :
self.quaternion = args [0].quaternion
self.translation = args [0].translation
elif isinstance (args [0], list):
self.translation = np.array (args [0][0:3])
self.quaternion = Quaternion (args [0][3:7])
self.quaternion.normalize ()
def __mul__ (self, other):
rot = self.quaternion * other.quaternion
trans = self.quaternion.transform (other.translation) +\
self.translation
return Transform (rot, trans)
def inverse (self):
rot = self.quaternion.conjugate()
trans = - self.quaternion.conjugate().transform (self.translation)
return Transform (rot, trans)
def transform (self, v):
res = self.quaternion.transform (v) + self.translation
return res
def __str__ (self):
return "quaternion: %s,\ntranslation: %s"%(self.quaternion,
self.translation)
def __getitem__ (self, i):
if i<3: return self.translation [i]
if i<7: return self.quaternion.toTuple () [i-3]
raise IndexError ("index out of range")
def __len__ (self):
return 7
def toHomogeneousMatrix (self):
H = np.eye(4)
H[:3,:3] = self.quaternion.toRotationMatrix()
H[:3,3] = self.translation
return H
def toTuple (self):
return tuple (self.translation) + self.quaternion.toTuple ()
chessboard_Z = random.uniform(dist_from_camera[0],
dist_from_camera[1])
chessboard_X = ((x - projection_matrix[0, 2]) /
projection_matrix[0, 0] * chessboard_Z)
chessboard_Y = ((y - projection_matrix[1, 2]) /
projection_matrix[1, 1] * chessboard_Z)
chessboard_position = np.matrix(
[chessboard_X, chessboard_Y, chessboard_Z])
q = Quaternion().fromRPY(
random.uniform(-pi / 12.0, pi / 12.0),
random.uniform(-pi / 12.0, pi / 12.0),
random.uniform(-pi / 12.0, pi / 12.0),
)
shoot_pose += 1
R = q.toRotationMatrix()
if (R * chessboard_normal)[2] >= 0.0:
continue
Rt = np.hstack(
(R, (chessboard_position - camera_position).transpose()))
if not all([
isInImage(projectPoint(Rt,
np.matrix(pt).transpose()))
for pt in chessboard_pts
]):
continue
q = Quaternion().fromRPY(
-pi, 0, -pi) * q # Switch tn the real camera frame
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
import numpy as np
import csv
from hpp import Quaternion
# Test construction from SO3 matrix
for _ in range(1000):
q1 = Quaternion(np.random.sample(4)).normalize()
mat = q1.toRotationMatrix()
q2 = Quaternion(mat)
if abs(q1 - q2) > 1e-10 and abs(q1 + q2) > 1e-10:
raise RuntimeError("Error in conversion quaternion matrix")
# test addition
with open('./test/add-quaternions.csv', 'r') as f:
r = csv.reader(f, delimiter=',')
iline = 0
for line in r:
iline += 1
data = list(map(float, line))
q1 = Quaternion(data[0:4])
q2 = Quaternion(data[4:8])
q3 = Quaternion(data[8:12])
if abs(q1 + q2 - q3) > 1e-10:
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
import numpy as np
import csv
from hpp import Quaternion
# Test construction from SO3 matrix
for _ in xrange (1000):
q1 = Quaternion (np.random.sample (4)).normalize ()
mat = q1.toRotationMatrix ()
q2 = Quaternion (mat)
if abs (q1-q2) > 1e-10 and abs (q1+q2) > 1e-10:
raise RuntimeError ("Error in conversion quaternion matrix")
# test addition
with open ('./test/add-quaternions.csv', 'r') as f:
r = csv.reader (f, delimiter=',')
iline=0
for line in r:
iline+=1
data = map (float, line)
q1 = Quaternion (data [0:4])
q2 = Quaternion (data [4:8])
q3 = Quaternion (data [8:12])
if abs(q1+q2-q3) > 1e-10:
