示例#1
0
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 ()
示例#3
0
            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: