patty.registration.coarse_registration() - Code Metrics - NLeSC/PattyAnalytics - Measure and Improve Code Quality continuously with Scrutinizer

patty.registration.coarse_registration() B
last analyzed 2016-03-07 10:18 UTC

↳ Parent: Project

Complexity

Conditions

Size

Total Lines

Duplication

Lines	0
Ratio	0 %

Code Coverage

Tests	1
CRAP Score	11.1001

Metric	Value
cc	3
dl	0
loc	83
ccs	1
cts	29
cp	0.0345
crap	11.1001
rs	8.7468

How to fix Long Method

#!/usr/bin/env python

"""
Registration algorithms and utility functions
"""

from __future__ import print_function
import numpy as np
from .. import BoundingBox, force_srs, extract_mask, clone
from .stickscale import get_stick_scale
from pcl.registration import gicp

from patty.utils import (
    log,
    save,
    downsample_voxel,
)

from patty.segmentation import (
    boundary_of_center_object,
    boundary_of_drivemap,
    boundary_of_lowest_points,
)

from sklearn.decomposition import PCA


def align_footprints(loose_pc, fixed_pc,
                     allow_scaling=True,
                     allow_rotation=True,
                     allow_translation=True):
    '''
    Align a pointcloud 'loose_pc' by placing it on top of
    'fixed_pc' as good as poosible. Done by aligning the
    principle axis of both pointclouds.

    NOTE: Both pointclouds are assumed to be the footprint (or projection)
    on the xy plane, with basically zero extent along the z-axis.

    (allow_rotation=True)
        The pointcloud boundary is alinged with the footprint
        by rotating its pricipal axis in the (x,y) plane.

    (allow_translation=True)
        Then, it is translated so the centers of mass coincide.

    (allow_scaling=True)
        Finally, the pointcloud is scaled to have the same extent.

    Arguments:
        loose_pc          : pcl.PointCloud
        fixed_pc          : pcl.PointCloud

        allow_scaling     : Bolean
        allow_rotation    : Bolean
        allow_translation : Bolean

    Returns:
        rot_matrix, rot_center, scale, translation : np.array()

    '''

    rot_center = loose_pc.center()

    if allow_rotation:
        log(" - Finding rotation")
        rot_matrix = find_rotation_xy(loose_pc, fixed_pc)
        loose_pc.rotate(rot_matrix, origin=rot_center)
    else:
        log(" - Skipping rotation")
        rot_matrix = np.eye(3)

    if allow_scaling:
        fixed_bb = BoundingBox(fixed_pc)  # used 2x below
        loose_bb = BoundingBox(loose_pc)
        scale = fixed_bb.size[0:2] / loose_bb.size[0:2]

        # take the average scale factor for the x and y dimensions
        scale = np.mean(scale)
        loose_pc.scale(scale, origin=rot_center)
        log(" - Scale: %s" % scale)
    else:
        log(" - Skipping scale")
        scale = 1.0

    if allow_translation:
        translation = fixed_pc.center() - rot_center
        loose_pc.translate(translation)
        log(" - Translation: %s" % translation)
    else:
        log(" - Skipping translation")
        translation = np.array([0.0, 0.0, 0.0])

    return rot_matrix, rot_center, scale, translation


def estimate_pancake_up(pointcloud):
    '''
    Assuming a pancake like pointcloud, the up direction is the third PCA.
    '''
    pca = PCA(n_components=3)

    points = np.asarray(pointcloud)
    pca.fit(points[:, 0:3])

    return pca.components_[2]


def _find_rotation_xy_helper(pointcloud):
    pca = PCA(n_components=2)

    points = np.asarray(pointcloud)
    pca.fit(points[:, 0:2])

    rotxy = np.array(pca.components_)

    # make sure the rotation is a proper rotation, ie det = +1
    if np.linalg.det(rotxy) < 0:
        rotxy[:, 1] *= -1.0

    # create a 3D rotation around the z-axis
    rotation = np.eye(3)
    rotation[0:2, 0:2] = rotxy

    return rotation


def find_rotation_xy(pc, ref):
    '''Find the transformation that rotates the principal axis of the
    pointcloud onto those of the reference.
    Keep the z-axis pointing upwards.

    Arguments:
        pc: pcl.PointCloud

        ref: pcl.PointCloud

    Returns:
        numpy array of shape [3,3], can be used to rotate pointclouds
        with pc.rotate()
    '''

    pc_transform = _find_rotation_xy_helper(pc)
    ref_transform = _find_rotation_xy_helper(ref)

    return np.dot(np.linalg.inv(ref_transform), pc_transform)


def rotate_upwards(pc, up):
    '''
    Rotate the pointcloud in-place around its center, such that the
    'up' vector points along [0,0,1]

    Arguments:
        pc : pcl.PointCloud
        up : np.array([3])

    Returns:
        pc : pcl.PointCloud the input pointcloud, for convenience.

    '''

    newz = np.array(up)

    # Right-handed coordiante system:
    # np.cross(x,y) = z
    # np.cross(y,z) = x
    # np.cross(z,x) = y

    # normalize
    newz /= (np.dot(newz, newz)) ** 0.5

    # find two orthogonal vectors to represent x and y,
    # randomly choose a vector, and take cross product. If we're unlucky,
    # this ones is parallel to z, so cross pruduct is zero.
    # In that case, try another one
    try:
        newx = np.cross(np.array([0, 1, 0]), newz)
        newx /= (np.dot(newx, newx)) ** 0.5

        newy = np.cross(newz, newx)
        newy /= (np.dot(newy, newy)) ** 0.5
    except:
        print("Alternative")
        newy = np.cross(newz, np.array([1, 0, 0]))
        newy /= (np.dot(newy, newy)) ** 0.5

        newx = np.cross(newy, newz)
        newx /= (np.dot(newx, newx)) ** 0.5

    rotation = np.zeros([3, 3])
    rotation[0, 0] = newx[0]
    rotation[1, 0] = newx[1]
    rotation[2, 0] = newx[2]

    rotation[0, 1] = newy[0]
    rotation[1, 1] = newy[1]
    rotation[2, 1] = newy[2]

    rotation[0, 2] = newz[0]
    rotation[1, 2] = newz[1]
    rotation[2, 2] = newz[2]

    rotation = np.linalg.inv(rotation)
    pc.rotate(rotation, origin=pc.center())

    return pc


def initial_registration(pointcloud, up, drivemap,
                         initial_scale=None, trust_up=True):
    """
    Initial registration adds an spatial reference system to the pointcloud,
    and place the pointlcoud on top of the drivemap. The pointcloud is rotated
    so that the up vector points along [0,0,1], and scaled such that it has the
    right order of magnitude in size.

    Arguments:
        pointcloud : pcl.PointCloud
            The high-res object to register.

        up: np.array([3])
            Up direction for the pointcloud.
            If None, assume the object is pancake shaped, and chose the
            upvector such that it is perpendicullar to the pancake.

        drivemap : pcl.PointCloud
            A small part of the low-res drivemap on which to register.

        initial_scale : float
            if given, scale pointcloud using this value; estimate scale factor
            from bounding boxes.

        trust_up : Boolean, default to True
            True:  Assume the up vector is exact.
            False: Calculate 'up' as if it was None, but orient it such that
                   np.dot( up, pancake_up ) > 0

    NOTE: Modifies the input pointcloud in-place, and leaves
    it in a undefined state.

    """
    log("Starting initial registration")

    #####
    # set scale and offset of pointcloud, drivemap, and footprint
    # as the pointcloud is unregisterd, the coordinate system is undefined,
    # and we lose nothing if we just copy it

    if(hasattr(pointcloud, "offset")):
        log(" - Dropping initial offset, was: %s" % pointcloud.offset)
    else:
        log(" - No initial offset")
    force_srs(pointcloud, same_as=drivemap)
    log(" - New offset forced to: %s" % pointcloud.offset)

    if up is not None:
        log(" - Rotating the pointcloud so up points along [0,0,1]")

        if trust_up:
            rotate_upwards(pointcloud, up)
            log(" - Using trusted up: %s" % up)
        else:
            pancake_up = estimate_pancake_up(pointcloud)
            if np.dot(up, pancake_up) < 0.0:
                pancake_up *= -1.0
            log(" - Using estimated up: %s" % pancake_up)
            rotate_upwards(pointcloud, pancake_up)

    else:
        log(" - No upvector, skipping")

    if initial_scale is None:
        bbDrivemap = BoundingBox(points=np.asarray(drivemap))

        bbObject = BoundingBox(points=np.asarray(pointcloud))

        scale = bbDrivemap.size[0:2] / bbObject.size[0:2]  # ignore z-direction

        # take the average scale factor for x and y dimensions
        scale = np.mean(scale)
    else:
        # use user provided scale
        scale = initial_scale

    log(" - Applying rough estimation of scale factor", scale)
    pointcloud.scale(scale)  # dont care about origin of scaling


def coarse_registration(pointcloud, drivemap, footprint, downsample=None):
    """
    Improve the initial registration.
    Find the proper scale by looking for the red meter sticks, and calculate
    and align the pointcloud's footprint.

    Arguments:
        pointcloud: pcl.PointCloud
                    The high-res object to register.

        drivemap:   pcl.PointCloud
                    A small part of the low-res drivemap on which to register

        footprint:  pcl.PointCloud
                    Pointlcloud containing the objects footprint

        downsample: float, default=None, no resampling
                    Downsample the high-res pointcloud before footprint
                    calculation.
    """
    log("Starting coarse registration")

    ###
    # find redstick scale, and use it if possible
    log(" - Redstick scaling")

    allow_scaling = True

    scale, confidence = get_stick_scale(pointcloud)
    log(" - Red stick scale=%s confidence=%s" % (scale, confidence))

    if (confidence > 0.5):
        log(" - Applying red stick scale")
        pointcloud.scale(1.0 / scale)  # dont care about origin of scaling
        allow_scaling = False
    else:
        log(" - Not applying red stick scale, confidence too low")

    #####
    # find all the points in the drivemap along the footprint
    # use bottom two meters of drivemap (not trees)

    dm_boundary = boundary_of_drivemap(drivemap, footprint)
    dm_bb = BoundingBox(dm_boundary)

    # set footprint height from minimum value,
    # as trees, or high object make the pc.center() too high
    fixed_boundary = clone(footprint)
    fp_array = np.asarray(fixed_boundary)
    fp_array[:, 2] = dm_bb.min[2]

    save(fixed_boundary, "fixed_bound.las")

    #####
    # find all the boundary points of the pointcloud

    loose_boundary = boundary_of_center_object(pointcloud, downsample)
    if loose_boundary is None:
        log(" - boundary estimation failed, using lowest 30 percent of points")
        loose_boundary = boundary_of_lowest_points(pointcloud,
                                                   height_fraction=0.3)

    ####
    # match the pointcloud boundary with the footprint boundary

    log(" - Aligning footprints:")
    rot_matrix, rot_center, scale, translation = align_footprints(
        loose_boundary, fixed_boundary,
        allow_scaling=allow_scaling,
        allow_rotation=True,
        allow_translation=True)

    save(loose_boundary, "aligned_bound.las")

    ####
    # Apply to the main pointcloud

    pointcloud.rotate(rot_matrix, origin=rot_center)
    pointcloud.scale(scale, origin=rot_center)
    pointcloud.translate(translation)
    rot_center += translation

    return rot_center


def _fine_registration_helper(pointcloud, drivemap, voxelsize=0.05, attempt=0):
    """
    Perform ICP on pointcloud with drivemap, and return convergence indicator.
    Reject large translatoins.

    Returns:
        transf : np.array([4,4])
            transform
        success : Boolean
            if icp was successful
        fitness : float
            sort of sum of square differences, ie. smaller is better
    """
    ####
    # Downsample to speed up
    # use voxel filter to keep evenly distributed spatial extent

    log(" - Downsampling with voxel filter: %s" % voxelsize)
    pc = downsample_voxel(pointcloud, voxelsize)

    ####
    # Clip to drivemap to prevent outliers confusing the ICP algorithm

    log(" - Clipping to drivemap")
    bb = BoundingBox(drivemap)
    z = bb.center[2]

    extracted = extract_mask(pc, [bb.contains([point[0], point[1], z])
                                  for point in pc])

    log(" - Remaining points: %s" % len(extracted))

    ####
    # GICP

    converged, transf, estimate, fitness = gicp(extracted, drivemap)

    ####
    # Dont accept large translations

    translation = transf[0:3, 3]
    if np.dot(translation, translation) > 5 ** 2:
        log(" - Translation too large, considering it a failure.")
        converged = False
        fitness = 1e30
    else:
        log(" - Success, fitness: ", converged, fitness)

    force_srs(estimate, same_as=pointcloud)
    save(estimate, "attempt%s.las" % attempt)

    return transf, converged, fitness


def fine_registration(pointcloud, drivemap, center, voxelsize=0.05):
    """
    Final registration step using ICP.

    Find the local optimal postion of the pointcloud on the drivemap; due to
    our coarse_registration algorithm, we have to try two orientations:
    original, and rotated by 180 degrees around the z-axis.

    Arguments:
        pointcloud: pcl.PointCloud
                    The high-res object to register.

        drivemap: pcl.PointCloud
                    A small part of the low-res drivemap on which to register

        center: np.array([3])
                    Vector giving the centerpoint of the pointcloud, used to do
                    the 180 degree rotations.

        voxelsize: float default : 0.05
                    Size in [m] of the voxel grid used for downsampling
    """
    log("Starting fine registration")

    # for rotation around z-axis
    rot = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])

    ####
    # do a ICP step for 4 orientations

    transf = {}
    success = {}
    fitness = {}
    for i in range(4):
        log(" - attempt: %s" % i)
        transf[i], success[i], fitness[i] = _fine_registration_helper(
            pointcloud,
            drivemap,
            attempt=i,
            voxelsize=voxelsize)
        pointcloud.rotate(rot, origin=center)

    ####
    # pick best

    best, value = min(fitness.iteritems(), key=lambda x: x[1])
    if success[best]:
        log(" - Best attempt: %s" % best)
        if best > 0:  # np.array()**0 does weird things
            pointcloud.rotate(rot**best, origin=center)
        pointcloud.transform(transf[best])
        return

    # ICP failed:
    # return the pointcloud with just footprints aligned
    # no use to undo a rotation, as any orientationi is equally likely.
    log(" - Unable to do fine registration")

    return


1		#!/usr/bin/env python
2
3	1	"""
4		Registration algorithms and utility functions
5		"""
6
7	1	from __future__ import print_function
8	1	import numpy as np
9	1	from .. import BoundingBox, force_srs, extract_mask, clone
10	1	from .stickscale import get_stick_scale
11	1	from pcl.registration import gicp
12
13	1	from patty.utils import (
14		log,
15		save,
16		downsample_voxel,
17		)
18
19	1	from patty.segmentation import (
20		boundary_of_center_object,
21		boundary_of_drivemap,
22		boundary_of_lowest_points,
23		)
24
25	1	from sklearn.decomposition import PCA
26
27
28	1	def align_footprints(loose_pc, fixed_pc,
29		allow_scaling=True,
30		allow_rotation=True,
31		allow_translation=True):
32		'''
33		Align a pointcloud 'loose_pc' by placing it on top of
34		'fixed_pc' as good as poosible. Done by aligning the
35		principle axis of both pointclouds.
36
37		NOTE: Both pointclouds are assumed to be the footprint (or projection)
38		on the xy plane, with basically zero extent along the z-axis.
39
40		(allow_rotation=True)
41		The pointcloud boundary is alinged with the footprint
42		by rotating its pricipal axis in the (x,y) plane.
43
44		(allow_translation=True)
45		Then, it is translated so the centers of mass coincide.
46
47		(allow_scaling=True)
48		Finally, the pointcloud is scaled to have the same extent.
49
50		Arguments:
51		loose_pc : pcl.PointCloud
52		fixed_pc : pcl.PointCloud
53
54		allow_scaling : Bolean
55		allow_rotation : Bolean
56		allow_translation : Bolean
57
58		Returns:
59		rot_matrix, rot_center, scale, translation : np.array()
60
61		'''
62
63		rot_center = loose_pc.center()
64
65		if allow_rotation:
66		log(" - Finding rotation")
67		rot_matrix = find_rotation_xy(loose_pc, fixed_pc)
68		loose_pc.rotate(rot_matrix, origin=rot_center)
69		else:
70		log(" - Skipping rotation")
71		rot_matrix = np.eye(3)
72
73		if allow_scaling:
74		fixed_bb = BoundingBox(fixed_pc) # used 2x below
75		loose_bb = BoundingBox(loose_pc)
76		scale = fixed_bb.size[0:2] / loose_bb.size[0:2]
77
78		# take the average scale factor for the x and y dimensions
79		scale = np.mean(scale)
80		loose_pc.scale(scale, origin=rot_center)
81		log(" - Scale: %s" % scale)
82		else:
83		log(" - Skipping scale")
84		scale = 1.0
85
86		if allow_translation:
87		translation = fixed_pc.center() - rot_center
88		loose_pc.translate(translation)
89		log(" - Translation: %s" % translation)
90		else:
91		log(" - Skipping translation")
92		translation = np.array([0.0, 0.0, 0.0])
93
94		return rot_matrix, rot_center, scale, translation
95
96
97	1	def estimate_pancake_up(pointcloud):
98		'''
99		Assuming a pancake like pointcloud, the up direction is the third PCA.
100		'''
101		pca = PCA(n_components=3)
102
103		points = np.asarray(pointcloud)
104		pca.fit(points[:, 0:3])
105
106		return pca.components_[2]
107
108
109	1	def _find_rotation_xy_helper(pointcloud):
110	1	pca = PCA(n_components=2)
111
112	1	points = np.asarray(pointcloud)
113	1	pca.fit(points[:, 0:2])
114
115	1	rotxy = np.array(pca.components_)
116
117		# make sure the rotation is a proper rotation, ie det = +1
118	1	if np.linalg.det(rotxy) < 0:
119	1	rotxy[:, 1] *= -1.0
120
121		# create a 3D rotation around the z-axis
122	1	rotation = np.eye(3)
123	1	rotation[0:2, 0:2] = rotxy
124
125	1	return rotation
126
127
128	1	def find_rotation_xy(pc, ref):
129		'''Find the transformation that rotates the principal axis of the
130		pointcloud onto those of the reference.
131		Keep the z-axis pointing upwards.
132
133		Arguments:
134		pc: pcl.PointCloud
135
136		ref: pcl.PointCloud
137
138		Returns:
139		numpy array of shape [3,3], can be used to rotate pointclouds
140		with pc.rotate()
141		'''
142
143	1	pc_transform = _find_rotation_xy_helper(pc)
144	1	ref_transform = _find_rotation_xy_helper(ref)
145
146	1	return np.dot(np.linalg.inv(ref_transform), pc_transform)
147
148
149	1	def rotate_upwards(pc, up):
150		'''
151		Rotate the pointcloud in-place around its center, such that the
152		'up' vector points along [0,0,1]
153
154		Arguments:
155		pc : pcl.PointCloud
156		up : np.array([3])
157
158		Returns:
159		pc : pcl.PointCloud the input pointcloud, for convenience.
160
161		'''
162
163		newz = np.array(up)
164
165		# Right-handed coordiante system:
166		# np.cross(x,y) = z
167		# np.cross(y,z) = x
168		# np.cross(z,x) = y
169
170		# normalize
171		newz /= (np.dot(newz, newz)) ** 0.5
172
173		# find two orthogonal vectors to represent x and y,
174		# randomly choose a vector, and take cross product. If we're unlucky,
175		# this ones is parallel to z, so cross pruduct is zero.
176		# In that case, try another one
177		try:
178		newx = np.cross(np.array([0, 1, 0]), newz)
179		newx /= (np.dot(newx, newx)) ** 0.5
180
181		newy = np.cross(newz, newx)
182		newy /= (np.dot(newy, newy)) ** 0.5
183		except:
184		print("Alternative")
185		newy = np.cross(newz, np.array([1, 0, 0]))
186		newy /= (np.dot(newy, newy)) ** 0.5
187
188		newx = np.cross(newy, newz)
189		newx /= (np.dot(newx, newx)) ** 0.5
190
191		rotation = np.zeros([3, 3])
192		rotation[0, 0] = newx[0]
193		rotation[1, 0] = newx[1]
194		rotation[2, 0] = newx[2]
195
196		rotation[0, 1] = newy[0]
197		rotation[1, 1] = newy[1]
198		rotation[2, 1] = newy[2]
199
200		rotation[0, 2] = newz[0]
201		rotation[1, 2] = newz[1]
202		rotation[2, 2] = newz[2]
203
204		rotation = np.linalg.inv(rotation)
205		pc.rotate(rotation, origin=pc.center())
206
207		return pc
208
209
210	1	def initial_registration(pointcloud, up, drivemap,
211		initial_scale=None, trust_up=True):
212		"""
213		Initial registration adds an spatial reference system to the pointcloud,
214		and place the pointlcoud on top of the drivemap. The pointcloud is rotated
215		so that the up vector points along [0,0,1], and scaled such that it has the
216		right order of magnitude in size.
217
218		Arguments:
219		pointcloud : pcl.PointCloud
220		The high-res object to register.
221
222		up: np.array([3])
223		Up direction for the pointcloud.
224		If None, assume the object is pancake shaped, and chose the
225		upvector such that it is perpendicullar to the pancake.
226
227		drivemap : pcl.PointCloud
228		A small part of the low-res drivemap on which to register.
229
230		initial_scale : float
231		if given, scale pointcloud using this value; estimate scale factor
232		from bounding boxes.
233
234		trust_up : Boolean, default to True
235		True: Assume the up vector is exact.
236		False: Calculate 'up' as if it was None, but orient it such that
237		np.dot( up, pancake_up ) > 0
238
239		NOTE: Modifies the input pointcloud in-place, and leaves
240		it in a undefined state.
241
242		"""
243		log("Starting initial registration")
244
245		#####
246		# set scale and offset of pointcloud, drivemap, and footprint
247		# as the pointcloud is unregisterd, the coordinate system is undefined,
248		# and we lose nothing if we just copy it
249
250		if(hasattr(pointcloud, "offset")):
251		log(" - Dropping initial offset, was: %s" % pointcloud.offset)
252		else:
253		log(" - No initial offset")
254		force_srs(pointcloud, same_as=drivemap)
255		log(" - New offset forced to: %s" % pointcloud.offset)
256
257		if up is not None:
258		log(" - Rotating the pointcloud so up points along [0,0,1]")
259
260		if trust_up:
261		rotate_upwards(pointcloud, up)
262		log(" - Using trusted up: %s" % up)
263		else:
264		pancake_up = estimate_pancake_up(pointcloud)
265		if np.dot(up, pancake_up) < 0.0:
266		pancake_up *= -1.0
267		log(" - Using estimated up: %s" % pancake_up)
268		rotate_upwards(pointcloud, pancake_up)
269
270		else:
271		log(" - No upvector, skipping")
272
273		if initial_scale is None:
274		bbDrivemap = BoundingBox(points=np.asarray(drivemap))
		0 ignored issues – show Coding Style Naming introduced 2016-02-11 14:39 UTC by Report Bug Copy Issue Report The name `bbDrivemap` does not conform to the variable naming conventions (`[a-z_][a-z0-9_]{1,30}$`). This check looks for invalid names for a range of different identifiers. You can set regular expressions to which the identifiers must conform if the defaults do not match your requirements. If your project includes a Pylint configuration file, the settings contained in that file take precedence. To find out more about Pylint, please refer to their site. Loading history...
275		bbObject = BoundingBox(points=np.asarray(pointcloud))
		0 ignored issues – show Coding Style Naming introduced 2016-02-11 14:39 UTC by Report Bug Copy Issue Report The name `bbObject` does not conform to the variable naming conventions (`[a-z_][a-z0-9_]{1,30}$`). This check looks for invalid names for a range of different identifiers. You can set regular expressions to which the identifiers must conform if the defaults do not match your requirements. If your project includes a Pylint configuration file, the settings contained in that file take precedence. To find out more about Pylint, please refer to their site. Loading history...
276		scale = bbDrivemap.size[0:2] / bbObject.size[0:2] # ignore z-direction
277
278		# take the average scale factor for x and y dimensions
279		scale = np.mean(scale)
280		else:
281		# use user provided scale
282		scale = initial_scale
283
284		log(" - Applying rough estimation of scale factor", scale)
285		pointcloud.scale(scale) # dont care about origin of scaling
286
287
288	1	def coarse_registration(pointcloud, drivemap, footprint, downsample=None):
289		"""
290		Improve the initial registration.
291		Find the proper scale by looking for the red meter sticks, and calculate
292		and align the pointcloud's footprint.
293
294		Arguments:
295		pointcloud: pcl.PointCloud
296		The high-res object to register.
297
298		drivemap: pcl.PointCloud
299		A small part of the low-res drivemap on which to register
300
301		footprint: pcl.PointCloud
302		Pointlcloud containing the objects footprint
303
304		downsample: float, default=None, no resampling
305		Downsample the high-res pointcloud before footprint
306		calculation.
307		"""
308		log("Starting coarse registration")
309
310		###
311		# find redstick scale, and use it if possible
312		log(" - Redstick scaling")
313
314		allow_scaling = True
315
316		scale, confidence = get_stick_scale(pointcloud)
317		log(" - Red stick scale=%s confidence=%s" % (scale, confidence))
318
319		if (confidence > 0.5):
320		log(" - Applying red stick scale")
321		pointcloud.scale(1.0 / scale) # dont care about origin of scaling
322		allow_scaling = False
323		else:
324		log(" - Not applying red stick scale, confidence too low")
325
326		#####
327		# find all the points in the drivemap along the footprint
328		# use bottom two meters of drivemap (not trees)
329
330		dm_boundary = boundary_of_drivemap(drivemap, footprint)
331		dm_bb = BoundingBox(dm_boundary)
332
333		# set footprint height from minimum value,
334		# as trees, or high object make the pc.center() too high
335		fixed_boundary = clone(footprint)
336		fp_array = np.asarray(fixed_boundary)
337		fp_array[:, 2] = dm_bb.min[2]
338
339		save(fixed_boundary, "fixed_bound.las")
340
341		#####
342		# find all the boundary points of the pointcloud
343
344		loose_boundary = boundary_of_center_object(pointcloud, downsample)
345		if loose_boundary is None:
346		log(" - boundary estimation failed, using lowest 30 percent of points")
347		loose_boundary = boundary_of_lowest_points(pointcloud,
348		height_fraction=0.3)
349
350		####
351		# match the pointcloud boundary with the footprint boundary
352
353		log(" - Aligning footprints:")
354		rot_matrix, rot_center, scale, translation = align_footprints(
355		loose_boundary, fixed_boundary,
356		allow_scaling=allow_scaling,
357		allow_rotation=True,
358		allow_translation=True)
359
360		save(loose_boundary, "aligned_bound.las")
361
362		####
363		# Apply to the main pointcloud
364
365		pointcloud.rotate(rot_matrix, origin=rot_center)
366		pointcloud.scale(scale, origin=rot_center)
367		pointcloud.translate(translation)
368		rot_center += translation
369
370		return rot_center
371
372
373	1	def _fine_registration_helper(pointcloud, drivemap, voxelsize=0.05, attempt=0):
374		"""
375		Perform ICP on pointcloud with drivemap, and return convergence indicator.
376		Reject large translatoins.
377
378		Returns:
379		transf : np.array([4,4])
380		transform
381		success : Boolean
382		if icp was successful
383		fitness : float
384		sort of sum of square differences, ie. smaller is better
385		"""
386		####
387		# Downsample to speed up
388		# use voxel filter to keep evenly distributed spatial extent
389
390		log(" - Downsampling with voxel filter: %s" % voxelsize)
391		pc = downsample_voxel(pointcloud, voxelsize)
392
393		####
394		# Clip to drivemap to prevent outliers confusing the ICP algorithm
395
396		log(" - Clipping to drivemap")
397		bb = BoundingBox(drivemap)
398		z = bb.center[2]
		0 ignored issues – show Coding Style Naming introduced 2016-02-11 14:39 UTC by Report Bug Copy Issue Report The name `z` does not conform to the variable naming conventions (`[a-z_][a-z0-9_]{1,30}$`). This check looks for invalid names for a range of different identifiers. You can set regular expressions to which the identifiers must conform if the defaults do not match your requirements. If your project includes a Pylint configuration file, the settings contained in that file take precedence. To find out more about Pylint, please refer to their site. Loading history...
399		extracted = extract_mask(pc, [bb.contains([point[0], point[1], z])
400		for point in pc])
401
402		log(" - Remaining points: %s" % len(extracted))
403
404		####
405		# GICP
406
407		converged, transf, estimate, fitness = gicp(extracted, drivemap)
408
409		####
410		# Dont accept large translations
411
412		translation = transf[0:3, 3]
413		if np.dot(translation, translation) > 5 ** 2:
414		log(" - Translation too large, considering it a failure.")
415		converged = False
416		fitness = 1e30
417		else:
418		log(" - Success, fitness: ", converged, fitness)
419
420		force_srs(estimate, same_as=pointcloud)
421		save(estimate, "attempt%s.las" % attempt)
422
423		return transf, converged, fitness
424
425
426	1	def fine_registration(pointcloud, drivemap, center, voxelsize=0.05):
427		"""
428		Final registration step using ICP.
429
430		Find the local optimal postion of the pointcloud on the drivemap; due to
431		our coarse_registration algorithm, we have to try two orientations:
432		original, and rotated by 180 degrees around the z-axis.
433
434		Arguments:
435		pointcloud: pcl.PointCloud
436		The high-res object to register.
437
438		drivemap: pcl.PointCloud
439		A small part of the low-res drivemap on which to register
440
441		center: np.array([3])
442		Vector giving the centerpoint of the pointcloud, used to do
443		the 180 degree rotations.
444
445		voxelsize: float default : 0.05
446		Size in [m] of the voxel grid used for downsampling
447		"""
448		log("Starting fine registration")
449
450		# for rotation around z-axis
451		rot = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
452
453		####
454		# do a ICP step for 4 orientations
455
456		transf = {}
457		success = {}
458		fitness = {}
459		for i in range(4):
460		log(" - attempt: %s" % i)
461		transf[i], success[i], fitness[i] = _fine_registration_helper(
462		pointcloud,
463		drivemap,
464		attempt=i,
465		voxelsize=voxelsize)
466		pointcloud.rotate(rot, origin=center)
467
468		####
469		# pick best
470
471		best, value = min(fitness.iteritems(), key=lambda x: x[1])
472		if success[best]:
473		log(" - Best attempt: %s" % best)
474		if best > 0: # np.array()**0 does weird things
475		pointcloud.rotate(rot**best, origin=center)
476		pointcloud.transform(transf[best])
477		return
478
479		# ICP failed:
480		# return the pointcloud with just footprints aligned
481		# no use to undo a rotation, as any orientationi is equally likely.
482		log(" - Unable to do fine registration")
483
484		return
485

NLeSC / PattyAnalytics

patty.registration.coarse_registration() B last analyzed 2016-03-07 10:18 UTC

Complexity

Size

Duplication

Code Coverage

How to fix Long Method

Long Method

Duplication Side-by-Side

Filter issues like

patty.registration.coarse_registration() B
last analyzed 2016-03-07 10:18 UTC