def _recursive_procrustes(self):
        r"""
        Recursively calculates a procrustes alignment.
        """
        from menpo.shape import mean_pointcloud
        from menpo.transform import Similarity
        if self.n_iterations > self.max_iterations:
            return False
        new_tgt = mean_pointcloud([t.aligned_source.points
                                   for t in self.transforms])
        # rescale the new_target to be the same size as the original about
        # it's centre
        rescale = Similarity.identity(new_tgt.n_dims)

        s = UniformScale(self.initial_target_scale / new_tgt.norm(),
                         self.n_dims, skip_checks=True)
        t = Translation(-new_tgt.centre, skip_checks=True)
        rescale.compose_before_inplace(t)
        rescale.compose_before_inplace(s)
        rescale.compose_before_inplace(t.pseudoinverse)
        rescale.apply_inplace(new_tgt)
        # check to see if we have converged yet
        delta_target = np.linalg.norm(self.target.points - new_tgt.points)
        if delta_target < 1e-6:
            return True
        else:
            self.n_iterations += 1
            for t in self.transforms:
                t.set_target(new_tgt)
            self.target = new_tgt
            return self._recursive_procrustes()

def optimal_cylindrical_unwrap(points):
    r"""
    Returns a :map:`TransformChain` of
    [:map:`Translation`, :map:`CylindricalUnwrap`]
    which optimally cylindrically unwraps the points provided. This is done by:

    #. Find an optimal :map:`Translation` to centre the points in ``x-z`` plane
    #. Use :map:`circle_fit` to find the optimal radius for fitting the points
    #. Calculate a :map:`CylindricalUnwrap` using the optimal radius
    #. Return a composition of the two.

    Parameters
    ----------
    points : :map:`PointCloud`
        The 3D points that will be used to find the optimum unwrapping position

    Returns
    -------

    transform: :map:`TransformChain`
        A :map:`TransformChain` which performs the optimal translation and
        unwrapping.

    """
    # find the optimum centre to unwrap
    xy = points.points[:, [0, 2]]  # just in the x-z plane
    centre, radius = radial_fit(xy)
    # convert the 2D circle data into the 3D space
    translation = np.array([centre[0], 0, centre[1]])
    centring_transform = Translation(-translation)
    unwrap = CylindricalUnwrap(radius)
    return centring_transform.compose_before(unwrap)

def noisy_alignment_similarity_transform(source, target, noise_type='uniform',
                                         noise_percentage=0.1,
                                         allow_alignment_rotation=False):
    r"""
    Constructs and perturbs the optimal similarity transform between the source
    and target shapes by adding noise to its parameters.

    Parameters
    ----------
    source : `menpo.shape.PointCloud`
        The source pointcloud instance used in the alignment
    target : `menpo.shape.PointCloud`
        The target pointcloud instance used in the alignment
    noise_type : ``{'uniform', 'gaussian'}``, optional
        The type of noise to be added.
    noise_percentage : `float` in ``(0, 1)`` or `list` of `len` `3`, optional
        The standard percentage of noise to be added. If `float`, then the same
        amount of noise is applied to the scale, rotation and translation
        parameters of the optimal similarity transform. If `list` of
        `float` it must have length 3, where the first, second and third elements
        denote the amount of noise to be applied to the scale, rotation and
        translation parameters, respectively.
    allow_alignment_rotation : `bool`, optional
        If ``False``, then the rotation is not considered when computing the
        optimal similarity transform between source and target.

    Returns
    -------
    noisy_alignment_similarity_transform : `menpo.transform.Similarity`
        The noisy Similarity Transform between source and target.
    """
    if isinstance(noise_percentage, float):
        noise_percentage = [noise_percentage] * 3
    elif len(noise_percentage) == 1:
        noise_percentage *= 3

    similarity = AlignmentSimilarity(source, target,
                                     rotation=allow_alignment_rotation)

    if noise_type is 'gaussian':
        s = noise_percentage[0] * (0.5 / 3) * np.asscalar(np.random.randn(1))
        r = noise_percentage[1] * (180 / 3) * np.asscalar(np.random.randn(1))
        t = noise_percentage[2] * (target.range() / 3) * np.random.randn(2)

        s = scale_about_centre(target, 1 + s)
        r = rotate_ccw_about_centre(target, r)
        t = Translation(t, source.n_dims)
    elif noise_type is 'uniform':
        s = noise_percentage[0] * 0.5 * (2 * np.asscalar(np.random.randn(1)) - 1)
        r = noise_percentage[1] * 180 * (2 * np.asscalar(np.random.rand(1)) - 1)
        t = noise_percentage[2] * target.range() * (2 * np.random.rand(2) - 1)

        s = scale_about_centre(target, 1. + s)
        r = rotate_ccw_about_centre(target, r)
        t = Translation(t, source.n_dims)
    else:
        raise ValueError('Unexpected noise type. '
                         'Supported values are {gaussian, uniform}')

    return similarity.compose_after(t.compose_after(s.compose_after(r)))

    def _recursive_procrustes(self):
        r"""
        Recursively calculates a procrustes alignment.
        """
        from menpo.shape import PointCloud

        if self.n_iterations > self.max_iterations:
            return False
        av_aligned_source = sum(t.aligned_source.points for t in self.transforms) / self.n_sources
        new_target = PointCloud(av_aligned_source)
        # rescale the new_target to be the same size as the original about
        # it's centre
        rescale = UniformScale(self.initial_target_scale / new_target.norm(), self.n_dims)
        centre = Translation(-new_target.centre)
        rescale_about_centre = centre.compose_before(rescale).compose_before(centre.pseudoinverse)
        rescale_about_centre.apply_inplace(new_target)
        # check to see if  we have converged yet
        delta_target = np.linalg.norm(self.target.points - new_target.points)
        if delta_target < 1e-6:
            return True
        else:
            self.n_iterations += 1
            for t in self.transforms:
                t.set_target(new_target)
            self.target = new_target
            return self._recursive_procrustes()

def lm_centres_correction(centres):
    r"""
    Construct a transform that will correct landmarks for a window
    iterating feature calculation

    Parameters
    ----------
    centres : `ndarray` (H, W, 2)
        The location of the window centres in the features

    Returns
    -------
    :map:`Affine`
        An affine transform that performs the correction.
        Should be applied to the landmarks on the target image.
    """
    t = Translation(-centres.min(axis=0).min(axis=0), skip_checks=True)
    step_v = centres[0, 0, 0]
    if centres.shape[0] > 1:
        step_v = centres[1, 0, 0] - centres[0, 0, 0]
    step_h = centres[0, 0, 1]
    if centres.shape[1] > 1:
        step_h = centres[0, 1, 1] - centres[0, 0, 1]
    s = NonUniformScale((1./step_v, 1./step_h), skip_checks=True)
    return t.compose_before(s)

def test_translation_compose_after_homog():
    # can't do this inplace - so should just give transform chain
    homog = Homogeneous(np.array([[0, 1, 0],
                                  [1, 0, 0],
                                  [0, 0, 1]]))
    t = Translation([3, 4])
    res = t.compose_after(homog)
    assert(type(res) == Homogeneous)

def test_align_2d_translation():
    t_vec = np.array([1, 2])
    translation = Translation(t_vec)
    source = PointCloud(np.array([[0, 1], [1, 1], [-1, -5], [3, -5]]))
    target = translation.apply(source)
    # estimate the transform from source and target
    estimate = AlignmentTranslation(source, target)
    # check the estimates is correct
    assert_allclose(translation.h_matrix, estimate.h_matrix)

def test_align_2d_translation_set_h_matrix_raises_notimplemented_error():
    t_vec = np.array([1, 2])
    translation = Translation(t_vec)
    source = PointCloud(np.array([[0, 1], [1, 1], [-1, -5], [3, -5]]))
    target = translation.apply(source)
    # estimate the transform from source to source..
    estimate = AlignmentTranslation(source, source)
    # and change the target.
    estimate.set_h_matrix(translation.h_matrix)

    def crop(self, min_indices, max_indices, constrain_to_boundary=True):
        r"""
        Crops this image using the given minimum and maximum indices.
        Landmarks are correctly adjusted so they maintain their position
        relative to the newly cropped image.

        Parameters
        -----------
        min_indices: (n_dims, ) ndarray
            The minimum index over each dimension

        max_indices: (n_dims, ) ndarray
            The maximum index over each dimension

        constrain_to_boundary: boolean, optional
            If True the crop will be snapped to not go beyond this images
            boundary. If False, an ImageBoundaryError will be raised if an
            attempt is made to go beyond the edge of the image.

            Default: True

        Returns
        -------
        cropped_image : :class:`type(self)`
            This image, but cropped.

        Raises
        ------
        ValueError
            min_indices and max_indices both have to be of length n_dims.
            All max_indices must be greater than min_indices.

        ImageBoundaryError
            Raised if constrain_to_boundary is False, and an attempt is made
            to crop the image in a way that violates the image bounds.

        """
        min_indices = np.floor(min_indices)
        max_indices = np.ceil(max_indices)
        if not (min_indices.size == max_indices.size == self.n_dims):
            raise ValueError(
                "Both min and max indices should be 1D numpy arrays of" " length n_dims ({})".format(self.n_dims)
            )
        elif not np.all(max_indices > min_indices):
            raise ValueError("All max indices must be greater that the min " "indices")
        min_bounded = self.constrain_points_to_bounds(min_indices)
        max_bounded = self.constrain_points_to_bounds(max_indices)
        if not constrain_to_boundary and not (np.all(min_bounded == min_indices) or np.all(max_bounded == max_indices)):
            # points have been constrained and the user didn't want this -
            raise ImageBoundaryError(min_indices, max_indices, min_bounded, max_bounded)
        slices = [slice(int(min_i), int(max_i)) for min_i, max_i in zip(list(min_bounded), list(max_bounded))]
        self.pixels = self.pixels[slices].copy()
        # update all our landmarks
        lm_translation = Translation(-min_bounded)
        lm_translation.apply_inplace(self.landmarks)
        return self

def _build_reference_frame(landmarks, boundary=3, group='source'):
    # translate landmarks to the origin
    minimum = landmarks.bounds(boundary=boundary)[0]
    landmarks = Translation(-minimum).apply(landmarks)

    resolution = landmarks.range(boundary=boundary)
    reference_frame = MaskedImage.blank(resolution)
    reference_frame.landmarks[group] = landmarks

    return reference_frame

def test_align_2d_translation_from_vector_inplace():
    t_vec = np.array([1, 2])
    translation = Translation(t_vec)
    source = PointCloud(np.array([[0, 1], [1, 1], [-1, -5], [3, -5]]))
    target = translation.apply(source)
    # estimate the transform from source to source..
    estimate = AlignmentTranslation(source, source)
    # and update from_vector
    estimate.from_vector_inplace(t_vec)
    # check the estimates is correct
    assert_allclose(target.points, estimate.target.points)

def chain_compose_after_inplace_chain_test():
    a = PointCloud(np.random.random([10, 2]))
    b = PointCloud(np.random.random([10, 2]))

    t = Translation([3, 4])
    s = Scale([4, 2])
    chain_1 = TransformChain([t, s])
    chain_2 = TransformChain([s.pseudoinverse(), t.pseudoinverse()])
    chain_1.compose_before_inplace(chain_2)

    points = PointCloud(np.random.random([10, 2]))
    chain_res = chain_1.apply(points)
    assert(np.allclose(points.points, chain_res.points))

    def init_from_pointcloud(cls, pointcloud, group=None, boundary=0,
                             constrain=True, fill=True):
        r"""
        Create an Image that is big enough to contain the given pointcloud.
        The pointcloud will be translated to the origin and then translated
        according to its bounds in order to fit inside the new image.
        An optional boundary can be provided in order to increase the space
        around the boundary of the pointcloud. The boundary will be added
        to *all sides of the image* and so a boundary of 5 provides 10 pixels
        of boundary total for each dimension.

        By default, the mask will be constrained to the convex hull of the
        provided pointcloud.

        Parameters
        ----------
        pointcloud : :map:`PointCloud`
            Pointcloud to place inside the newly created image.
        group : `str`, optional
            If ``None``, the pointcloud will only be used to create the image.
            If a `str` then the pointcloud will be attached as a landmark
            group to the image, with the given string as key.
        boundary : `float`
            A optional padding distance that is added to the pointcloud bounds.
            Default is ``0``, meaning the max/min of tightest possible
            containing image is returned.
        fill : `int`, optional
            The value to fill all pixels with.
        constrain : `bool`, optional
            If ``True``, the ``True`` values will be image will be constrained
            to the convex hull of the provided pointcloud. If ``False``,
            the mask will be the value of ``fill``.

        Returns
        -------
        image : :map:`MaskedImage`
            A new image with the same size as the given pointcloud, optionally
            with the pointcloud attached as landmarks and the mask constrained
            to the convex hull of the pointcloud.
        """
        # Translate pointcloud to the origin
        minimum = pointcloud.bounds(boundary=boundary)[0]
        origin_pc = Translation(-minimum).apply(pointcloud)
        image_shape = origin_pc.range(boundary=boundary)
        new_image = cls.init_blank(image_shape, fill=fill)
        if constrain:
            new_image = new_image.constrain_to_pointcloud(origin_pc)
        if group is not None:
            new_image.landmarks[group] = origin_pc
        return new_image

def chain_compose_before_tps_test():
    a = PointCloud(np.random.random([10, 2]))
    b = PointCloud(np.random.random([10, 2]))
    tps = ThinPlateSplines(a, b)

    t = Translation([3, 4])
    s = Scale([4, 2])
    chain = TransformChain([t, s])
    chain_mod = chain.compose_before(tps)

    points = PointCloud(np.random.random([10, 2]))

    manual_res = tps.apply(s.apply(t.apply(points)))
    chain_res = chain_mod.apply(points)
    assert(np.all(manual_res.points == chain_res.points))

def test_align_2d_translation_from_vector():
    t_vec = np.array([1, 2])
    translation = Translation(t_vec)
    source = PointCloud(np.array([[0, 1], [1, 1], [-1, -5], [3, -5]]))
    target = translation.apply(source)
    # estimate the transform from source to source..
    estimate = AlignmentTranslation(source, source)
    # and update from_vector
    new_est = estimate.from_vector(t_vec)
    # check the original is unchanged
    assert_allclose(estimate.source.points, source.points)
    assert_allclose(estimate.target.points, source.points)
    # check the new estimate has the source and target correct
    assert_allclose(new_est.source.points, source.points)
    assert_allclose(new_est.target.points, target.points)

def model_to_clip_transform(points, xy_scale=0.9, z_scale=0.3):
    r"""
    Produces an Affine Transform which centres and scales 3D points to fit
    into the OpenGL clipping space ([-1, 1], [-1, 1], [1, 1-]). This can be
    used to construct an appropriate projection matrix for use in an
    orthographic Rasterizer. Note that the z-axis is flipped as is default in
    OpenGL - as a result this transform converts the right handed coordinate
    input into a left hand one.

    Parameters
    ----------

    points: :map:`PointCloud`
        The points that should be adjusted.

    xy_scale: `float` 0-1, optional
        Amount by which the boundary is relaxed so the points are not
        right against the edge. A value of 1 means the extremities of the
        point cloud will be mapped onto [-1, 1] [-1, 1] exactly (no boarder)
        A value of 0.5 means the points will be mapped into the range
        [-0.5, 0.5].

        Default: 0.9 (map to [-0.9, 0.9])

    z_scale: float 0-1, optional
        Scale factor by which the z-dimension is squeezed. A value of 1
        means the z-range of the points will be mapped to exactly fit in
        [1, -1]. A scale of 0.1 means the z-range is compressed to fit in the
        range [0.1, -0.1].

    Returns
    -------
    :map:`Affine`
        The affine transform that creates this mapping
    """
    # 1. Centre the points on the origin
    center = Translation(points.centre_of_bounds()).pseudoinverse()
    # 2. Scale the points to exactly fit the boundaries
    scale = Scale(points.range() / 2.0)
    # 3. Apply the relaxations requested - note the flip in the z axis!!
    # This is because OpenGL by default evaluates depth as bigger number ==
    # further away. Thus not only do we need to get to clip space [-1, 1] in
    # all dims) but we must invert the z axis so depth buffering is correctly
    # applied.
    b_scale = NonUniformScale([xy_scale, xy_scale, -z_scale])
    return center.compose_before(scale.pseudoinverse()).compose_before(b_scale)

def test_translation_2d_from_vector():
    params = np.array([1, 2])
    homo = np.array([[1, 0, params[0]],
                     [0, 1, params[1]],
                     [0, 0, 1]])

    tr = Translation.init_identity(2).from_vector(params)

    assert_equal(tr.h_matrix, homo)

def test_translation_3d_from_vector():
    params = np.array([1, 2, 3])
    homo = np.array([[1, 0, 0, params[0]],
                     [0, 1, 0, params[1]],
                     [0, 0, 1, params[2]],
                     [0, 0, 0, 1]])

    tr = Translation.identity(3).from_vector(params)

    assert_almost_equal(tr.h_matrix, homo)

def test_translation_identity_3d():
    assert_allclose(Translation.init_identity(3).h_matrix, np.eye(4))

def non_rigid_icp(source, target, eps=1e-3, stiffness_values=None,
                  verbose=False, landmarks=None, lm_weight=None):
    r"""
    Deforms the source trimesh to align with to optimally the target.
    """
    # Scale factors completely change the behavior of the algorithm - always
    # rescale the source down to a sensible size (so it fits inside box of
    # diagonal 1) and is centred on the origin. We'll undo this after the fit
    # so the user can use whatever scale they prefer.
    tr = Translation(-1 * source.centre())
    sc = UniformScale(1.0 / np.sqrt(np.sum(source.range() ** 2)), 3)
    prepare = tr.compose_before(sc)

    source = prepare.apply(source)
    target = prepare.apply(target)

    # store how to undo the similarity transform
    restore = prepare.pseudoinverse()

    n_dims = source.n_dims
    # Homogeneous dimension (1 extra for translation effects)
    h_dims = n_dims + 1
    points, trilist = source.points, source.trilist
    n = points.shape[0]  # record number of points

    edge_tris = source.boundary_tri_index()

    M_s = node_arc_incidence_matrix(source)

    # weight matrix
    G = np.identity(n_dims + 1)

    M_kron_G_s = sp.kron(M_s, G)

    # build octree for finding closest points on target.
    target_vtk = trimesh_to_vtk(target)
    closest_points_on_target = VTKClosestPointLocator(target_vtk)

    # save out the target normals. We need them for the weight matrix.
    target_tri_normals = target.tri_normals()

    # init transformation
    X_prev = np.tile(np.zeros((n_dims, h_dims)), n).T
    v_i = points

    if stiffness_values is not None:
        stiffness = stiffness_values
        if verbose:
            print('using user defined stiffness values: {}'.format(stiffness))
    else:
        # these values have been empirically found to perform well for well
        # rigidly aligned facial meshes
        stiffness = [50, 20, 5, 2, 0.8, 0.5, 0.35, 0.2]
        if verbose:
            print('using default stiffness values: {}'.format(stiffness))

    if lm_weight is not None:
        lm_weight = lm_weight
        if verbose:
            print('using user defined lm_weight values: {}'.format(lm_weight))
    else:
        # these values have been empirically found to perform well for well
        # rigidly aligned facial meshes
        lm_weight = [5,  2, .5, 0,   0,   0,    0,    0]
        if verbose:
            print('using default lm_weight values: {}'.format(lm_weight))

    # to store per iteration information
    info = []

    # we need to prepare some indices for efficient construction of the D
    # sparse matrix.
    row = np.hstack((np.repeat(np.arange(n)[:, None], n_dims, axis=1).ravel(),
                     np.arange(n)))

    x = np.arange(n * h_dims).reshape((n, h_dims))
    col = np.hstack((x[:, :n_dims].ravel(),
                     x[:, n_dims]))

    if landmarks is not None:
        if verbose:
            print("'{}' landmarks will be used as a landmark constraint.".format(landmarks))
        source_lm_index = source.distance_to(
            source.landmarks[landmarks].lms).argmin(axis=0)
        target_lms = target.landmarks[landmarks].lms
        U_L = target_lms.points
        n_landmarks = target_lms.n_points
        lm_mask = np.in1d(row, source_lm_index)
        col_lm = col[lm_mask]
        # pull out the rows for the lms - but the values are
        # all wrong! need to map them back to the order of the landmarks
        row_lm_to_fix = row[lm_mask]
        source_lm_index_l = list(source_lm_index)
        row_lm = np.array([source_lm_index_l.index(r) for r in row_lm_to_fix])

    o = np.ones(n)

    for alpha, beta in zip(stiffness, lm_weight):
        alpha_M_kron_G_s = alpha * M_kron_G_s  # get the term for stiffness
        j = 0
#.........这里部分代码省略.........